stm32实现舵机速度控制_stm32普通定时器控制舵机

作者：黑客灵魂 | 2024-08-01 09:49:29

踩

stm32普通定时器控制舵机

一、舵机介绍

舵机是一种常用的器件，可以用于机械臂，云台等项目中，通过pwm的占空比来调节实现舵机的旋转角度，所以首先你得会stm32输出pwm波。常用的舵机有90度舵机，180度舵机，270度舵机和360度舵机，根据你的需求来选择舵机的种类，注意一点就是360度舵机控制不了旋转角度。同时如果想要控制舵机的话需要知道PWM占空比和角度的对应关系，占空比0.5ms到2.5ms对应角度0到最大角度，是一个线性关系，所以无论是180还是270的舵机控制起来都差不多。

二、输出pwm波

1、定时器初始化

以下代码实现的是stm32f103定时器2输出3路pwm波，不知道什么原因stm32f103的CH3都输出不了PWM波。


void PWM_Init(void)
{
	TIM_OCInitTypeDef TIM_OCInitStructure;
	GPIO_InitTypeDef  GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	
	
	
	GPIO_InitStructure.GPIO_Mode= GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_2|GPIO_Pin_1|GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	
	TIM_InternalClockConfig(TIM2);
 
	TIM_TimeBaseInitStructure.TIM_ClockDivision= TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period=20000-1 ;			//ARR
	TIM_TimeBaseInitStructure.TIM_Prescaler=72-1;		//PSC
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter=0;
	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseInitStructure);
	
	
	
	TIM_OCStructInit(&TIM_OCInitStructure);
	TIM_OCInitStructure.TIM_OCMode= TIM_OCMode_PWM1;
	TIM_OCInitStructure.TIM_OCPolarity= TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState= TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse=0 ;			//CCR
	TIM_OC1Init(TIM2,&TIM_OCInitStructure);
	TIM_OC2Init(TIM2,&TIM_OCInitStructure);
//	TIM_OC3Init(TIM2,&TIM_OCInitStructure);
	TIM_OC4Init(TIM2,&TIM_OCInitStructure);
	
	TIM_Cmd(TIM2,ENABLE);
}

2、控制角度

这里使用的是270度舵机，如果使用的是180度舵机就直接将270改成180就行了


//角度转换成脉冲高电平
void translate_angle_to_pulse(void)
{
	angle_1=j1;
	angle_2=j2;
	angle_3=j3;
	angle_4=j4;
	pulse1 = (j1)/270*2000+500; 
	pulse2 = (j2)/270*2000+500;//参与姿态解算，控制倾转角度，范围   270到102
	pulse3 =(j3)/270*2000+500;//参与姿态解算，控制倾转角度，范围   340到170
	pulse4 = (j4)/270*2000+500;//参与姿态解算，控制倾转角度，范围    140到330
	
 
}

三、高级控制

如果你已经学会如何驱动舵机旋转对应的角度，那么就可以进阶高级的控制了，直接给定角度的话舵机旋转速度较快，长时间如此使用会对舵机产生损害，而且不够丝滑，于是我们可以通过算法来实现控制舵机的速度控制。

1、均匀插值法（我顺便取得名字）

实际上就是将角度均匀的分成二十份，通过定时器来控制角度均匀增加，以下代码仅供参考，不能拿来直接使用，因为我注释了蛮多东西，自己看懂之后可以尝试自己写，逻辑蛮简单。


#include "stm32f10x.h"                  // Device header
#include "OLED.h"
#include "Delay.h"
#include "Servo.h"
#include "Key.h"
#include "TIM1.h"
#include "LED.h"
#include "PWM.h"
#include "USART.h"
#include "math.h"
 
uint8_t KeyNum;
 
int Flag=0;
int Key_TR[6]={1,2,1,2,1,2};
float Angle;
uint16_t jiao_du1;
 
uint16_t CPWM[5]={1500,1500,1500,1500,1500};
uint16_t pos[2][5]={
			{2500,1530,1300,2100,2100},
			{500,1530,2500,2500,2500}
};
 
uint8_t flag_vpwm=0;
unsigned char flag_Tover; //舵机旋转标志位
uint8_t flag_pwm=0;//定时器计次
char point_now=0;
char point_aim=1;
uint16_t sum=50; //用来计算需要建立多少个中间数据
uint16_t cnt=1;//用来累计已经执行了多少中间数据
double dp;
double dp0[6];
 
void change(uint16_t a,uint16_t b,uint16_t c,uint16_t d);  //改变舵机目标值
void vpwm(void)	;     //进行舵机运动
void PWM_OUT(int a);  //设置占空比
int ABS(int a,int b); //相减求绝对值
 
 
 
 
 
 
int main(void)
{
//	LED_Init();
	int flag=0;
	Serial_Init();
	Servo_Init();
//	servo_angle_calculate(0,15,0);
//	translate_angle_to_pulse();
//	Timer1_Init(2000,72);
//    change(1530,1300,2100,2100);
	
	while(1)
	{
//        if(flag_vpwm==1){
//			vpwm();
//			flag_vpwm=0;
//	
//		}
//		if(flag_Tover==1){
//			TIM_Cmd(TIM1,DISABLE);
 
//			change((int)pulse1,(int)pulse2,(int)pulse3,(int)pulse4);
//			flag_Tover=0;
			Delay_ms(100);
//			TIM_Cmd(TIM1,ENABLE);
//			
//		}
 
	}
}
int ABS(int a,int b)
{
	if(a>b)
	{
		return a-b;
	}
	else return b-a;
	
}
/***************************************************************************************************************
函 数 名：设置pwm波  
功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
		：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
备    注： 先进先出，循环访问	
****************************************************************************************************************/	
 
void PWM_OUT(int a)
{
	switch(a){
	case 1:TIM_SetCompare1(TIM2,CPWM[a]) ;   break;
	case 2:TIM_SetCompare2(TIM2,CPWM[a]) ;   break;
	case 3:TIM_SetCompare4(TIM2,CPWM[a]) ;   break;
	case 4:TIM_SetCompare1(TIM3,CPWM[a]) ;   break;
//	case 5:TIM_SetCompare1(TIM2,CPWM[a]) ;   break;
	}
//	TIM_SetCompare1(TIM2,(int)pulse1);  //越大向左转  1530   1530
//	TIM_SetCompare2(TIM2,(int)pulse2);  //越大向前转  1300   1560
//	TIM_SetCompare4(TIM2,(int)pulse3);  //越大向前转  2100   1500
//	TIM_SetCompare1(TIM3,(int)pulse4);  //越大向前转  2100   1490 		
}
 
/***************************************************************************************************************
函 数 名：作业初位置，末尾置更新函数  
功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
		：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
备    注： 先进先出，循环访问	
****************************************************************************************************************/	
void change(uint16_t a,uint16_t b,uint16_t c,uint16_t d)
{	
	unsigned char s;
	pos[point_aim][1]=a;
	pos[point_aim][2]=b;
	pos[point_aim][3]=c;
	pos[point_aim][4]=d;
//	if(point_aim==1)
//		{
//		   point_aim=0;
//			 point_now=1;
//    }
//		else
//		{
//			 point_aim=1;
//			 point_now=0;
//		}
//		sum=time/20;	//计算新的插补次数	
 
		for(s=1;s<5;s++)	//计算新的插补增量
		{
		 if(pos[point_aim][s]>pos[point_now][s])
			{
	   			dp=pos[point_aim][s]-pos[point_now][s];
			   	dp0[s]=dp/sum;
			}
		    if(pos[point_aim][s]<=pos[point_now][s])
			{
				dp=pos[point_now][s]-pos[point_aim][s];
				dp0[s]=dp/sum;
				dp0[s]=-dp0[s];
			}
			
	   }
		cnt=0;				  	//m清0
 
}
 
/***************************************************************************************************************
函 数 名：vpwm()  
功能描述：数据插补，插补时间间隔为20/12ms，由timer0控制，使舵机平滑实现速度控制
		：另一个功能是执行完一行后去更新下一行数据，即调用change()
备    注：
****************************************************************************************************************/	
void vpwm(void)		 
{
	unsigned char j=0;
	unsigned char k;
		cnt++;							//用来累加插补过的次数
		if(cnt==sum)						//n是本行作业要插补的总次数
		{
		  flag_Tover=1;				//一行数据的执行时间已经完成
		}
		for(j=1;j<5;j++)
		{
			if(ABS(CPWM[j],pos[point_aim][j])<5)
			{						   	//检测靠近终点位置
			 //  how++;				   	//是，则累加一个
			   CPWM[j]=pos[point_aim][j];//并且直接过度到终点位置
//			   Serial_Printf("%d,%d\r\n",CPWM[j],cnt);
				PWM_OUT(j);
//				Delay_ms(20);
			}	
			else						//不靠近终点，继续插补
			{
				CPWM[j]=pos[point_now][j]+cnt*dp0[j];
//				Serial_Printf("%d,%d\r\n",CPWM[j],cnt);
				PWM_OUT(j);
//				Delay_ms(20);
			}
		} 
			   												
		if(flag_Tover==1)
		{								//从插补次数，和脉宽宽度两方面都到达终点，本作业行完成
//			 flag_Tover=0;
			 for(k=1;k<5;k++){
				pos[point_now][k]=pos[point_aim][k];
			 }
		}
		
	//return;
 
}
void TIM1_UP_IRQHandler(void)
{
 
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
		{
			flag_pwm++;
			if(flag_pwm==10){
			    flag_pwm=0;
				flag_vpwm=1;
			}
 
			TIM_ClearITPendingBit(TIM1, TIM_IT_Update  );  //清除TIMx更新中断标志 
			/*写入执行的操作*/	
		}
	
 
}

2、三条插值法（数学上好像是叫这个名字）

这种算法相比于上一种算法均匀的增加角度更加高级，可以控制舵机一开始的加速度慢慢增加，然后到目标角度，加速度逐渐减少到零，这种非常丝滑。

$\Theta (t)=\Theta (0)+3*(\Theta (f)-\Theta (0))*t^{2}/tf^{2}-2*(\Theta(f)-\Theta (0) )*t^{3}/tf^{3}$

$\Theta (0)$ 是初始的角度

$\Theta(f)$ 为最终的角度

$\Theta (t)$ 为对应时刻的角度

$tf$ 为舵机旋转1度的时间的一倍到两倍，可以自己设定，一般舵机旋转1度为4ms，这个参数可以给8ms

$t$ 是当前的时间，可以由定时器来计时

对公式求导一次可以得到角速度，求导两次可以得到角加速度，可以尝试用软件将图画出来，可以看到一个加速度的一个曲线。

具体代码，

代码是在是一个算法的基础上改动的，其实本质就是实现上面的公式，每2ms进入定时器算出对应的角度，然后设定，可以自己尝试写一下。


#include "stm32f10x.h"                  // Device header
#include "OLED.h"
#include "Delay.h"
#include "Servo.h"
#include "Key.h"
#include "TIM1.h"
#include "LED.h"
#include "PWM.h"
#include "USART.h"
#include "math.h"
 
uint8_t KeyNum;
 
int Flag=0;
int Key_TR[6]={1,2,1,2,1,2};
float Angle;
uint16_t jiao_du1;
 
float CPWM[5]={1500,1500,1500,1500,1500};
float pos[2][5]={
			{2500,94,-35,216,216.4},
			{500,1530,2500,2500,2500}
};
 
uint8_t flag_vpwm=0;
unsigned char flag_Tover; //舵机旋转标志位
uint8_t flag_pwm=0;//定时器计次
char point_now=0;
char point_aim=1;
uint16_t sum=50; //用来计算需要建立多少个中间数据
uint16_t cnt=1;//用来累计已经执行了多少中间数据
double dp;
double dp0[6];
 
void change(void);  //改变舵机目标值
void vpwm(void)	;     //进行舵机运动
void PWM_OUT(int a);  //设置占空比
float ABS(float a,float b); //相减求绝对值
 
 
 
//舵机加减速算法
double s0=90,s1=0,s2;
double tf[6];
int t=0; //时间基准
int t1[6];
int g;
 
int main(void)
{
//	LED_Init();
	int flag=0;
	Serial_Init();
//	tf=(s1-s0)*4;
	Servo_Init();
	Delay_ms(500);
//	servo_angle_calculate(0,15,0);
//	translate_angle_to_pulse();
	change();
	Timer1_Init(2000,72);
//    change(1530,1300,2100,2100);
	
	while(1)
	{
//        if(flag_vpwm==1){
//			vpwm();
//			flag_vpwm=0;
//	
//		}
//		if(flag_Tover==1){
//			TIM_Cmd(TIM1,DISABLE);
 
//			change((int)pulse1,(int)pulse2,(int)pulse3,(int)pulse4);
//			flag_Tover=0;
			Delay_ms(100);
//			TIM_Cmd(TIM1,ENABLE);
//			
//		}
 
	}
}
float ABS(float a,float b)
{
	if(a>b)
	{
		return a-b;
	}
	else return b-a;
	
}
/***************************************************************************************************************
函 数 名：设置pwm波  
功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
		：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
备    注： 先进先出，循环访问	
****************************************************************************************************************/	
 
void PWM_OUT(int a)
{
	switch(a){
	case 1:TIM_SetCompare1(TIM2,(CPWM[1]+49)/270*2000+500) ;   break;
	case 2:TIM_SetCompare2(TIM2,(CPWM[2]+143)/270*2000+500) ;   break;
	case 3:TIM_SetCompare4(TIM2,(CPWM[3]+135)/270*2000+500) ;   break;
	case 4:TIM_SetCompare1(TIM3,(CPWM[4]+133)/270*2000+500) ;   break;
//	case 5:TIM_SetCompare1(TIM2,CPWM[a]) ;   break;
	}
//	TIM_SetCompare1(TIM2,(int)pulse1);  //越大向左转  1530   1530
//	TIM_SetCompare2(TIM2,(int)pulse2);  //越大向前转  1300   1560
//	TIM_SetCompare4(TIM2,(int)pulse3);  //越大向前转  2100   1500
//	TIM_SetCompare1(TIM3,(int)pulse4);  //越大向前转  2100   1490 		
}
 
/***************************************************************************************************************
函 数 名：作业初位置，末尾置更新函数  
功能描述：从缓存中取一个新的目标位置替换原来的目标位置，原来的目标位置变为新的初位置，一次更替
		：有效的数据是插补增量，和插补次数，知道这两个量，和当前初位置即可
备    注： 先进先出，循环访问	
****************************************************************************************************************/	
void change(void)
{	
//	unsigned char s;
	pos[point_aim][1]=135;
	pos[point_aim][2]=0;
	pos[point_aim][3]=216;
//	pos[point_aim][4]=216;
 
 
 
//		for(s=1;s<5;s++)	//计算新的插补增量
//		{
//		 if(pos[point_aim][s]>pos[point_now][s])
//			{
//	   			dp=pos[point_aim][s]-pos[point_now][s];
//			   	dp0[s]=dp/sum;
//			}
//		    if(pos[point_aim][s]<=pos[point_now][s])
//			{
//				dp=pos[point_now][s]-pos[point_aim][s];
//				dp0[s]=dp/sum;
//				dp0[s]=-dp0[s];
//			}
//			
//	   }
//		cnt=0;				  	//m清0
 
}
 
/***************************************************************************************************************
函 数 名：vpwm()  
功能描述：数据插补，插补时间间隔为20/12ms，由timer0控制，使舵机平滑实现速度控制
		：另一个功能是执行完一行后去更新下一行数据，即调用change()
备    注：
****************************************************************************************************************/	
void vpwm(void)		 
{
	unsigned char j=0;
	unsigned char k;
		cnt++;							//用来累加插补过的次数
		if(cnt==sum)						//n是本行作业要插补的总次数
		{
		  flag_Tover=1;				//一行数据的执行时间已经完成
		}
		for(j=1;j<5;j++)
		{
			if(ABS(CPWM[j],pos[point_aim][j])<5)
			{						   	//检测靠近终点位置
			 //  how++;				   	//是，则累加一个
			   CPWM[j]=pos[point_aim][j];//并且直接过度到终点位置
//			   Serial_Printf("%d,%d\r\n",CPWM[j],cnt);
				PWM_OUT(j);
//				Delay_ms(20);
			}	
			else						//不靠近终点，继续插补
			{
				CPWM[j]=pos[point_now][j]+cnt*dp0[j];
//				Serial_Printf("%d,%d\r\n",CPWM[j],cnt);
				PWM_OUT(j);
//				Delay_ms(20);
			}
		} 
			   												
		if(flag_Tover==1)
		{								//从插补次数，和脉宽宽度两方面都到达终点，本作业行完成
//			 flag_Tover=0;
			 for(k=1;k<5;k++){
				pos[point_now][k]=pos[point_aim][k];
			 }
		}
		
	//return;
 
}
void TIM1_UP_IRQHandler(void)
{
 
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
		{
//			flag_pwm++;
//			if(flag_pwm==10){
//			    flag_pwm=0;
//				flag_vpwm=1;
//			}
			t+=8;
			for(g=1;g<4;g++){
				if(CPWM[g]==pos[point_aim][g]){
					  continue;
				}
				else 	t1[g]=t;
			}
//			s2=s0+3/pow(tf,2)*(s1-s0)*pow(t,2)-2/pow(tf,3)*(s1-s0)*pow(t,3);
//		
//			Serial_Printf("%.4lf\r\n",(s2+45)/270*2000+500);
//			if(ABS(s2,s1)<0.3){
//				TIM_Cmd(TIM1,DISABLE);
//			}
			for(g=1;g<4;g++){
				tf[g]=(pos[point_aim][g]-pos[point_now][g])*4;  //舵机转动总时间
			}
			for(g=1;g<4;g++){
				CPWM[g]=pos[point_now][g]+3/pow(tf[g],2)*(pos[point_aim][g]-pos[point_now][g])*pow(t1[g],2)-2/pow(tf[g],3)*(pos[point_aim][g]-pos[point_now][g])*pow(t1[g],3);
				if(ABS(CPWM[g],pos[point_aim][g])<0.5)
				{
					CPWM[g]=pos[point_aim][g];
				}
//				TIM_SetCompare1(TIM2,(CPWM[g]+46)/270*2000+500);
				PWM_OUT(g);
				Serial_Printf("%lf,%lf,%lf,%lf\r\n",CPWM[1]*100,CPWM[2]*100,CPWM[3]*100,CPWM[4]*100);
			}
			if(CPWM[1]==pos[point_aim][1]&&CPWM[2]==pos[point_aim][2]&&CPWM[3]==pos[point_aim][3]&&CPWM[3]==pos[point_aim][3] ){
				t=0;
				for(g=1;g<4;g++)
				{
					pos[point_now][g]=CPWM[g];
					t1[g]=0;
				}
				
				TIM_Cmd(TIM1,DISABLE);
			}
			TIM_ClearITPendingBit(TIM1, TIM_IT_Update  );  //清除TIMx更新中断标志 
			/*写入执行的操作*/	
		}
	
 
}

四、总结

心血来潮，想写这篇文章，缓解一下学习蓝桥杯的痛苦，算法谁爱学去学吧，真的是学不懂

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/黑客灵魂/article/detail/913511