目标:
1、指定简单的电机控制协议,支持通过串口通讯对多步进电机进行运动控制
2、支持详细参数输入的运动控制模式:控制方向、转速、加减速率、细分等
3、支持位置控制模式,按照协议格式,输入电机编号和位置,对多电机进行控制
先展示下最终效果:通过3条指令控制三个电机,在不同的加减速曲线下运行
(这里只展示逻辑分析仪结果,实测和调试效果一致)
加减速曲线的积分计算matlab仿真(为了研究下步数和加减速的关系)
-------------------------------------------------设计部分---------------------------------------------------
硬件设计:
主控芯片: STM32F1 或 STM32F4 (支持多IO多计时器中断即可,最好能跑freeRTOS)
电机驱动芯片:DRV8825 - 使用PWM+DIR+EN进行电机控制 (占用资源较少)
电机:5-36V输入范围的所有步进电机均可
软件设计:
制定控制协议:
代码(水平有限供参考):
【串口初始化】
uint8_t aRxBuffer[1]; // HAL库使用的串口接收缓冲
uint8_t USART2_RX_BUF[100]; //接收缓冲,最大100字节
uint16_t USART_RX_STA = 0; //接收状态标志
//bit15 接收完成标志 0x55
//bit14 接收开始标志 0xAA
//bit13~bit0 接收到的有效字节数目
void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
HAL_UART_Receive_IT(&huart2, (uint8_t *)aRxBuffer, 1); //该函数会�??启接收中断:标志位UART_IT_RXNE,并且设置接收缓冲以及接收缓冲接收最大数据量
/* USER CODE END USART2_Init 2 */
}
void USART2_IRQHandler(void)
{
/* USER CODE BEGIN USART2_IRQn 0 */
/* USER CODE END USART2_IRQn 0 */
HAL_UART_IRQHandler(&huart2);
/* USER CODE BEGIN USART2_IRQn 1 */
uint32_t timeout=0;
timeout=0;
while (HAL_UART_GetState(&huart2) != HAL_UART_STATE_READY) //等待就绪
{
timeout++; //超时处理
if(timeout>HAL_MAX_DELAY) break;
}
timeout=0;
while(HAL_UART_Receive_IT(&huart2, (uint8_t *)aRxBuffer, 1) != HAL_OK)//�?次处理完成之后,重新�?启中断并设置RxXferCount�?1
{
timeout++; //超时处理
if(timeout>HAL_MAX_DELAY) break;
}
/* USER CODE END USART2_IRQn 1 */
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) //串口2的中断回调函�?
{
if(huart->Instance==USART2)
{
if((USART_RX_STA&0x8000)==0) //如果USART_RX_STA的bit15=0:接收未完成
{
if((USART_RX_STA&0x4000)==0) //如果没有收到起始标志,USART_RX_STA的bit15�?0
{
if(aRxBuffer[0]==0xAA) //如果收到起始标志
{
USART_RX_STA|=0x4000;
USART2_RX_BUF[USART_RX_STA&0X3FFF]=aRxBuffer[0] ;
USART_RX_STA++;
HAL_TIM_Base_Start_IT(&htim7);
}
else
{
USART_RX_STA=0;
}
}
else if(aRxBuffer[0]==0x55)
{
USART_RX_STA|=0x8000;
USART2_RX_BUF[USART_RX_STA&0X3FFF]=aRxBuffer[0] ;
USART_RX_STA++;
}
else
{
USART2_RX_BUF[USART_RX_STA&0X3FFF]=aRxBuffer[0] ;
USART_RX_STA++;
}
}
}
}
【协议解析】
#define Forward 1
#define Backward 0
struct MotorDefine Motor_Temp ;
void deal_buffer_motorCtrl_data(struct MotorDefine *a)
{
a->MotorNumber = USART2_RX_BUF[1];
if ( USART2_RX_BUF[3] == 0b10000000 ) // 0x80
{
a->MotorDirection = Forward ;
}
else if (USART2_RX_BUF[3] == 0b01000000) // 0x40
{
a->MotorDirection = Backward ;
}
a->DesiredSpeedInRads = (float)USART2_RX_BUF[4] / 10;
a->NumberofRads = (float)USART2_RX_BUF[5] + (float)USART2_RX_BUF[6] / 100 ;
a->StartupSpeedInRads = (float)USART2_RX_BUF[7] / 10 ;
a->accelerationRate = USART2_RX_BUF[8] * 100 ;
a->decelerationRate = USART2_RX_BUF[9] * 100 ;
printf("\r\nInput Information:\r\n");
printf(" MotorNumber:%ld MotorDirection:%ld\r\n DesiredSpeedInRads:%.2f\r\n NumberofRads:%.2f\r\n StartupSpeedInRads:%.2f\r\n accelerationRate:%ld /Hz\r\n decelerationRate:%ld /Hz\r\n"
,a->MotorNumber,a->MotorDirection,a->DesiredSpeedInRads,a->NumberofRads,a->StartupSpeedInRads,a->accelerationRate,a->decelerationRate);
}
void deal_buffer_motorCtrl_position(struct MotorDefine *a)
{
//等待添加代码
printf("\r\nMotor Control - Position Mode \r\n");
}
void StartUartCommondTask(void *argument)
{
uint8_t len = 0;
osDelay(10);
printf("Uart Commond Receive Task starts! \r\n\r\n");
for(;;)
{
osDelay(1);
if(USART_RX_STA&0x8000)
{
len=USART_RX_STA&0x3fff;
switch ( USART2_RX_BUF[2] )
{
case 0b10000000: // 电机控制-位置模式 ,16进制0x80
deal_buffer_motorCtrl_position(&Motor_Temp);
USART_RX_STA=0;
break;
case 0b01000000: // 电机控制-参数模式 ,16进制0x40
deal_buffer_motorCtrl_data(&Motor_Temp);
MotorMove_steps(&Motor_Temp);
USART_RX_STA=0;
break;
case 0b00000001: // 打印回传接收到的协议数据 , 16进制0x01
HAL_UART_Transmit(&huart2, USART2_RX_BUF,len,1000);
while(__HAL_UART_GET_FLAG(&huart2,UART_FLAG_TC)!=SET);
USART_RX_STA=0;
break;
}
}
}
}
【加减速计算函数(参考TI建议设计思路)】
uint32_t AccelDecelTimeCompute(uint32_t AccelDecelRate) //根据输入的加减速率,计算加减速计时器的TMR
{
uint32_t temp_AccelDecelTimeTMR;
if(AccelDecelRate > MOTORTIM_TMR)
{
printf("[WRONG]AccelDecel Rate Oversize!\r\n"); //如果加减速超过20000Hz/s,判断过快报错
return 10000;
}
temp_AccelDecelTimeTMR = MOTORTIM_TMR / AccelDecelRate ; // 计算对应的加减速计时器TMR
return temp_AccelDecelTimeTMR;
}
void AccelDecel(uint32_t AccelDecelState,struct MotorDefine *a)
{
switch (AccelDecelState)
{
case 0:
break;
case 1: // 加速
a->ActualSpeedInHz ++ ;
if (a->ActualSpeedInHz >= a->DesiredSpeedInHz)
{
a->ActualSpeedInHz = a->DesiredSpeedInHz;
AccelDecelState = 0;
}
a->StepperSpeedTMR = MOTORTIM_TMR / a->ActualSpeedInHz;
break;
case 2: // 减速
a->ActualSpeedInHz -- ;
if (a->ActualSpeedInHz <= a->StartupSpeedInHz)
{
a->ActualSpeedInHz = a->StartupSpeedInHz;
AccelDecelState = 0;
}
a->StepperSpeedTMR = MOTORTIM_TMR / a->ActualSpeedInHz;
}
}
【电机控制-参数控制模式】
void MotorMove_steps(struct MotorDefine *temp)
{
if (temp->MotorNumber == 1)
{
temp->deceleration_ratio = Motor1.deceleration_ratio ;
temp->step_angle = Motor1.step_angle ;
temp->mircro_steps = Motor1.mircro_steps ;
//temp->StartupSpeedInRads = Motor1.StartupSpeedInRads ;
temp->MaxSpeedInRads = Motor1.MaxSpeedInRads ;
}
else if (temp->MotorNumber == 2)
{
temp->deceleration_ratio = Motor2.deceleration_ratio ;
temp->step_angle = Motor2.step_angle ;
temp->mircro_steps = Motor2.mircro_steps ;
//temp->StartupSpeedInRads = Motor2.StartupSpeedInRads ;
temp->MaxSpeedInRads = Motor2.MaxSpeedInRads ;
}
else if (temp->MotorNumber == 3)
{
temp->deceleration_ratio = Motor3.deceleration_ratio ;
temp->step_angle = Motor3.step_angle ;
temp->mircro_steps = Motor3.mircro_steps ;
//temp->StartupSpeedInRads = Motor3.StartupSpeedInRads ;
temp->MaxSpeedInRads = Motor3.MaxSpeedInRads ;
}
if(temp->DesiredSpeedInRads > temp->MaxSpeedInRads) // 判断电机设置速度是否超过最大转速
{
printf("[WRONG] Setup Speed faster than max speed:%.2f rad/s !\r\n",temp->MaxSpeedInRads);
return ;
}
temp->StepsInOneCircle = 360 / temp->step_angle * temp->deceleration_ratio * temp->mircro_steps;
temp->StartupSpeedInHz = temp->StepsInOneCircle * temp->StartupSpeedInRads ;
temp->ActualSpeedInHz = temp->StartupSpeedInHz; // 设定初始实际频率为启动频率
temp->DesiredSpeedInHz = temp->StepsInOneCircle * temp->DesiredSpeedInRads ;
temp->StepperSpeedTMR = MOTORTIM_TMR / temp->ActualSpeedInHz;
temp->NumberofSteps = temp->NumberofRads * temp->StepsInOneCircle ;
temp->AccelerationTimeTMR = AccelDecelTimeCompute(temp->accelerationRate);
temp->DecelerationTimeTMR = AccelDecelTimeCompute(temp->decelerationRate);
//根据电机设定的转速、加速度率、减速度率、运行距离,计算电机的加减速曲线
uint32_t DesiredNumberofSteptoAccel ;
uint32_t DesiredNumberofSteptoDecel ;
float DesiredAccellTimeInSeconds ;
float DesiredDecellTimeInSeconds ;
DesiredAccellTimeInSeconds = ((float)temp->DesiredSpeedInHz-temp->StartupSpeedInHz) / temp->accelerationRate;
DesiredDecellTimeInSeconds = ((float)temp->DesiredSpeedInHz-temp->StartupSpeedInHz) / temp->decelerationRate;
DesiredNumberofSteptoAccel = DesiredAccellTimeInSeconds * (temp->DesiredSpeedInHz-temp->StartupSpeedInHz) /2 + temp->StartupSpeedInHz*DesiredAccellTimeInSeconds; //对速度曲线求积分,理论计算完成加速需要的步数
DesiredNumberofSteptoDecel = DesiredDecellTimeInSeconds * (temp->DesiredSpeedInHz-temp->StartupSpeedInHz) /2 + temp->StartupSpeedInHz*DesiredDecellTimeInSeconds ; //对速度曲线求积分,理论计算完成减速需要的步数
if ( (DesiredNumberofSteptoAccel + DesiredNumberofSteptoDecel) <= temp->NumberofSteps ) //如果加减速需要的步数和,小于总步数,则进行完整加减速
{
temp->NumberofSteps_StopAccel = temp->NumberofSteps - DesiredNumberofSteptoAccel ;
temp->NumberofSteps_BeginDecel = DesiredNumberofSteptoDecel ;
printf("Complete AccelDecel \r\n");
}
else // 如果行进距离不能完成完整的加减速曲线,则前1/3加速,后2/3减速
{
temp->NumberofSteps_StopAccel = temp->NumberofSteps /3*2 ;
temp->NumberofSteps_BeginDecel = temp->NumberofSteps /3*2 ;
printf("Incomplete AccelDecel \r\n");
}
printf(" DesiredNumberofSteptoAccel:%ld\r\n DesiredNumberofSteptoDecel:%ld \r\n",DesiredNumberofSteptoAccel,DesiredNumberofSteptoDecel);
printf("\r\nMotor Move Information:\r\n");
printf(" MotorNumber:%ld\r\n Direction:%ld\r\n DesiredSpeedInRads:%.2f\r\n DesiredSpeedInHz:%ld\r\n NumberofRads:%.2f\r\n NumberofSteps:%ld\r\n accelerationRate:%ld Hz/s\r\n decelerationRate:%ld Hz/s\r\n",
temp->MotorNumber,temp->MotorDirection,temp->DesiredSpeedInRads,temp->DesiredSpeedInHz,temp->NumberofRads,temp->NumberofSteps,temp->accelerationRate,temp->decelerationRate);
printf(" DesiredAccellTimeInSeconds:%.2f s\r\n DesiredDecellTimeInSeconds:%.2f s\r\n",
DesiredAccellTimeInSeconds,DesiredDecellTimeInSeconds);
if (temp->MotorNumber == 1)
{
Motor1 = *temp ;
if(Motor1.MotorDirection == 1)
{
Motor1_Dir_Forward();
}
else
{
Motor1_Dir_Backward();
}
HAL_TIM_Base_Start_IT(&htim2);
}
else if (temp->MotorNumber == 2)
{
Motor2 = *temp ;
if(Motor2.MotorDirection == 1)
{
Motor2_Dir_Forward();
}
else
{
Motor2_Dir_Backward();
}
HAL_TIM_Base_Start_IT(&htim3);
}
else if (temp->MotorNumber == 3)
{
Motor3 = *temp ;
if(Motor3.MotorDirection == 1)
{
Motor3_Dir_Forward();
}
else
{
Motor3_Dir_Backward();
}
HAL_TIM_Base_Start_IT(&htim4);
}
}
【使用定时器,对步进电机驱动PWM进行控制】
if (htim->Instance == TIM10)
{
timecount_TIM10++;
AccelDecelcount_TIM10++;
if(timecount_TIM10 <= Pluse_High)
{
Motorpluse1_High();
}
else if(timecount_TIM10 > Pluse_High)
{
Motorpluse1_Low();
}
if(timecount_TIM10 >= Motor[1].StepperSpeedTMR)
{
timecount_TIM10 = 0 ;
Motor[1].NumberofSteps--;
if (Motor[1].MotorDirection == 1){
Motor[1].StepPosition++;
if (Motor[1].StepPosition >= Motor[1].StepsInOneCircle){
Motor[1].StepPosition = Motor[1].StepPosition - Motor[1].StepsInOneCircle ;
}
}
else{
Motor[1].StepPosition--;
if (Motor[1].StepPosition < 0){
Motor[1].StepPosition = Motor[1].StepPosition + Motor[1].StepsInOneCircle ;
}
}
}
if(Motor[1].StepPosition == Motor[1].TargetPosition)
{
Motor[1].Status = 0;
printf("---Motor1 Steps Position:%ld---\r\n",Motor[1].StepPosition);
HAL_TIM_Base_Stop_IT(&htim10);
}
else if (Motor[1].NumberofSteps <= 0){
Motor[1].Status = 0;
//printf("[WRONG]Motor1 Goto Target Position Failed!---Current_Position:%ld---\r\n",Motor[1].StepPosition);
HAL_TIM_Base_Stop_IT(&htim10);
}
if (Motor[1].NumberofSteps > Motor[1].NumberofSteps_StopAccel) // if.. begin Acceleration
{
if(AccelDecelcount_TIM10 >= Motor[1].AccelerationTimeTMR)
{
AccelDecelcount_TIM10=0;
AccelDecel(ACCEL,&Motor[1]);
}
}
else if (Motor[1].NumberofSteps < Motor[1].NumberofSteps_BeginDecel) // if.. begin Deceleration
{
if(AccelDecelcount_TIM10 >= Motor[1].DecelerationTimeTMR)
{
AccelDecelcount_TIM10=0;
AccelDecel(DECEL,&Motor[1]);
}
}
}
【使用定时器,对步进电机两个相电流进行采样和控制】
{
timecount_TIM7++;
AccelDecelcount_TIM7++;
if ( get_ADC1_Current_Phase(0) < (abs(Motor6_MicroSteps[Motor6_State][1])*VM6_Full_Current) ){
if ( Motor6_MicroSteps[Motor6_State][1] > 0 ){
Motor6_A();
}else{
Motor6_a();
}
}else{
Motor6_A_release();
}
if ( get_ADC1_Current_Phase(1) < (abs(Motor6_MicroSteps[Motor6_State][2])*VM6_Full_Current) ){
if ( Motor6_MicroSteps[Motor6_State][2] > 0 ){
Motor6_B();
}else{
Motor6_b();
}
}else{
Motor6_B_release();
}
if(timecount_TIM7 >= Motor[6].StepperSpeedTMR)
{
timecount_TIM7 = 0 ;
Motor[6].NumberofSteps--;
if (Motor[6].MotorDirection == 1){
Motor6_State += Motor6_MicroSteps_Increment ;
if ( Motor6_State > 31 ){
Motor6_State = 0;
}
}
else{
Motor6_State -= Motor6_MicroSteps_Increment;
if ( Motor6_State < 0 ){
Motor6_State = 32 - Motor6_MicroSteps_Increment;
}
}
if (Motor[6].MotorDirection == 1){
Motor[6].StepPosition++;
}
else{
Motor[6].StepPosition--;
}
if(Motor[6].StepPosition == Motor[6].TargetPosition){
Motor[6].Status = 0;
Motor6_Release();
printf("---Motor[6] Steps Position:%ld---\r\n",Motor[6].StepPosition);
HAL_TIM_Base_Stop_IT(&htim7);
}
else if (Motor[6].NumberofSteps <= 0){
Motor[6].Status = 0;
Motor6_Release();
printf("[WRONG]Motor6 Goto Target Position Failed!---Current_Position:%ld---\r\n",Motor[6].StepPosition);
HAL_TIM_Base_Stop_IT(&htim7);
}
}
if (Motor[6].NumberofSteps > Motor[6].NumberofSteps_StopAccel){
if(AccelDecelcount_TIM7 >= Motor[6].AccelerationTimeTMR){
AccelDecelcount_TIM7=0;
AccelDecel(ACCEL,&Motor[6]);
}
}
else if (Motor[6].NumberofSteps < Motor[6].NumberofSteps_BeginDecel){
if(AccelDecelcount_TIM7 >= Motor[6].DecelerationTimeTMR){
AccelDecelcount_TIM7=0;
AccelDecel(DECEL,&Motor[6]);
}
}
}
【使用定时器,对两路直流电机进行控制】
else if (htim->Instance == TIM12)
{
timecount_TIM12_DCM7++;
timecount_TIM12_DCM6++;
if (Motor[7].Status){
if(timecount_TIM12_DCM7 <= Motor[7].AccelerationTimeTMR)
{
switch( Motor[7].Status ){
case 0b00000010:
VM7_IN1_H();
break;
case 0b00000001:
VM7_IN4_H();
break;
case 0b00000011:
VM7_IN1_H();
VM7_IN4_H();
break;
}
}
else if(timecount_TIM12_DCM7 > Motor[7].AccelerationTimeTMR)
{
switch( Motor[7].Status ){
case 0b00000010:
VM7_IN1_L();
break;
case 0b00000001:
VM7_IN4_L();
break;
case 0b00000011:
VM7_IN1_L();
VM7_IN4_L();
break;
}
}
if(timecount_TIM12_DCM7 >= Motor[7].StepperSpeedTMR)
{
timecount_TIM12_DCM7 = 0 ;
}
}
if (Motor[6].Status){
if(timecount_TIM12_DCM6 <= Motor[6].AccelerationTimeTMR)
{
switch( Motor[6].Status ){
case 0b00000010:
VM6_IN1_H();
break;
case 0b00000001:
VM6_IN4_H();
break;
case 0b00000011:
VM6_IN1_H();
VM6_IN4_H();
break;
}
}
else if(timecount_TIM12_DCM6 > Motor[6].AccelerationTimeTMR)
{
switch( Motor[6].Status ){
case 0b00000010:
VM6_IN1_L();
break;
case 0b00000001:
VM6_IN4_L();
break;
case 0b00000011:
VM6_IN1_L();
VM6_IN4_L();
break;
}
}
if(timecount_TIM12_DCM6 >= Motor[6].StepperSpeedTMR)
{
timecount_TIM12_DCM6 = 0 ;
}
}
}