机器人微控制器编程（CoCube）-强化实践

机器人微控制器编程（CoCube）-强化实践

之前的案例，可以进一步扩展：

​​机器人微控制器编程（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 #include #include // Source: RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: mpu6050(Wire);Kalman kalmanX; // Create the Kalman instancesKalman 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 onlydouble compAngleX, compAngleY; // Calculated angle using a complementary filterdouble kalAngleX, kalAngleY; // Calculated angle using a Kalman filter// TODO: Make calibration routinevoid 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: eq. 25 and eq. 26 // atan2 outputs the value of -π to π (radians) - see // 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: eq. 25 and eq. 26 // atan2 outputs the value of -π to π (radians) - see // 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("\r\n");}

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。