#include "APPDEF.H" enum enumPumpSelect PumpSelect[pumpMax]; /******************************** 功能说明 ************************************* * *******************************************************************************/ void Motor_IECMD( enum enumPumpSelect Select, _Bool CMD ) { switch( Select ) { case M1: TIM4_IECMD( CMD ); break; case M2: TIM3_IECMD( CMD ); break; default: break; } } /********************************** 功能说明 ************************************* * 机械臂运行速度设置 *********************************************************************************/ /********************************** 功能说明 ************************************* * 机械臂运行速度设置 *********************************************************************************/ void Motor_PWM( enum enumPumpSelect Select, uint16_t Value ) { switch( Select ) { case M1: TIM4_CH1_CTRL( Value, Value / 2 ); break; case M2: TIM3_CH2_CTRL( Value, Value / 2 ); break; default: break; } } /********************************** 功能说明 ************************************* * 机械臂PWM开关 *********************************************************************************/ void Motor_PWMCmd( enum enumPumpSelect Select, _Bool State ) { switch( Select ) { case M1: TIM4_CH1_CMD( State ); break; case M2: TIM3_CH2_CMD( State ); break; default: break; } } /********************************** 功能说明 ************************************* * 机械臂运行方向设置 *********************************************************************************/ void Motor_Dir( enum enumPumpSelect Select, _Bool State ) { switch( Select ) { case M1: // TIM4_CH1_DIR( State); 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 uint32_t FullCoordinate[pumpMax] = { 20500u, 65000u,65000u }; int32_t CoordinateSet[pumpMax]; // 目标位置 int32_t CoordinatePosition[pumpMax]; // 实时更新实时位置 uint32_t Position[pumpMax]; // 本次运行起始位置 _Bool TaskisRunning[pumpMax] = {0}; // 防止重入标志 _Bool IdelEn[pumpMax] = { false,false,false}; // 电机空闲使能 int32_t DCMotorSpeed[pumpMax]; // 直流电机速度 uint32_t CoordinateSetOld[pumpMax]; // 上次所在位置 int32_t Differ[pumpMax]; // 本次所需运行脉冲数 float SpeedSet[pumpMax]={ 100.0f, 1000.0f, 5000.0f }; // 最高速度p/s uint32_t period[pumpMax] = { 20000u, 2000u, 2000u }; // PWM实时周期 float Velocity[pumpMax] = { 0.0f, 20.0f, 20.0f }; // 实时速度 uint32_t Shifting[pumpMax] = { 500u, 500u, 500u }; // 加速过程脉冲数 float Speeda[pumpMax] ={ 10000.0f, 10000.0f, 10000.0f }; // 加速度 p/(s*s) float VelocityH[pumpMax]={ 10000.0f, 5000.0f, 5000.0f }; // 最高速度p/s float VelocityL[pumpMax]={ 10.0f, 1000.0f, 1000.0f }; // 最低速度p/s float ShiftPluse[pumpMax]={ 100.0f, 1500.0f, 1500.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; } } /******************************** 功能说明 ************************************* * *******************************************************************************/ _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 ); } 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] // 且所有的任务没有正在初始化 ) ) { 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) - 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] ); } 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 ); } else { osDelay(15); } } } //_Bool SpeedMode[pumpMax]; //_Bool PositionMode[pumpMax]; void MotorCtrl(enum enumPumpSelect Select) { // static _Bool SpeedModeOld[pumpMax]; // static _Bool PositionModeOld[pumpMax]; 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; } } } #include "stm32f10x.h" /******************************** 功能说明 ************************************* * 中断端口初始化使能 *******************************************************************************/ void BIOS_EXTI_Init( void ) { // // 浮空输入模式 SET_BIT( RCC->APB2ENR, RCC_APB2ENR_IOPBEN ); GPIOB->BSRR |= 0x0002u; // 端口输出为1； // GPIOF->BSRR |= 0x3800u; // 端口输出为1； // GPIOE->BSRR |= 0x0040u; // 端口输出为1； // SET_BIT( GPIOC->PUPDR, GPIO_PUPDR_PUPD0_0|GPIO_PUPDR_PUPD1_0|GPIO_PUPDR_PUPD2_0|GPIO_PUPDR_PUPD3_0 ); // IO口配置 上拉 // SET_BIT( GPIOF->PUPDR, GPIO_PUPDR_PUPD11_0|GPIO_PUPDR_PUPD12_0|GPIO_PUPDR_PUPD13_0 ); // IO口配置 上拉 CLEAR_BIT( GPIOB->CRL, GPIO_CRL_CNF1_0 ); SET_BIT( GPIOB->CRL, GPIO_CRL_CNF1_1 ); SET_BIT( GPIOB->ODR, GPIO_ODR_ODR1 ); // IO口配置 上拉 // MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE0, GPIO_MODER_MODE0_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE1, GPIO_MODER_MODE1_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE2, GPIO_MODER_MODE2_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOC->MODER, GPIO_MODER_MODE3, GPIO_MODER_MODE3_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE11, GPIO_MODER_MODE11_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE12, GPIO_MODER_MODE12_1 ); // IO口配置 复用输出 // MODIFY_REG( GPIOF->MODER, GPIO_MODER_MODE13, GPIO_MODER_MODE13_1 ); // IO口配置 复用输出 // SET_BIT(RCC->APB2ENR, RCC_APB2ENR_SYSCFGEN); // // 开启中断复用 // SYSCFG->EXTICR[0] = // SYSCFG_EXTICR1_EXTI1_PC // |SYSCFG_EXTICR1_EXTI1_PC // |SYSCFG_EXTICR1_EXTI2_PC // |SYSCFG_EXTICR1_EXTI3_PC ; // SYSCFG->EXTICR[1] = // SYSCFG_EXTICR2_EXTI6_PF; // SYSCFG->EXTICR[2] = // SYSCFG_EXTICR3_EXTI11_PF; // SYSCFG->EXTICR[3] = // SYSCFG_EXTICR4_EXTI12_PF // |SYSCFG_EXTICR4_EXTI13_PF // ; SET_BIT( RCC->APB2ENR, RCC_APB2ENR_AFIOEN ); AFIO->EXTICR[0] = AFIO_EXTICR1_EXTI1_PB // | AFIO_EXTICR1_EXTI1_PB // | AFIO_EXTICR1_EXTI2_PA // | AFIO_EXTICR1_EXTI3_PA // ; // AFIO->EXTICR[1] = AFIO_EXTICR2_EXTI4_PA // | AFIO_EXTICR2_EXTI5_PA // | AFIO_EXTICR2_EXTI6_PA // | AFIO_EXTICR2_EXTI7_PA ; CLEAR_BIT( EXTI->IMR, 0xFFFFu ); // 禁止中断 CLEAR_BIT( EXTI->EMR, 0xFFFFu ); // no event 无中断事件 // SET_BIT ( EXTI->RTSR, 0x0006u ); // rising edge trigger 上升沿触发 SET_BIT ( EXTI->FTSR, 0x0002u ); // falling edge trigger 下降沿触发 WRITE_REG( EXTI->PR, 0xFFFFu ); // 通过写1能复位触发的中断标志 // SET_BIT ( EXTI->IMR, 0x384Fu ); // 允许中断 // // IO中断 // NVIC_EnableIRQ( EXTI0_IRQn ); NVIC_EnableIRQ( EXTI1_IRQn ); // NVIC_EnableIRQ( EXTI2_IRQn ); // NVIC_EnableIRQ( EXTI3_IRQn ); // NVIC_EnableIRQ( EXTI9_5_IRQn ); // NVIC_EnableIRQ( EXTI15_10_IRQn ); } /********************************** 功能说明 *********************************** * 中断开关控制 *******************************************************************************/ 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 EXTI1_IRQHandler( void ) { iz++; SET_BIT( EXTI->PR, 0x01u << 1 ); // 写1复位中断标志位 if(ArmInitIsRuning[M1] ) { if( !READ_BIT( GPIOB->IDR, GPIO_Pin_1 ) ) { EXTIx_IRQ_Disable( 1 ); Motor_PWMCmd(M1, false ); CoordinateSet[M1] = 0u; CoordinatePosition[M1] = 0; } } else { if( READ_BIT( GPIOB->IDR, GPIO_Pin_1 ) ) { EXTIx_IRQ_Disable( 1 ); osDelay(400); Motor_PWMCmd(M1, false ); if(CoordinateSet[M1] >0) CoordinatePosition[M1] = 1; else CoordinatePosition[M1] = -1; CoordinateSet[M1] = 0u; } } } _Bool Exti_IOStateRead( enum enumPumpSelect Select ) { _Bool state = false; switch (Select) { case M1: state = READ_BIT( GPIOB->IDR, GPIO_IDR_IDR1); break; default: break; } return state; } void Exti_IRQ_Ctrl( enum enumPumpSelect Select, _Bool State ) { switch (Select) { case M1: State? EXTIx_IRQ_Enable(1) : EXTIx_IRQ_Disable(1); break; default: break; } } void MotorZero_Init( enum enumPumpSelect Select) { SET_BIT( GPIOA->BSRR, GPIO_BSRR_BS5 ); 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 ); 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; SET_BIT( GPIOA->BSRR, GPIO_BSRR_BR5 ); } 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,100); SamplePumpSpeed_AcceleratedConver(M2,1000); SamplePumpSpeed_AcceleratedConver(M3,100); temp1 = xTaskCreate(_task_MotorCTRL, // 任务函数 "TaskName1", // 任务名（用于调试） 512, // 堆栈大小（以堆栈类型的大小为单位） (void *)M1, // 传递给任务的参数 osPriorityNormal, // 任务优先级（根据需要调整） &xTaskHandles[M1]); // 任务句柄的地址（用于引用） temp2 = xTaskCreate(_task_MotorCTRL, // 任务函数 "TaskName2", // 任务名（用于调试） 512, // 堆栈大小（以堆栈类型的大小为单位） (void *)M2, // 传递给任务的参数 osPriorityNormal, // 任务优先级（根据需要调整） &xTaskHandles[M2]); // 任务句柄的地址（用于引用） temp3 = xTaskCreate(_task_MotorCTRL, // 任务函数 "TaskName3", // 任务名（用于调试） 512, // 堆栈大小（以堆栈类型的大小为单位） (void *)M3, // 传递给任务的参数 osPriorityNormal, // 任务优先级（根据需要调整） &xTaskHandles[M3]); // 任务句柄的地址（用于引用） }