之前的案例，可以进一步扩展：
机器人微控制器编程（CoCube）-突破边界
从AD到电压；电压和电量，什么关系：
锂电池电压和电量之间，有一定的对应关系，通过对开路电压的测量，可以大致得出电池的剩余电量。不过用电压测量电量的方式有一定的不稳定性，例如放电电流、环境温度、循环、放电平台、电极材料等，都会给最后结果的准确与否带来影响。
电压和电量的对应关系是：100%----4.20V、90%-----4.06V、80%-----3.98V、70%-----3.92V、60%-----3.87V、50%-----3.82V、40%-----3.79V、30%-----3.77V、20%-----3.74V、10%-----3.68V、5%------3.45V、0%------3.00V。锂电池能够实现用电压测量剩余电量，主要是因为这种电池有一个很独特的性质：在电池放电时，电池电压会随着电量的流失而逐渐降低，从而形成了一种正相关的关系，并且有一定的斜率。因此我们能够依据剩余电量估算出大概的电压，反之亦然。
进一步：
电量和机器人任务状态的关系：
充电：检测是否满电
工作：检测电量是否在范围内，执行任务
低电：机器人进入低功耗和准备充电模式
哪些由机器人做，哪些由服务器做？
卡尔曼滤波：
MPU6050数据进一步扩展
机器人微控制器编程（CoCube）-深度融合
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
MPU6050 mpu6050(Wire);
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
uint32_t timer;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
// TODO: Make calibration routine
void setup() {
Serial.begin(115200);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
delay(1000); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
mpu6050.update();
accX = mpu6050.getAccX();
accY = mpu6050.getAccY();
accZ = mpu6050.getAccZ();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
/* Update all the values */
mpu6050.update();
accX = mpu6050.getAccX();
accY = mpu6050.getAccY();
accZ = mpu6050.getAccZ();
tempRaw = mpu6050.getTemp();
gyroX = mpu6050.getGyroX();
gyroY = mpu6050.getGyroY();
gyroZ = mpu6050.getGyroZ();
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle = gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle = gyroYrate * dt;
//gyroXangle = kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle = kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX gyroXrate * dt) 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY gyroYrate * dt) 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("t");
Serial.print(accY); Serial.print("t");
Serial.print(accZ); Serial.print("t");
Serial.print(gyroX); Serial.print("t");
Serial.print(gyroY); Serial.print("t");
Serial.print(gyroZ); Serial.print("t");
Serial.print("t");
#endif
Serial.print(roll); Serial.print("t");
Serial.print(gyroXangle); Serial.print("t");
Serial.print(compAngleX); Serial.print("t");
Serial.print(kalAngleX); Serial.print("t");
Serial.print("t");
Serial.print(pitch); Serial.print("t");
Serial.print(gyroYangle); Serial.print("t");
Serial.print(compAngleY); Serial.print("t");
Serial.print(kalAngleY); Serial.print("t");
#if 0 // Set to 1 to print the temperature
Serial.print("t");
double temperature = (double)tempRaw / 340.0 36.53;
Serial.print(temperature); Serial.print("t");
#endif
Serial.print("rn");
}
