舵机的梯形加减速控制

舵机的梯形加减速控制

引用

CSDN

1.

https://blog.csdn.net/Myself_study/article/details/142743369

原理

我们以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);
}