自平衡小车（全部资料，项目已完成，帖子正在更新）

SproutME

2014年12月13日
本帖不定时更新
--------------------------------------------------------------------------------------
自己做的自平衡小车的材料：
MCU：Arduino mega 2560 R3
MPU：MPU－6050
电机驱动：LN298N
电源：18650 X2（串联供电）
电机：直流电机带编码器
电源管理：升压模块（8V升压到16V，直接供给电机）
--------------------------------------------------------------------------------------
注意：
有很多人都会忽略电源供电问题（开了很多帖子，好像没有看见人说电机供电问题），但是我却偏偏遇到了。
－－－－－
一开始我用的是五号电池那种型号的锂电四节串联，容量比较小；一开始硬件搭建好后开始调节PID参数（刚开始没有用编码器，后来发现车在斜坡上不能平衡才改了用编码器的，这里建议大家在搭建自己的模型的时候还是买那种带编码器的电机，或者直接上步进电机），晚上调的时候测试还是蛮好的，但是第二天再去测试的时候就不稳定了。

--------------------------------------------------------------------------------------
2014年12月13日
上个之前拍的视频


贴个自己的代码

主程序  

这个是主程序FilterCompare.ino所用到的库，网址   https://github.com/SproutOrc/Arduino/tree/master/libraries

软件部分
第一步：MPU数据处理（数据读取的话参考论坛代码，如果这一块有问题的话底下留言）

读取MPU6050数据，我没有使用6050内部自带的DMP

1. I2Cdev::readBytes(ADR,ACCEL,6,buffer);
   
2.     ax = (((int16_t)buffer[0]) << 8) | buffer[1];
   
3.     ay = (((int16_t)buffer[2]) << 8) | buffer[3];
   
4.     az = (((int16_t)buffer[4]) << 8) | buffer[5];
   
5. 
   
6.     I2Cdev::readBytes(ADR,GYRO,6,buffer);
   
7.     gx = (((int16_t)buffer[0]) << 8) | buffer[1];
   
8.     gy = (((int16_t)buffer[2]) << 8) | buffer[2];
   
9.     gz = (((int16_t)buffer[4]) << 8) | buffer[5];
   
10.     // 计算与x轴夹角
    
11.     angleAx = -(atan2(ax, -az) * 180.0 / PI - 1.5);
    
12.     // 计算角速度
    
13.     gyroGy = -gy / 131.00;

复制代码

参数说明：

angleAx -> 小车的倾斜角度，范围-90 ~ 0 ~ +90
gyroGy  -> Y轴的角速度


这里的角度值变化要和角速度值得正负号一致，比如：角度值在变小，那么角速度就应该是负的。方向千万不能出错，不然后面滤波就不对了。



下面提供一个自己写的processing程序

软件使用说明：

软件是通过串口进行通信的；
单片机上利用串口输出字符串，格式   a = 0.0, b = 0.0, c = 0.0  以换行符结束
下面是我的串口调试数据

1. Serial2.print("setPoint = ");
   
2.     Serial2.print(0.0);
   
3.     Serial2.print(',');
   
4. 
   
5.     Serial2.print("angleAx = ");
   
6.     Serial2.print(angleAx);
   
7.     Serial2.print(',');
   
8. 
   
9.     Serial2.print("kfAngle = ");
   
10.     Serial2.print(kf.getAngle());
    
11.     Serial2.print(',');
    
12. 
    
13.     Serial2.print("GYRO = ");
    
14.     Serial2.print(kf.getRate());
    
15.     Serial2.print(',');
    
16. 
    
17.     Serial2.print("rightSpeed = ");
    
18.     Serial2.print(float(sendRSpeed));
    
19.     Serial2.print(',');
    
20. 
    
21.     Serial2.print("leftSpeed = ");
    
22.     Serial2.println(float(sendLSpeed));

复制代码

单片机能输出串口数据后，打开processing软件，选择SelfBlanceRobot.pde；
代码简单的说明[pre lang="processing" line="1"]// 显示需要显示的波形

// 创建一个波形对象，就是一个变量名
//
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"));

    }
}[/code]
--------------------------------------------------------------------------------------
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滤波器有两个输出，最优的角度和最优的角速度

第三步：小车直立核心角度环控制

1. /**
   
2. * 角度环控制
   
3. */
   
4. // 角度偏移值
   
5. #define ANGLE_OFFSET 0
   
6. // 角速度偏移值
   
7. #define GYRO_OFFSET 0
   
8. // 角度环比例系数（P）大小
   
9. #define ANGLE_P 55.60
   
10. // 角度环积分系数（D）大小
    
11. #define ANGLE_D 0.52
    
12. /**
    
13. * 角度环控制主函数
    
14. * 输出
    
15. * angleControl
    
16. * （&意思是取引用，如果你对C++不了解的话）
    
17. 
    
18. * 输入
    
19. * angle -> 角度值
    
20. * guro  -> 角速度
    
21. */
    
22. 
    
23. void AngleSabilityControl(
    
24.         float &angleControl,
    
25.   const float &angle,
    
26.   const float &gyro
    
27. ) {
    
28.     /**
    
29.      * 这里这个变量别省，涉及到参数的完整性问题
    
30.      * 详细看 [url]http://www.geek-workshop.com/thread-11336-1-1.html[/url]
    
31.      */
    
32.    
    
33.     float value;
    
34. 
    
35.     value = (ANGLE_OFFSET - angle) * ANGLE_P +
    
36.             (GYRO_OFFSET - gyro) *  ANGLE_D;
    
37. 
    
38.     angleControl = value;
    
39. }

复制代码

角度环参数调节和一般的PD调节一样先调节P然后调节D，单纯一个P是可以让小车立起来的，这个我已经做过测试了。


--------------------------------------------------------------------------------------
2014年12月20日

最近比较黑，快学期末了嘛。考试啊，突击花钱啊，英语六级（英语太烂了），找工作（大四了快失业了）
--------------------------------------------------------------------------------------

话不多说接着小车；
PID调节一直都比较麻烦，看到别人有用电位器来调节参数的。这样有好处有弊端：好处是调节方便，坏处是对电位器电路设计要求比较高。（当然还有模糊PID调节啦，那个不会)。这里还是建议大家用软件调节

1. // 接收的总的字符长度
   
2. #define RECEIVE_LENGTH 20
   
3. 
   
4. // 接收到的数据通过这个结构体类型输出
   
5. typedef struct {
   
6.     String key;
   
7.     float value;
   
8. }DictFloat;
   
9. 
   
10. // 字符转换成浮点数字典类型
    
11. bool stringToFloat(
    
12.      // 输出字典类型
    
13.     DictFloat &dict,
    
14.      // 输入字符串
    
15.      // 结构 ax = 0.0 是字符串当然要加上\0
    
16.     const char *const buff
    
17. ) {
    
18.     // 临时存储用的
    
19.     char charAtIndex;
    
20.     // 索引
    
21.     unsigned char i;
    
22.     // 输入字符串索引
    
23.     unsigned char buffIndex = 0;
    
24.     // 输出字符串索引
    
25.     unsigned char stringIndex = 0;
    
26.     // 存储输出的字符串
    
27.     char string[RECEIVE_LENGTH];
    
28.    
    
29. 
    
30.     for (i = 0; i < RECEIVE_LENGTH; ++i) {
    
31.         // 提取索引位置的字符
    
32.         charAtIndex = *(buff + buffIndex);
    
33.         buffIndex ++;
    
34.         // 如果到字符串末尾直接结束
    
35.         if (charAtIndex == '\0') {
    
36.             *(string + stringIndex) = '\0';
    
37.             break;
    
38.         } else if (charAtIndex == '=') {
    
39.             *(string + stringIndex) = '\0';
    
40.             break;
    
41.         } else if (charAtIndex== ' ') {
    
42.             continue;
    
43.         } else {
    
44.             *(string + stringIndex) = charAtIndex;
    
45.             stringIndex++;
    
46.         }
    
47.     }
    
48.    
    
49.     if (i == RECEIVE_LENGTH) {
    
50.         return false;
    
51.     }
    
52.    
    
53.     dict.key = string;
    
54.     // 将字符串数据转换成浮点数
    
55.     // buff是字符串指针
    
56.     // 所以buff+buffIndex就是等于号后面的数据的字符串
    
57.     dict.value = atof(buff + buffIndex);
    
58.    
    
59.     return true;
    
60. }
    

复制代码

上面有个字符串转浮点数的代码，大家可以试试。

Super169

樓主只是電的問題, 應該很容易解決吧.  期待樓主的更新.....
我也在測試中, 但這幾天有點忙, 車子沒進展, 還未可以成功地站起來.
樓主用的是 16V 的電機?   請問用的是那種編碼?  
我有兩個 12V 的帶編碼電機, 用的是光柵, 一圈就 4200 格, 我的 mega 也應付不了.
不知你的編碼是多少格的.

palm

关注中，楼主能否介绍详细过程？
谢谢！

学慧放弃

想弱弱的问下大神，姿态怎么使用到电机上面qu平衡，更何况是在前进和后退的情况下，

SproutME

Super169 发表于 2014-12-14 01:33
樓主只是電的問題, 應該很容易解決吧.  期待樓主的更新.....
我也在測試中, 但這幾天有點忙, 車子沒進展,  ...

就是普通的那种AB项的编码器，10ms大概100多；电机我也不知道多少伏的，上16v看着使用没有什么问题。电机功率不够只能升升电压了。

SproutME

学慧放弃 发表于 2014-12-14 21:18
想弱弱的问下大神，姿态怎么使用到电机上面qu平衡，更何况是在前进和后退的情况下，

就是用PD调节，角速度 X P + 角速度 X D，先调节P再调D

学慧放弃

那样稳定吗？？
谢了

SproutME

学慧放弃 发表于 2014-12-15 20:17
那样稳定吗？？
谢了

时间长了会有点漂，就是会忘一边跑

学慧放弃

SproutME 发表于 2014-12-15 20:24
时间长了会有点漂，就是会忘一边跑

感觉还是不太理解

yongyuan824

顶一下，先参考研究一下{:soso_e183:}

酷爱diy

看到美女了，漂亮的女孩子

lyy2k

楼主我用您的编码出现
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
这个问题，不知道是什么原因，请解答一下。谢谢

suoma

谢谢分享学习一下

SproutME

lyy2k 发表于 2015-1-13 02:19
楼主我用您的编码出现
FilterCompare.ino: In function 'void setup()':
FilterCompare:86: error: 'Seri ...

我板子是mega 2560的 有多个串口  Uno的话吧  将 Serial2 改成 Serial

lyy2k

晕，现在又多了一个问题了
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:(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"
  在 文件>首选项 中启用
是不是我的蓝牙库不行啊，那库是在您的网址下的啊。求楼主再帮帮我这菜鸟一趟，谢谢