636 lines
18 KiB
C
636 lines
18 KiB
C
#include "APPDEF.H"
|
|
#include "stm32f10x.h"
|
|
|
|
|
|
enum enumPumpSelect PumpSelect[pumpMax];
|
|
/******************************** 功能说明 *************************************
|
|
*
|
|
*******************************************************************************/
|
|
void Motor_IECMD( enum enumPumpSelect Select, _Bool CMD )
|
|
{
|
|
switch( Select )
|
|
{
|
|
case M1:
|
|
TIM5_IECMD( CMD );
|
|
break;
|
|
case M2:
|
|
TIM3_IECMD( CMD );
|
|
break;
|
|
case M3:
|
|
default:
|
|
TIM2_IECMD( CMD );
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂运行速度设置
|
|
*********************************************************************************/
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂运行速度设置
|
|
*********************************************************************************/
|
|
void Motor_PWM( enum enumPumpSelect Select, uint16_t Value )
|
|
{
|
|
switch( Select )
|
|
{
|
|
|
|
|
|
case M1:
|
|
TIM5_CH3_CTRL( Value, Value / 2 );
|
|
break;
|
|
case M2:
|
|
TIM3_CH1_CTRL( Value, Value / 2 );
|
|
break;
|
|
case M3:
|
|
TIM2_CH2_CTRL( Value, Value / 2 );
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂PWM开关
|
|
*********************************************************************************/
|
|
void Motor_PWMCmd( enum enumPumpSelect Select, _Bool State )
|
|
{
|
|
switch( Select )
|
|
{
|
|
case M1:
|
|
TIM5_CH3_CMD( State );
|
|
break;
|
|
case M2:
|
|
TIM3_CH1_CMD( State );
|
|
break;
|
|
case M3:
|
|
TIM2_CH2_CMD( State );
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂运行方向设置
|
|
*********************************************************************************/
|
|
void Motor_Dir( enum enumPumpSelect Select, _Bool State )
|
|
{
|
|
|
|
switch( Select )
|
|
{
|
|
case M1:
|
|
if(State)
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR3 );
|
|
else
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS3 );
|
|
break;
|
|
|
|
case M2:
|
|
if(State)
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR5 );
|
|
else
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS5 );
|
|
break;
|
|
case M3:
|
|
if(State)
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS0 );
|
|
else
|
|
SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR0 );
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂控制器使能
|
|
*********************************************************************************/
|
|
void Motor_En( enum enumPumpSelect Select, _Bool State )
|
|
{
|
|
switch( Select )
|
|
{
|
|
case M1:
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/******************************** 功能说明 *************************************
|
|
* TIMx 中断处理函数
|
|
*******************************************************************************/
|
|
volatile uint32_t SetPluseCount[7u]; // 设置运行脉冲数
|
|
volatile uint32_t PluseCount[7u]; // 实际运行脉冲数
|
|
volatile _Bool CountOver[7u] = {false}; // 计数器溢出标志
|
|
volatile uint32_t SamplePumpSetPluseCount[7u]; // 设置运行脉冲数
|
|
volatile uint32_t SamplePumpPluseCount[7u]; // 实际运行脉冲数
|
|
volatile _Bool SamplePumpCountOver[7u] = {false}; // 计数器溢出标志
|
|
|
|
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂运行任务
|
|
*********************************************************************************/
|
|
//上电前需要修改的安全参数
|
|
/*
|
|
用于限制外部程序调用参数的极限值（超过此参数会无效或按极限值处理）
|
|
*/
|
|
const int32_t FullCoordinate[pumpMax] = { 20500u, 65000u,125000u }; // 最大行程限制
|
|
const float ShiftPluse[pumpMax]={ 100.0f, 1500.0f, 1500.0f }; // 标准最大加速过程脉冲数限制
|
|
const float VelocityH[pumpMax]={ 10000.0f, 5000.0f, 20000.0f }; // 最高速度限制p/s
|
|
const float VelocityL[pumpMax]={ 10.0f, 1000.0f, 1000.0f }; // 最低速度限制p/s
|
|
|
|
|
|
//外部程序调用所需修改参数
|
|
float SpeedSet[pumpMax]={ 20.0f, 1000.0f, 20000.0f }; // 最高速度p/s
|
|
int32_t CoordinateSet[pumpMax]; // 目标位置
|
|
_Bool TaskisRunning[pumpMax] = {0}; // 防止重入标志
|
|
_Bool IdelEn[pumpMax] = { false,false,false}; // 电机空闲使能
|
|
|
|
|
|
// 程序运行时自动更新的参数
|
|
int32_t CoordinatePosition[pumpMax]; // 实时更新实时位置
|
|
int32_t Differ[pumpMax]; // 本次所需运行脉冲数
|
|
uint32_t Shifting[pumpMax] = { 500u, 500u, 500u }; // 加速过程脉冲数
|
|
uint32_t period[pumpMax] = { 20000u, 2000u, 2000u }; // PWM实时周期
|
|
uint32_t CoordinateSetOld[pumpMax]; // 上次所在位置
|
|
uint32_t Position[pumpMax]; // 本次运行起始位置
|
|
float Speeda[pumpMax] ={ 10000.0f, 10000.0f, 10000.0f }; // 加速度 p/(s*s)
|
|
float Velocity[pumpMax] = { 0.0f, 20.0f, 20.0f }; // 实时速度
|
|
float VelocityMax[pumpMax] = { 10000.0f, 20.0f, 20.0f }; // 最大速度
|
|
_Bool Direction[pumpMax] = { false,false,false}; // 运行方向
|
|
|
|
/******************************** 功能说明 *************************************
|
|
* 速度加速度转换
|
|
*******************************************************************************/
|
|
static uint16_t SpeedOld[pumpMax];
|
|
_Bool PumpShutDown[pumpMax] = {false, false}; // 急停标志
|
|
_Bool DirectionSet[pumpMax]; // 目标方向
|
|
_Bool ENSet[pumpMax] = {false,false,false}; // 是否使能电机
|
|
|
|
void SamplePumpSpeed_AcceleratedConver( enum enumPumpSelect Select, uint16_t Speed )
|
|
{
|
|
if( Speed != SpeedOld[Select] )
|
|
{
|
|
Speeda[Select] = (Speed * Speed) / (2u * ShiftPluse[Select]);
|
|
SpeedOld[Select] = Speed;
|
|
}
|
|
}
|
|
/******************************** 功能说明 *************************************
|
|
*
|
|
*******************************************************************************/
|
|
//osSemaphoreId ArmRunFinish;
|
|
//osSemaphoreDef(ArmRunFinish);
|
|
//osSemaphoreId SampleRunFinish;
|
|
//osSemaphoreDef(SampleRunFinish);
|
|
_Bool SpeedMode[pumpMax];
|
|
_Bool PositionMode[pumpMax];
|
|
_Bool FindZero[pumpMax];
|
|
_Bool FindedZero[pumpMax];
|
|
void SamplePumpCtrl( enum enumPumpSelect Select )
|
|
{
|
|
// for(;;)
|
|
{
|
|
if((CoordinateSet[Select] != 0u) && (!TaskisRunning[Select]))
|
|
{
|
|
SamplePumpSpeed_AcceleratedConver(Select,SpeedSet[Select]);
|
|
|
|
TaskisRunning[Select] = true; // 置位 防重入标志
|
|
VelocityMax[Select] = 0;
|
|
period [Select] = 2000u;
|
|
Velocity[Select] = 6.0f;
|
|
Shifting[Select] = 100u;
|
|
|
|
PluseCount[Select] = 0u; // 清零计数脉冲
|
|
CountOver[Select] = false; // 清除计数器溢出
|
|
|
|
SetPluseCount[Select] = CoordinateSet[Select]; // 设置泵运行脉冲
|
|
Motor_IECMD( Select, true);
|
|
Motor_Dir ( Select, DirectionSet[Select] ); // 选择方向
|
|
Motor_PWM ( Select, 100000.0f / VelocityL[Select] ); // 重置pwm为最低输出
|
|
osDelay( 50u );
|
|
Motor_En ( Select, true ); // 步进电机控制器控制使能
|
|
|
|
Shifting[Select] = SetPluseCount[Select] / 2u;
|
|
if( Shifting[Select] > ShiftPluse[Select] )
|
|
Shifting[Select] = ShiftPluse[Select];
|
|
Motor_PWMCmd( Select, true ); // 开启脉冲输出
|
|
|
|
while( !CountOver[Select] ) // 计数器没有溢出
|
|
{
|
|
if(PumpShutDown[Select])
|
|
{
|
|
PumpShutDown[Select] = false;
|
|
break;
|
|
}
|
|
|
|
if( PluseCount[Select] < Shifting[Select] ) // 前Shifting[Select]脉冲加速
|
|
{
|
|
Velocity[Select] = sqrt( 2* PluseCount[Select] * (Speeda[Select]/* / 2u*/));
|
|
if( VelocityMax[Select] < Velocity[Select] )
|
|
VelocityMax[Select] = Velocity[Select];
|
|
if(Velocity[Select] > VelocityH[Select])
|
|
Velocity[Select] = VelocityH[Select];
|
|
if(Velocity[Select] < VelocityL[Select] )
|
|
Velocity[Select] = VelocityL[Select];
|
|
|
|
if( Velocity[Select] > SpeedOld[Select])
|
|
Velocity[Select] = SpeedOld[Select];
|
|
period[Select] = 100000.0f / Velocity[Select];
|
|
Motor_PWM( Select, period[Select] );
|
|
}
|
|
else // 中间匀速
|
|
if( PluseCount[Select] <= (SetPluseCount[Select] - Shifting[Select]) )
|
|
{
|
|
period[Select] = 100000.0f / SpeedOld[Select];
|
|
Motor_PWM( Select, period[Select] );
|
|
while(CoordinateSet[Select] == 65535u)
|
|
{
|
|
PluseCount[Select] = 0;
|
|
osDelay( 5u );
|
|
}
|
|
if(CoordinateSet[Select] == 0u)
|
|
{
|
|
SetPluseCount[Select] = Shifting[Select]*2;
|
|
PluseCount[Select] = Shifting[Select]+1;
|
|
}
|
|
}
|
|
else // 后Shifting[Select]脉冲减速
|
|
// if( PluseCount[Select] >= (SetPluseCount[Select] - Shifting[Select]) )
|
|
{
|
|
|
|
Velocity[Select] = sqrt( 2* (SetPluseCount[Select] - (float)PluseCount[Select]) * (Speeda[Select] / 2u));
|
|
if(Velocity[Select] > VelocityH[Select])
|
|
Velocity[Select] = VelocityH[Select];
|
|
if(Velocity[Select] < VelocityL[Select] )
|
|
Velocity[Select] = VelocityL[Select];
|
|
period[Select] = 100000.0f / Velocity[Select];
|
|
Motor_PWM( Select, period[Select] );
|
|
}
|
|
osDelay( 5u );
|
|
}
|
|
// usRegHoldingBuf[20u+Select*5u] = 0u;
|
|
osDelay( 15u );
|
|
CoordinateSet[Select] = 0u;
|
|
Motor_En( Select, ENSet[Select] ); // 步进电机控制器控制使能
|
|
TaskisRunning[Select] = false; // 清除防重入标志
|
|
CountOver[Select] = false; // 清除计数器溢出
|
|
osDelay( 15u );
|
|
// osSemaphoreRelease(SampleRunFinish);
|
|
}
|
|
else
|
|
{
|
|
osDelay(15);
|
|
}
|
|
}
|
|
}
|
|
static bool ArmInitIsRuning[pumpMax] = { false,false,false};
|
|
/********************************** 功能说明 *************************************
|
|
* 机械臂运行任务
|
|
*********************************************************************************/
|
|
void ArmSoftCtrl( enum enumPumpSelect Select )
|
|
{
|
|
/*Vmin = 50p/s Vmax = 20000p/s At = 0.2s a = 100000p/s2 △s = 2000p △t = 0.001s P = 0.000001s */
|
|
|
|
//#define LowSpeed 40.0f
|
|
// for(;;)
|
|
{
|
|
|
|
if(
|
|
(CoordinateSet[Select] != CoordinateSetOld[Select]) // 如果目标位置与当前位置不同
|
|
&& !TaskisRunning[Select] // 如果此任务没有正在运行
|
|
&&!( ArmInitIsRuning[M3] // 且所有的任务没有正在初始化
|
|
)
|
|
)
|
|
{
|
|
SamplePumpSpeed_AcceleratedConver(Select,SpeedSet[Select]);
|
|
|
|
TaskisRunning[Select] = true; // 置位 防重入标志
|
|
|
|
VelocityMax[Select] = 0;
|
|
|
|
if( CoordinateSet[Select] > FullCoordinate[Select])
|
|
{
|
|
/*usRegHoldingBuf[7] = */CoordinateSet[Select] = CoordinateSetOld[Select];
|
|
osDelay(15); // 极限位置保护
|
|
}
|
|
|
|
period [Select] = 20000u; //周期
|
|
Velocity[Select] = 500.0f; //实时速度
|
|
Shifting[Select] = 1500u; //加速脉冲
|
|
|
|
CoordinateSetOld[Select] = CoordinateSet[Select]; //
|
|
|
|
PluseCount[Select] = 0u; // 清零计数脉冲
|
|
CountOver[Select] = false; // 清除计数器溢出
|
|
|
|
Differ[Select] = CoordinateSet[Select] - CoordinatePosition[Select]; // 计算设置位置与当前位置的偏差和方向
|
|
if(Differ[Select] != 0)
|
|
{
|
|
if( Differ[Select] > 0 )
|
|
Direction[Select] = true;
|
|
if( Differ[Select] < 0 )
|
|
Direction[Select] = false;
|
|
SetPluseCount[Select] = abs( Differ[Select]); // 设置泵运行脉冲
|
|
Position[Select] = CoordinatePosition[Select];
|
|
|
|
Motor_IECMD( Select, true);
|
|
Motor_Dir ( Select, ( Differ[Select] >= 0 ) ? false : true ); // 选择方向
|
|
Motor_PWM ( Select, period[Select] ); // 重置pwm为最低输出
|
|
osDelay( 50u );
|
|
|
|
Shifting[Select] = abs(Differ[Select]) / 2u;
|
|
if( Shifting[Select] > ShiftPluse[Select] )
|
|
Shifting[Select] = ShiftPluse[Select];
|
|
Motor_PWMCmd( Select, true ); // 开启脉冲输出
|
|
Motor_En ( Select, true ); // 步进电机控制器控制使能
|
|
while( !CountOver[Select] ) // 计数器没有溢出
|
|
{
|
|
|
|
// 前Shifting[Select]脉冲加速至匀速并匀速运行
|
|
if( PluseCount[Select] <= (abs(Differ[Select]) - Shifting[Select]) )
|
|
{
|
|
|
|
|
|
if( Velocity[Select] < SpeedOld[Select])
|
|
{
|
|
Velocity[Select] = sqrt( 2* PluseCount[Select] * (Speeda[Select] / 2u));
|
|
if( Velocity[Select] > SpeedOld[Select])
|
|
Velocity[Select] = SpeedOld[Select];
|
|
|
|
if( VelocityMax[Select] < Velocity[Select] )
|
|
VelocityMax[Select] = Velocity[Select];
|
|
if(Velocity[Select] > VelocityH[Select])
|
|
Velocity[Select] = VelocityH[Select];
|
|
if(Velocity[Select] < VelocityL[Select] )
|
|
Velocity[Select] = VelocityL[Select];
|
|
period[Select] = 1000000.0f / Velocity[Select];
|
|
Motor_PWM( Select, period[Select] );
|
|
}
|
|
|
|
}
|
|
else if( PluseCount[Select] >= (abs(Differ[Select]) - Shifting[Select]) ) // 后Shifting[Select]脉冲减速
|
|
{
|
|
|
|
|
|
Velocity[Select] = sqrt( 2* (abs(Differ[Select]) - (float)PluseCount[Select]) * (Speeda[Select] / 2u));
|
|
if( VelocityMax[Select] < Velocity[Select] )
|
|
VelocityMax[Select] = Velocity[Select];
|
|
if(Velocity[Select] > VelocityH[Select])
|
|
Velocity[Select] = VelocityH[Select];
|
|
if(Velocity[Select] < VelocityL[Select] )
|
|
Velocity[Select] = VelocityL[Select];
|
|
period[Select] = 1000000.0f / Velocity[Select];
|
|
Motor_PWM( Select, period[Select] );
|
|
}
|
|
else
|
|
{
|
|
__NOP();
|
|
}
|
|
osDelay( 1u );
|
|
|
|
if( Differ[Select] >= 0 ) // 根据方向偏差来计算当前位置
|
|
{
|
|
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
|
|
}
|
|
else
|
|
{
|
|
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
|
|
}
|
|
|
|
}
|
|
|
|
if( Differ[Select] >= 0 ) // 根据最终计数来计算最终位置
|
|
{
|
|
CoordinatePosition[Select] = Position[Select] + PluseCount[Select];
|
|
}
|
|
else
|
|
{
|
|
CoordinatePosition[Select] = Position[Select] - PluseCount[Select];
|
|
}
|
|
}
|
|
Velocity[Select] = 0;
|
|
TaskisRunning[Select] = false; // 清除防重入标志
|
|
CountOver[Select] = false; // 清除计数器溢出
|
|
osDelay( 5u );
|
|
// osSemaphoreRelease(ArmRunFinish);
|
|
|
|
}
|
|
else
|
|
{
|
|
osDelay(15);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//_Bool SpeedMode[pumpMax];
|
|
//_Bool PositionMode[pumpMax];
|
|
|
|
void MotorCtrl(enum enumPumpSelect Select)
|
|
{
|
|
// static _Bool SpeedModeOld[pumpMax];
|
|
// static _Bool PositionModeOld[pumpMax];
|
|
// ArmRunFinish = osSemaphoreNew( 1,0,NULL);
|
|
// SampleRunFinish = osSemaphoreNew( 1,0,NULL);
|
|
for(;;)
|
|
{
|
|
|
|
// if( SpeedModeOld[Select] != SpeedMode[Select])
|
|
// {
|
|
// SpeedModeOld[Select] = SpeedMode[Select];
|
|
// Motor_IECMD( Select, SpeedModeOld[Select]);
|
|
// }
|
|
// if( PositionModeOld[Select] != PositionMode[Select])
|
|
// {
|
|
// PositionModeOld[Select] = PositionMode[Select];
|
|
// }
|
|
// if( PositionMode[Select] )
|
|
// {
|
|
// SamplePumpCtrl(Select);
|
|
// }
|
|
|
|
switch(Select)
|
|
{
|
|
case M1:
|
|
case M2:
|
|
SamplePumpCtrl(Select);
|
|
break;
|
|
case M3:
|
|
ArmSoftCtrl(Select);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/********************************** 功能说明 ***********************************
|
|
* 中断开关控制
|
|
*******************************************************************************/
|
|
void EXTIx_IRQ_Enable( uint8_t EXTIx )
|
|
{
|
|
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // 写1复位中断标志位
|
|
SET_BIT( EXTI->IMR, 0x01u << EXTIx ); // 允许中断。
|
|
}
|
|
|
|
void EXTIx_IRQ_Disable( uint8_t EXTIx )
|
|
{
|
|
CLEAR_BIT( EXTI->IMR, 0x01u << EXTIx ); // 禁止中断
|
|
SET_BIT( EXTI->PR, 0x01u << EXTIx ); // 写1复位中断标志位
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
uint16_t iz = 0;
|
|
//void EXTI0_IRQHandler( void )
|
|
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
|
|
{
|
|
iz++;
|
|
SET_BIT( EXTI->PR, 0x01u << 1 ); // 写1复位中断标志位
|
|
if(ArmInitIsRuning[M3] )
|
|
{
|
|
if( !READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0 ) )
|
|
{
|
|
EXTIx_IRQ_Disable( 0 );
|
|
Motor_PWMCmd(M3, false );
|
|
CoordinateSet[M3] = 0u;
|
|
CoordinatePosition[M3] = 0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0 ) )
|
|
{
|
|
EXTIx_IRQ_Disable( 0 );
|
|
osDelay(400);
|
|
Motor_PWMCmd(M3, false );
|
|
if(CoordinateSet[M3] >0)
|
|
CoordinatePosition[M3] = 1;
|
|
else
|
|
CoordinatePosition[M3] = -1;
|
|
|
|
CoordinateSet[M1] = 0u;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
_Bool Exti_IOStateRead( enum enumPumpSelect Select )
|
|
{
|
|
_Bool state = false;
|
|
switch (Select)
|
|
{
|
|
case M3:
|
|
state = READ_BIT( GPIOC->IDR, GPIO_IDR_IDR0);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return state;
|
|
}
|
|
void Exti_IRQ_Ctrl( enum enumPumpSelect Select, _Bool State )
|
|
{
|
|
switch (Select)
|
|
{
|
|
case M3:
|
|
State? EXTIx_IRQ_Enable(0) : EXTIx_IRQ_Disable(0);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
void MotorZero_Init( enum enumPumpSelect Select)
|
|
{
|
|
FindedZero[Select] =false;
|
|
EXTIx_IRQ_Disable(0);
|
|
Motor_PWMCmd( Select, false );
|
|
ArmInitIsRuning[Select] = true;
|
|
Exti_IRQ_Ctrl(Select, false);
|
|
Motor_En( Select, true );
|
|
Motor_Dir( Select, true );
|
|
osDelay(10u);
|
|
Motor_IECMD( Select, false );
|
|
Motor_PWM( Select, 200 );
|
|
|
|
// DCMotor_PWM( Select, 55 );
|
|
if( !Exti_IOStateRead(Select) )
|
|
{
|
|
Motor_Dir( Select, false );
|
|
osDelay(10u);
|
|
Motor_PWMCmd( Select, true );
|
|
osDelay(600u);
|
|
Motor_PWMCmd( Select, false );
|
|
Motor_Dir( Select, true );
|
|
osDelay(150u);
|
|
}
|
|
Exti_IRQ_Ctrl(Select, true);
|
|
Motor_PWMCmd( Select, true );
|
|
CoordinateSet[Select] = 0u;
|
|
CoordinatePosition[Select] = 1u;
|
|
while( CoordinatePosition[Select] )
|
|
{
|
|
PluseCount[Select] = 0u;
|
|
osDelay( 20u );
|
|
}
|
|
PluseCount[Select] = 0u;
|
|
SetPluseCount[Select] = 0u;
|
|
CoordinateSet[Select] = 0u;
|
|
CoordinateSetOld[Select] = 0u;
|
|
Direction[Select] = false;
|
|
FindedZero[Select] =true;
|
|
// RegCoilsBufRst( (22u+Select*3u) );
|
|
/*usRegHoldingBuf[43] = 0;
|
|
usRegInputBuf[43] = 2;
|
|
usRegHoldingBuf[7] =*/ CoordinateSet[Select] = 0;
|
|
ArmInitIsRuning[Select] = FindZero[Select] = false;
|
|
}
|
|
|
|
|
|
|
|
BaseType_t temp1,temp2,temp3;
|
|
void _task_MotorCTRL( void * p_arg )
|
|
{
|
|
uint32_t arg = (uint32_t)p_arg;
|
|
MotorCtrl((enum enumPumpSelect)arg );
|
|
}
|
|
|
|
//osThreadDef( _task_MotorCTRL, osPriorityAboveNormal, 2u, 0u );
|
|
osThreadId_t MName[pumpMax];
|
|
TaskHandle_t xTaskHandles[pumpMax];
|
|
void MotorCTRLInitiate( void )
|
|
{
|
|
SamplePumpSpeed_AcceleratedConver(M1,20); //中和泵
|
|
SamplePumpSpeed_AcceleratedConver(M2,1000); // 萃取泵
|
|
SamplePumpSpeed_AcceleratedConver(M3,20000); // 转盘电机
|
|
|
|
|
|
temp1 = xTaskCreate(_task_MotorCTRL, // 任务函数
|
|
"TaskName1", // 任务名（用于调试）
|
|
256, // 堆栈大小（以堆栈类型的大小为单位）
|
|
(void *)M1, // 传递给任务的参数
|
|
osPriorityNormal, // 任务优先级（根据需要调整）
|
|
&xTaskHandles[M1]); // 任务句柄的地址（用于引用）
|
|
temp2 = xTaskCreate(_task_MotorCTRL, // 任务函数
|
|
"TaskName2", // 任务名（用于调试）
|
|
256, // 堆栈大小（以堆栈类型的大小为单位）
|
|
(void *)M2, // 传递给任务的参数
|
|
osPriorityNormal, // 任务优先级（根据需要调整）
|
|
&xTaskHandles[M2]); // 任务句柄的地址（用于引用）
|
|
temp3 = xTaskCreate(_task_MotorCTRL, // 任务函数
|
|
"TaskName3", // 任务名（用于调试）
|
|
256, // 堆栈大小（以堆栈类型的大小为单位）
|
|
(void *)M3, // 传递给任务的参数
|
|
osPriorityNormal, // 任务优先级（根据需要调整）
|
|
&xTaskHandles[M3]); // 任务句柄的地址（用于引用）
|
|
|
|
}
|