自平衡小车（全部资料，项目已完成，帖子正在更新）
本帖最后由 SproutME 于 2014-12-20 22:52 编辑2014年12月13日
本帖不定时更新
--------------------------------------------------------------------------------------
自己做的自平衡小车的材料：
MCU：Arduino mega 2560 R3
MPU：MPU－6050
电机驱动：LN298N
电源：18650 X2（串联供电）
电机：直流电机带编码器
电源管理：升压模块（8V升压到16V，直接供给电机）
--------------------------------------------------------------------------------------
注意：
有很多人都会忽略电源供电问题（开了很多帖子，好像没有看见人说电机供电问题），但是我却偏偏遇到了。
－－－－－
一开始我用的是五号电池那种型号的锂电四节串联，容量比较小；一开始硬件搭建好后开始调节PID参数（刚开始没有用编码器，后来发现车在斜坡上不能平衡才改了用编码器的，这里建议大家在搭建自己的模型的时候还是买那种带编码器的电机，或者直接上步进电机），晚上调的时候测试还是蛮好的，但是第二天再去测试的时候就不稳定了。
--------------------------------------------------------------------------------------
2014年12月13日
上个之前拍的视频
http://v.youku.com/v_show/id_XODA3MDQwMjEy.html
贴个自己的代码
主程序
这个是主程序FilterCompare.ino所用到的库，网址 https://github.com/SproutOrc/Arduino/tree/master/libraries
软件部分
第一步：MPU数据处理（数据读取的话参考论坛代码，如果这一块有问题的话底下留言）
读取MPU6050数据，我没有使用6050内部自带的DMPI2Cdev::readBytes(ADR,ACCEL,6,buffer);
ax = (((int16_t)buffer) << 8) | buffer;
ay = (((int16_t)buffer) << 8) | buffer;
az = (((int16_t)buffer) << 8) | buffer;
I2Cdev::readBytes(ADR,GYRO,6,buffer);
gx = (((int16_t)buffer) << 8) | buffer;
gy = (((int16_t)buffer) << 8) | buffer;
gz = (((int16_t)buffer) << 8) | buffer;
// 计算与x轴夹角
angleAx = -(atan2(ax, -az) * 180.0 / PI - 1.5);
// 计算角速度
gyroGy = -gy / 131.00;
参数说明：
angleAx -> 小车的倾斜角度，范围-90 ~ 0 ~ +90
gyroGy-> Y轴的角速度
这里的角度值变化要和角速度值得正负号一致，比如：角度值在变小，那么角速度就应该是负的。方向千万不能出错，不然后面滤波就不对了。
下面提供一个自己写的processing程序
软件使用说明：
软件是通过串口进行通信的；
单片机上利用串口输出字符串，格式 a = 0.0, b = 0.0, c = 0.0以换行符结束
下面是我的串口调试数据
Serial2.print("setPoint = ");
Serial2.print(0.0);
Serial2.print(',');
Serial2.print("angleAx = ");
Serial2.print(angleAx);
Serial2.print(',');
Serial2.print("kfAngle = ");
Serial2.print(kf.getAngle());
Serial2.print(',');
Serial2.print("GYRO = ");
Serial2.print(kf.getRate());
Serial2.print(',');
Serial2.print("rightSpeed = ");
Serial2.print(float(sendRSpeed));
Serial2.print(',');
Serial2.print("leftSpeed = ");
Serial2.println(float(sendLSpeed));
单片机能输出串口数据后，打开processing软件，选择SelfBlanceRobot.pde；
代码简单的说明// 显示需要显示的波形
// 创建一个波形对象，就是一个变量名
//
Waveform angle;
Waveform angleAx;
Waveform gyro;
Waveform leftSpeed;
Waveform rightSpeed;
///////////////////////////////////
ConnectProtocol connect;
void setup() {
size(600, 600);
// 显示所有连接的串口
println(Serial.list());
// 配置单片机所使用的串口号
// COM4 -> 打开COM4口
// 115200 -> 波特率115200
connect = new ConnectProtocol("COM4", 115200);
// 配置波形显示的位置和范围
//
// 窗口示意图
// ——————————————————————>x+
// |
// |
// |
// |
// |
// |
// |
// |
// |
// y+
// Waveform(0, 0, width, 100)
// 在x = 0, y = 0为起点，
// width个像素作为波形显示宽度
// 100个像素作为波形显示高度
angle = new Waveform(0, 0, width, 100);
// 波形显示的范围，
// 串口如果发的数据大于等于40，则波形在再高的位置
angle.setRange(40, -40);
angleAx = new Waveform(0, 100, width, 100);
angleAx.setRange(40, -40);
gyro = new Waveform(0, 200, width, 100);
gyro.setRange(200, -200);
leftSpeed = new Waveform(0, 300, width, 100);
leftSpeed.setRange(400, -400);
rightSpeed = new Waveform(0, 400, width, 100);
rightSpeed.setRange(400, -400);
}
void draw() {
if (connect.available() > 0) {
background(0);
// 浮点型字典
// 串口发送过来的数据会保存在a中，保存形式
// 如
// a 0.0
// b 0.0
// c 0.0
FloatDict a = connect.getInFloatDict();
// 向波形添加数据
// 获取字符串为kfAngle的值的大小
// 这个kfAngle和单片机里面发送的值是一一对应的
angle.add(a.get("kfAngle"));
// 设置波形的x轴
angle.setZero(a.get("Setpoint"));
angleAx.add(a.get("angleAx"));
angleAx.setZero(a.get("setPoint"));
gyro.add(a.get("GYRO"));
gyro.setZero(0.0);
leftSpeed.add(a.get("leftSpeed"));
leftSpeed.setZero(0.0);
rightSpeed.add(a.get("rightSpeed"));
rightSpeed.setZero(0.0);
angle.showByLine();
angleAx.showByLine();
gyro.showByLine();
leftSpeed.showByLine();
rightSpeed.showByLine();
// println("leftSpeed = " + a.get("leftSpeed"));
// println("rightSpeed = " + a.get("rightSpeed"));
}
}
--------------------------------------------------------------------------------------
2014年12月15日
第二步：角度和角速度进行滤波处理。
滤波算法我用的是kalman滤波器（在上面提供的github地址里面有kalman滤波的库，这个库来自 http://aguegu.net/?p=1232， aGuegu / 官微宏），滤波器比较参照论坛里面 http://www.geek-workshop.com/thread-10172-1-1.html感谢275891381提供源码和测试程序，对我有很大帮助。
为什么选用kalman滤波？我也不知道。。。。。。就是想试试看看效果怎么样，后来就懒得改了。
kalman滤波器有两个输出，最优的角度和最优的角速度
第三步：小车直立核心角度环控制
/**
* 角度环控制
*/
// 角度偏移值
#define ANGLE_OFFSET 0
// 角速度偏移值
#define GYRO_OFFSET 0
// 角度环比例系数（P）大小
#define ANGLE_P 55.60
// 角度环积分系数（D）大小
#define ANGLE_D 0.52
/**
* 角度环控制主函数
* 输出
* angleControl
* （&意思是取引用，如果你对C++不了解的话）
* 输入
* angle -> 角度值
* guro-> 角速度
*/
void AngleSabilityControl(
float &angleControl,
const float &angle,
const float &gyro
) {
/**
* 这里这个变量别省，涉及到参数的完整性问题
* 详细看 http://www.geek-workshop.com/thread-11336-1-1.html
*/
float value;
value = (ANGLE_OFFSET - angle) * ANGLE_P +
(GYRO_OFFSET - gyro) *ANGLE_D;
angleControl = value;
}
角度环参数调节和一般的PD调节一样先调节P然后调节D，单纯一个P是可以让小车立起来的，这个我已经做过测试了。
--------------------------------------------------------------------------------------
2014年12月20日
最近比较黑，快学期末了嘛。考试啊，突击花钱啊，英语六级（英语太烂了），找工作（大四了快失业了）
--------------------------------------------------------------------------------------
话不多说接着小车；
PID调节一直都比较麻烦，看到别人有用电位器来调节参数的。这样有好处有弊端：好处是调节方便，坏处是对电位器电路设计要求比较高。（当然还有模糊PID调节啦，那个不会)。这里还是建议大家用软件调节
// 接收的总的字符长度
#define RECEIVE_LENGTH 20
// 接收到的数据通过这个结构体类型输出
typedef struct {
String key;
float value;
}DictFloat;
// 字符转换成浮点数字典类型
bool stringToFloat(
// 输出字典类型
DictFloat &dict,
// 输入字符串
// 结构 ax = 0.0 是字符串当然要加上\0
const char *const buff
) {
// 临时存储用的
char charAtIndex;
// 索引
unsigned char i;
// 输入字符串索引
unsigned char buffIndex = 0;
// 输出字符串索引
unsigned char stringIndex = 0;
// 存储输出的字符串
char string;
for (i = 0; i < RECEIVE_LENGTH; ++i) {
// 提取索引位置的字符
charAtIndex = *(buff + buffIndex);
buffIndex ++;
// 如果到字符串末尾直接结束
if (charAtIndex == '\0') {
*(string + stringIndex) = '\0';
break;
} else if (charAtIndex == '=') {
*(string + stringIndex) = '\0';
break;
} else if (charAtIndex== ' ') {
continue;
} else {
*(string + stringIndex) = charAtIndex;
stringIndex++;
}
}
if (i == RECEIVE_LENGTH) {
return false;
}
dict.key = string;
// 将字符串数据转换成浮点数
// buff是字符串指针
// 所以buff+buffIndex就是等于号后面的数据的字符串
dict.value = atof(buff + buffIndex);
return true;
}
上面有个字符串转浮点数的代码，大家可以试试。
樓主只是電的問題, 應該很容易解決吧.期待樓主的更新.....
我也在測試中, 但這幾天有點忙, 車子沒進展, 還未可以成功地站起來.
樓主用的是 16V 的電機? 請問用的是那種編碼?
我有兩個 12V 的帶編碼電機, 用的是光柵, 一圈就 4200 格, 我的 mega 也應付不了.
不知你的編碼是多少格的.
关注中，楼主能否介绍详细过程？
谢谢！ 想弱弱的问下大神，姿态怎么使用到电机上面qu平衡，更何况是在前进和后退的情况下， 本帖最后由 SproutME 于 2014-12-15 11:39 编辑
Super169 发表于 2014-12-14 01:33 static/image/common/back.gif
樓主只是電的問題, 應該很容易解決吧.期待樓主的更新.....
我也在測試中, 但這幾天有點忙, 車子沒進展,...
就是普通的那种AB项的编码器，10ms大概100多；电机我也不知道多少伏的，上16v看着使用没有什么问题。电机功率不够只能升升电压了。 学慧放弃 发表于 2014-12-14 21:18 static/image/common/back.gif
想弱弱的问下大神，姿态怎么使用到电机上面qu平衡，更何况是在前进和后退的情况下，
就是用PD调节，角速度 X P + 角速度 X D，先调节P再调D 那样稳定吗？？
谢了 学慧放弃 发表于 2014-12-15 20:17 static/image/common/back.gif
那样稳定吗？？
谢了
时间长了会有点漂，就是会忘一边跑 SproutME 发表于 2014-12-15 20:24 static/image/common/back.gif
时间长了会有点漂，就是会忘一边跑
感觉还是不太理解 顶一下，先参考研究一下{:soso_e183:} 看到美女了，漂亮的女孩子 楼主我用您的编码出现
FilterCompare.ino: In function 'void setup()':
FilterCompare:86: error: 'Serial2' was not declared in this scope
FilterCompare.ino: In function 'void printout()':
FilterCompare:121: error: 'Serial2' was not declared in this scope
FilterCompare:130: error: 'Serial2' was not declared in this scope
这个问题，不知道是什么原因，请解答一下。谢谢 谢谢分享学习一下 lyy2k 发表于 2015-1-13 02:19 static/image/common/back.gif
楼主我用您的编码出现
FilterCompare.ino: In function 'void setup()':
FilterCompare:86: error: 'Seri ...
我板子是mega 2560的 有多个串口Uno的话吧将 Serial2 改成 Serial 晕，现在又多了一个问题了
Arduino:1.5.5-r2 (Windows 7), 板："Arduino Uno"
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp: In static member function 'static boolean Bluetooth::setBluetoothBaud(HardwareSerial&, long int)':
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp:44: error: invalid conversion from 'char*' to 'const uint8_t*'
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp:44: error: initializing argument 1 of 'virtual size_t Print::write(const uint8_t*, size_t)'
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp: In static member function 'static boolean Bluetooth::handshake(HardwareSerial&, long int)':
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp:58: error: invalid conversion from 'const char*' to 'const uint8_t*'
E:\平行车\arduino软件\arduino_IDE_1.5.5-r2\arduino-1.5.5-r2\libraries\Bluetooth\Bluetooth.cpp:58: error: initializing argument 1 of 'virtual size_t Print::write(const uint8_t*, size_t)'
报告将会包含更多的信息
"Show verbose output during compilation"
在 文件>首选项 中启用
是不是我的蓝牙库不行啊，那库是在您的网址下的啊。求楼主再帮帮我这菜鸟一趟，谢谢