在基本完成了轮足机器人的控制之后，我们希望机器人变得更加的易用，比如在拿起之后机器人的轮子不会高速旋转，启动时放在地面上就可以自己平衡站起。这时，我们需要添加两大功能：离地检测和启动平衡。

7.1 离地检测

在机器人离开地面时，往往会遇到轮子高速旋转的问题，在手上拿着并不好控制，我们希望它能够“意识”到自己离地了，并停止转动。

我们观察离地状态，发现有两个比较明显的特征，也就是空转的特征：

1. 电流比较低
2. 轮子速度比较高

而正常在路上行走的时候，有负载，所以总体呈现高电流和低中速。

利用这个条件进行区分，我们就可以设计离地检测功能。示例如下：

static bool lastOffGround = false;
        float speedAbs = (fabs((float)leftSpeed) + fabs((float)rightSpeed)) / 2.0f;
        bool lowCurrent = (abs(leftCurrent) < RobotConstants::OFFGROUND_CURRENT_THRESH) && 
                          (abs(rightCurrent) < RobotConstants::OFFGROUND_CURRENT_THRESH);
        if (lowCurrent && speedAbs > RobotConstants::OFFGROUND_SPEED_THRESH) {
            if (robotState.offGroundStartTime == 0) robotState.offGroundStartTime = millis();
            if (millis() - robotState.offGroundStartTime >= RobotConstants::OFFGROUND_TIME_MS) 
                robotState.offGroundActive = true;
        } else {
            robotState.offGroundStartTime = 0;
            robotState.offGroundActive = false;
        }
        if (robotState.offGroundActive && !lastOffGround) {
            Serial.println("[Safety] Off-ground detected >1s: stopping motors and lowering legs.");
        }
        lastOffGround = robotState.offGroundActive;

if (robotState.offGroundActive) {
        robotState.rollOffset = 0;
        uint16_t leftServoPos = constrain(robotState.legLeftPos, SERVO_LEFT_MIN, SERVO_LEFT_MAX);
        uint16_t rightServoPos = constrain(robotState.legRightPos, SERVO_RIGHT_MIN, SERVO_RIGHT_MAX);
        pMotorServo->setServoPosition(leftServoPos, rightServoPos);
    }
}

此时，将机器人拿起，机器人离地一会儿后会自动停转。

7.2 启动平衡

为了启动方便，我们希望将机器人平放在地面上，就能平衡站稳。

首先我们需要一个固定的高度，用作启动后的目标平衡位置，我们算上中位偏移，带限位指定之：

uint16_t leftServoPos = constrain(robotState.legLeftPos + robotState.rollOffset, SERVO_LEFT_MIN, SERVO_LEFT_MAX);
uint16_t rightServoPos = constrain(robotState.legRightPos - robotState.rollOffset, SERVO_RIGHT_MIN, SERVO_RIGHT_MAX);

然后进入平衡任务。