STM32实现多步进电机的加减速运动控制

目标:

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 ;
				}
			}
	}

  • 7
    点赞
  • 87
    收藏
    觉得还不错? 一键收藏
  • 4
    评论
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值