舵机的梯形加减速控制

原理

我们以MG995等舵机为例，舵机有三个PIN，分别是VCC,GND,PWM，PWM引脚需要一个20ms周期信号，通过不同的占空比来控制不同角度，具体参数如下

0.5ms--------------0度；

1.0ms------------45度；

1.5ms------------90度；

2.0ms-----------135度；

2.5ms-----------180度；

在舵机的控制过程中，只需要控制占空比即可。然而，如果是大角度的切换，舵机将以最大转速到达目标位置，在简单的机械臂应用中，这样的控制往往会带来非常大的惯性。

为了避免这种惯性，对舵机进行类梯形加减速控制，如下图：

梯形加减速舵机控制

本质上是通过调节每20ms的占空比，使其逐步增加而不是直接变换，从而实现的梯形加减速控制。

每次占空比增加的数量是梯形加减速的核心。

给出控制结果如下图：

代码

1. 采用面向对象的方法编写C代码，首先使用一个结构体将舵机所需要的参数封装起来

   typedef struct
   {
           float MaxAngle; //对应舵机转动的最大角度
           float MinAngle; //对应舵机能转动的最小角度
           float TargetAngle; //舵机的目标角度
           float MaxOmega; //舵机的最大速度
           float Omega; //舵机的速度
           float Angle; //舵机的角度
           float Acc; //舵机的加速度
           uint8_t Dir; //舵机的方向 //#define CCW 0    #define CW 1
           void (*SetPluse)(uint16_t us); //放在定时器更新中断中使用：定时器时钟配置为1Mhz，更新频率配置为50Hz
   }Servo_t;

2. 然后就是舵机的速度和角度更新函数，这个更新函数 需要放在20ms的定时器中定时调用，因为舵机控制的最小时间间隔是20ms。

   uint8_t compulteNewPos(Servo_t*Node)
   {
           float AngleTo = Node->TargetAngle - Node->Angle;
           float AngleStop = Node->Omega * Node->Omega / 2 / Node->Acc;
           float Omega = Node->Omega;
           if (fabs(AngleTo) < 0.1 && AngleStop < 1) //在目标位置上
           {
                   Node->Angle = Node->TargetAngle;
                   Node->Omega = 0;
                   Node->Dir = CW;
                   return 0;
           }
           if (AngleTo > 0) //目标在当前位置的顺时针方向
           {
               if (AngleTo < AngleStop || Node->Dir == CCW) //应当减速
               {
                       if(Omega>0)
                               Omega = Omega - Node->Acc * 0.02;
                       else if(Omega <0)
                               Omega = Omega + Node->Acc * 0.02;
               }
               else if(AngleStop< AngleTo && Node->Dir == CW)//应当加速
               {
                       Omega = Omega + Node->Acc * 0.02;
                       Omega = min(Omega, Node->MaxOmega);
               }
           }
           else//目标在当前位置的逆时针方向
           {
               if (-AngleTo < AngleStop || Node->Dir == CW) //应当减速
               {
                       if (Omega > 0)
                               Omega =Omega - Node->Acc * 0.02;
                       else if (Omega <0)
                               Omega =Omega + Node->Acc * 0.02;
               }
               else if (AngleStop < -AngleTo&& Node->Dir == CCW)//应当加速
               {
                       Omega = Omega - Node->Acc * 0.02;
                       Omega = max(Omega, -Node->MaxOmega);
               }
           }
   
           if (Omega == 0)
           {
                   Node->Dir = AngleTo > 0?CW:CCW;
           }
           else
           {
                   Node->Dir = Omega > 0 ? CW : CCW;
           }
           Node->Omega = Omega;
           Node->Angle = Node->Angle + Node->Omega * 0.02;
           return 1;
   }

3. 其次就是要对其他的一些参数设置进行封装。

   /*
   * 将角度转换成脉冲宽度
   */
   uint16_t Angle2Pluse(Servo_t* Node)
   {
           return (Node->Angle - Node->MinAngle) / (Node->MaxAngle - Node->MinAngle) * 2000.0 + 500.0;
   }
   
   /*
   * 设置目标角度
   */
   void Servo_MoveTo(Servo_t* Node,float anlge)
   {
           Node->TargetAngle = anlge;
   }
   
   /*
   * 设置加速度
   */
   
   void Servo_SetAcc(Servo_t* Node, float acc)
   {
           Node->Acc = fabs(acc);
   }
   
   
   /*
   * 设置最大速度
   */
   void Servo_SetMaxOmega(Servo_t* Node, float max)
   {
           Node->MaxOmega = fabs(max);
   }