#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]); // 任务句柄的地址（用于引用） }