当PID算法遇上VEX机器人（下篇）

接上一篇的内容，这一篇开始讲解具体如何用代码来实现“不倒翁”机器人，下面直接展示核心的代码片段。
简单的再提一下机器人中用到主要传感器，主要是一个陀螺仪（拿来测量角度值），两只马达（一左一右，内置编码器，可以测量马达转了多少度）。

下面这一段代码是获取陀螺仪的值，

///
 //  Reading the Gyro.
///

gyroRateRaw = getGyroRateFloat(gyro);
gyroRate = (gyroRateRaw - gyroOffset)*radiansPerSecondPerRawGyroUnit;

下面这一段是根据马达内置的编码器获取马达现在所处的状态和一开始状态的误差，

///
//  Reading the Motor Position
///

motorAngleRaw = (getMotorEncoder(right) + getMotorEncoder(left))/2.0;
motorAngle = motorAngleRaw*radiansPerRawMotorUnit;

motorAngularSpeedReference = speed*radPerSecPerPercentSpeed;
motorAngleReference = motorAngleReference + motorAngularSpeedReference*loopTimeSec;

motorAngleError = motorAngle - motorAngleReference;

下面这一段代码是拿来计算出小车用来调整误差时所需要的速度值大小，

///
//  Computing Motor Speed
///

motorAngularSpeed = (motorAngle - motorAngleHistory[motorAngleIndex])/(motorAngleHistoryLength*loopTimeSec);
motorAngleHistory[motorAngleIndex] = motorAngle;
motorAngularSpeedError = motorAngularSpeed;

下面这一段代码是拿来实时更新小车的角度和陀螺仪的角度，以便下一次循环时使用，

///
//  Update angle estimate and Gyro Offset Estimate
///

gyroEstimatedAngle = gyroEstimatedAngle + gyroRate*loopTimeSec;
gyroOffset = (1-gyroDriftCompensationRate)*gyroOffset+gyroDriftCompensationRate*gyroRateRaw;

下面这一段是更新马达的误差，

///
//  Update Accumulated Motor Error
///

motorAngleErrorAccumulated = motorAngleErrorAccumulated + motorAngleError*loopTimeSec;
motorAngleIndex = (motorAngleIndex + 1) % motorAngleHistoryLength;

上面提到的几段代码是最最核心的内容，整段的代码我会上传，下载地址（https://download.csdn.net/my/uploads ）供大家下载调试。