6.1 可变高平衡

改变高度本身不难，你需要做的是改变舵机转动的角度。而如何在改变高度的时候可以平衡行走，就成了我们要考虑的一大问题。

在PID控制中，我们可能需要先固定某个高度测试平衡行走，测试不同的Kp和Kd等系数。以此类推，在不同高度上，我们需要不同的Kp和Kd等参数。据此，我们可以设计一个函数，返回线性变化的Kp和Kd。

为了简单起见，我们可以设计一个回调函数，在几个固定高度下返回固定的Kp和Kd值。你需要根据自己的机器人来调试这个参数值。

robotState.Kp_angle = calculateAdaptiveKp();
robotState.Kd_angle = calculateAdaptiveKd();

6.2 滚转平衡

我们可以将滚转平衡任务视为一个控制环节，称为滚转环。因此，我们可以考虑使用PID控制方式来设计这个滚转环。在这个过程中，我们不仅要调节轮子的速度，还要调整舵机的位置，换句话说也就是左右舵机需要处于不同的位置。

我们使用PD控制来实现滚转环，处理角度和角速度。

示例代码如下：

float rollError = roll - robotState.targetRoll;
float rollControlOutput = robotState.Kp_roll * rollError + robotState.Kd_roll * rollRate;

这个控制输出将成为我们的舵机目标调整位置。

在滚转平衡的过程中有三种基本的状态：向左转、向右转和水平。

如果向左转，那右高左低，重心在左边，右边舵机应该比左边位置大
如果向右转，那左高右低，重心在右边，左边舵机应该比右边位置大
如果水平，那左右一样高，同时减去偏差角

代码上我们需要做一个简单的判定，来决定这个输出应该用在左还是右舵机上：

if (roll > 0.5f) {  // 向左倾斜
            int16_t leftAdjust = robotState.rollOffset - servoRollAdjust;
            leftServoPos = constrain(robotState.legLeftPos - leftAdjust, SERVO_LEFT_MIN, SERVO_LEFT_MAX);
            rightServoPos = robotState.legRightPos;
        } else if (roll < -0.5f) {  // 向右倾斜
            leftServoPos = robotState.legLeftPos;
            int16_t rightAdjust = robotState.rollOffset - servoRollAdjust;
            rightServoPos = constrain(robotState.legRightPos - rightAdjust, SERVO_RIGHT_MIN, SERVO_RIGHT_MAX);
        } else {  // 基本水平
            leftServoPos = constrain(robotState.legLeftPos - robotState.rollOffset, SERVO_LEFT_MIN, SERVO_LEFT_MAX);
            rightServoPos = constrain(robotState.legRightPos - robotState.rollOffset, SERVO_RIGHT_MIN, SERVO_RIGHT_MAX);
        }

然后完成最终的参数传递。

6. 可变高+滚转平衡

6.1 可变高平衡

6.2 滚转平衡

相关文档