机器人微控制器编程(CoCube)-强化实践
2023-06-13 09:13:00 时间
之前的案例,可以进一步扩展:
从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数据进一步扩展
#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("\r\n");
}
相关文章
- 「产、学、用」如何推动机器人新突破、强落地?——「机器人智能论坛」圆满落幕
- 工业机器人在汽车制造行业中的应用
- 机器人步入3.0时代:科沃斯靠什么继续称霸扫地机江湖?
- 机器人控制器编程实践指导书旧版-实践六 LCD液晶显示(点阵)
- 机器人控制器编程实践指导书旧版-实践八 机器人综合设计
- 机器人控制器编程整理汇总-辞旧迎新-
- 云之家群组机器人如何定时自动发消息?
- 商用清洁机器人江湖:16 位创业者「同台竞技」
- 【工具】国内免加速免注册免费使用ChatGPT AI对话机器人
- ROS2机器人编程简述新书推荐-A Concise Introduction to Robot Programming with ROS2
- 使用OpenGPT(ChatGPT)搭建 QQ 机器人
- ROS2机器人编程简述humble-第二章-Controlling the Iterative Execution .3.1
- ROS2机器人编程简述humble-第二章-Executors .3.5
- ROS2机器人编程简述humble-第三章-COMPUTATION GRAPH .2
- ROS2机器人编程简述humble-第三章-BUMP AND GO IN C++ .3
- ROS2机器人编程简述humble-第三章-BUMP AND GO BEHAVIOR IN PYTHON .4
- ROS2机器人编程简述humble-第四章-BASIC DETECTOR .3
- 教机器人捏橡皮泥?MIT、IBM, UCSD等联合发布软体操作数据集PlasticineLab
- Facebook AI团队让机器人行走适应各种环境和路面
- 用Python端对端数据分析识别机器人“僵尸粉
- 深度丨微软小冰资深产品总监曹文韬:如何把聊天机器人打造为一个“知音+专家”的产品
- 现实版机械战警?迪拜首个机器人警察明年上岗
- 神经认知学在机器人中的发展和应用(附PPT+视频) | 雷锋网公开课
- MIT TR 35 Anca Dragan:为了人类与机器人和谐地工作、生活而奋斗
- 家庭机器人要爆发?看客拍手叫好,戏子才知冷暖
- 真·稳如狗:中国团队推出四足机器人,对标波士顿动力
- 乐聚机器人、猎户星空、擎朗智能、高仙机器人获2019 AI+机器人最佳成长奖 | CCF-GAIR 2019
- 韩国高校研发变色龙机器人登Nature子刊