代码收藏家 使用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、三条插值法（数学上好像是叫这个名字）
这种算法相比于上一种算法均匀的增加角度更加高级，可以控制舵机一开始的加速度慢慢增加，然后到目标角度，加速度逐渐减少到零，这种非常丝滑。
是初始的角度
为最终的角度
为对应时刻的角度
为舵机旋转1度的时间的一倍到两倍，可以自己设定，一般舵机旋转1度为4ms，这个参数可以给8ms
是当前的时间，可以由定时器来计时
对公式求导一次可以得到角速度，求导两次可以得到角加速度，可以尝试用软件将图画出来，可以看到一个加速度的一个曲线。
具体代码，
代码是在是一个算法的基础上改动的，其实本质就是实现上面的公式，每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更新中断标志
/*写入执行的操作*/
}
}
四、总结
心血来潮，想写这篇文章，缓解一下学习蓝桥杯的痛苦，算法谁爱学去学吧，真的是学不懂
作者：m0_72535042
