我的自平衡小车D4——PID控制器——第一次的直立行走

黑马

依旧先上代码

1. /***************************
   
2. 自平衡系统 for Arduino
   
3. by 黑马
   
4. 数据采集    2012-03-21
   
5. 滤波        2012-03-23
   
6. PID控制     2012-03-27
   
7. PID整定
   
8. 运动控制
   
9. ***************************/
   
10. 
    
11. #include <Wire.h>
    
12. #include <Servo.h>
    
13. 
    
14. /************ 传感器参数 ***********/
    
15. #define Acc 0x1D           // ADXL345地址
    
16. #define Gyr 0x69           // L3G4200D地址
    
17. #define Mag 0x1E           // HMC5883L地址
    
18. #define Gry_offset -13     // 陀螺仪偏移量
    
19. #define Gyr_Gain 0.07      // 满量程2000dps时灵敏度(dps/digital)
    
20. #define pi 3.14159
    
21. 
    
22. /********** 互补滤波器参数 *********/
    
23. unsigned long preTime = 0; // 采样时间
    
24. float f_angle = 0.0;       // 滤波处理后的角度值
    
25. 
    
26. /*********** PID控制器参数 *********/
    
27. unsigned long lastTime;           // 前次时间
    
28. float ITerm, lastInput;    // 积分项、前次输入
    
29. float Output = 0.0;        // PID输出值
    
30. 
    
31. /*********** 马达控制参数 **********/
    
32. Servo servoL;              // 定义左驱
    
33. Servo servoR;              // 定义右驱
    
34. # define servoL_offset 90  // 左驱偏置
    
35. # define servoR_offset 90  // 右驱偏置
    
36. 
    
37. /************ 程序初始化 ***********/
    
38. void setup() {
    
39.     sensor_init();        // 配置传感器
    
40.     Serial.begin(19200);  // 开启串口以便监视数据
    
41.     servoL.attach(30);    // PIN30输出到左驱
    
42.     servoR.attach(32);    // PIN32输出到右驱
    
43.     delay(1000);
    
44.   }
    
45. /************** 主程序 *************/
    
46. void loop() {
    
47.     unsigned long now = millis();                           // 当前时间(ms)
    
48.     float dt = (now - preTime) / 1000.0;                    // 微分时间(s)
    
49.     preTime = now;                                          // 记录本次时间(ms)
    
50. 
    
51. /********** 读取姿态传感器 *********/
    
52.     float Y_Acc = gDat(Acc, 1);                             // 获取向前的加速度(digite)
    
53.     float Z_Acc = gDat(Acc, 2);                             // 获取向下的加速度(digite)
    
54.     float angleA = atan(Y_Acc / Z_Acc) * 180 / pi;          // 根据加速度分量得到的角度(degree)
    
55.     float omega =  Gyr_Gain * (gDat(Gyr, 0) +  Gry_offset); // 当前角速度(degree/s)
    
56. 
    
57. /*********** 一阶互补滤波 **********/
    
58.     float K = 0.8;                                          // 取值权重
    
59.     float A = K / (K + dt);                                 // 加权系数
    
60.     f_angle = A * (f_angle + omega * dt) + (1-A) * angleA;  // 互补滤波算法
    
61. /************ PID控制器 ***********/
    
62.     now = millis();                                         // 当前时间(ms)
    
63.     float TimeCh = (now - lastTime) / 1000.0;               // 采样时间(s)
    
64.     float Kp = 10.0, Ki = 0.0, Kd = 0.0;                    // 比例系数、积分系数、微分系数
    
65.     float SampleTime = 0.1;                                 // 采样时间(s)
    
66.     float Setpoint = -3.8;                                  // 设定目标值(degree)
    
67.     float outMin = -80.0, outMax = +80.0;                   // 输出上限、输出下限
    
68.     if(TimeCh >= SampleTime) {                              // 到达预定采样时间时
    
69.         float Input = f_angle;                              // 输入赋值
    
70.         float error = Setpoint - Input;                     // 偏差值
    
71.         ITerm+= (Ki * error * TimeCh);                      // 计算积分项
    
72.         ITerm = constrain(ITerm, outMin, outMax);           // 限定值域
    
73.         float DTerm = Kd * (Input - lastInput) / TimeCh;    // 计算微分项
    
74.         Output = Kp * error + ITerm - DTerm;                // 计算输出值
    
75.         Output = constrain(Output, outMin, outMax);         // 限定值域
    
76.         servoL.write(Output + servoL_offset);               // 控制左驱
    
77.         servoR.write(Output + servoR_offset);               // 控制右驱
    
78.         lastInput = Input;                                  // 记录输入值
    
79.         lastTime = now;                                     // 记录本次时间
    
80.     }
    
81. /************ 参数上传 ***********/
    
82.     Serial.print(now);          // 计算时间
    
83.     Serial.print(",");
    
84.     Serial.print(f_angle, 6);   // 偏离角度
    
85.     Serial.print(",");
    
86.     Serial.print(Output, 6);    // PID输出值
    
87.     Serial.print(";");
    
88. // 控制微分时间
    
89.     delay(10);
    
90. }
    
91. 
    
92. /***************************************
    
93. 九轴姿态传感器寄存器读取函数
    
94. For Arduino, by 黑马
    
95. ****************************************
    
96.         调用参数表
    
97. ****************************************
    
98.   type    device      axis
    
99.                    0   1   2
    
100. ADXL345     Acc    x   y   z
     
101. L3G4200D    Gyr    x   y   z
     
102. HMC5883L    Mag    x   z   y
     
103. ****************************************
     
104. Example
     
105. ****************************************
     
106. 00 #include <Wire.h>
     
107. 01 #define Acc 0x1D;
     
108. 02 #define Gyr 0x69;
     
109. 03 #define Mag 0x1E;
     
110. 04
     
111. 05  void setup() {
     
112. 06    sensor_init();
     
113. 07    delay(1000);
     
114. 08  }
     
115. 09
     
116. 10  void loop() {
     
117. 11    int Z-Gyroscope;
     
118. 12    Z-Gyroscope = gDat(Gyr, 2);
     
119. 13    delay(50);
     
120. 14  }
     
121. ***************************************/
     
122. 
     
123. int gDat(int device, int axis) {
     
124.     int v;
     
125.     byte vL, vH, address;               // 存放byte数值
     
126.     if (device == Acc) address = 0x32;  // ADXL345的读数地址
     
127.     if (device == Gyr) address = 0xA8;  // L3G4200D的读数地址
     
128.     if (device == Mag) address = 0x03;  // HMC5883L的读数地址
     
129.     address = address + axis * 2;       // 数据偏移-坐标轴
     
130.     Wire.beginTransmission(device);     // 开始传输数据
     
131.     Wire.send(address);                 // 发送指针
     
132.     Wire.requestFrom(device, 2);        // 请求2 byte数据
     
133.     while(Wire.available() < 2);        // 成功获取前等待
     
134.     vL = Wire.receive();
     
135.     vH = Wire.receive();                // 读取数据
     
136.     Wire.endTransmission();             // 结束传输
     
137.     if (device == Mag) v = (vL << 8) | vH;
     
138.     else v = (vH << 8) | vL;            // 将byte数据合并为Int
     
139.     return v;                           // 返回读书值
     
140. }
     
141. 
     
142. /********************************************
     
143. 配置九轴姿态传感器
     
144. ********************************************/
     
145. void sensor_init() {
     
146. 
     
147. /************ 配置 ADXL345 ***********/
     
148.     writeRegister(Acc, 0x2D, 0b00001000);    // 测量模式
     
149. 
     
150. /************ 配置L3G4200D ***********/
     
151.     writeRegister(Gyr, 0x20, 0b00001111);    // 设置睡眠模式、x, y, z轴使能
     
152.     writeRegister(Gyr, 0x21, 0b00000000);    // 选择高通滤波模式和高通截止频率
     
153.     writeRegister(Gyr, 0x22, 0b00000000);    // 设置中断模式
     
154.     writeRegister(Gyr, 0x23, 0b00110000);    // 设置量程(2000dps)、自检状态、SPI模式
     
155.     writeRegister(Gyr, 0x24, 0b00000000);    // FIFO & 高通滤波
     
156. 
     
157. /************ 配置HMC5883L ***********/
     
158.     writeRegister(Mag, 0x02, 0x00);          // 连续测量
     
159. }
     
160. 
     
161. /********************************************
     
162. 寄存器写入函数
     
163. ********************************************/
     
164. void writeRegister(int device, byte address, byte val) {
     
165.     Wire.beginTransmission(device);          // 写入的传感器
     
166.     Wire.send(address);                      // 写入地址
     
167.     Wire.send(val);                          // 写入值
     
168.     Wire.endTransmission();                  // 结束传输
     
169. }

复制代码

这应该是最基础的框架了，调试的时候肯定还需要加进不少杂七杂八的东西，估计今天下班就可以开始做整体的调试了。不知道程序上有没有这么错漏，各位DX多指点啊。

PID算法参考三水的帖子写的，一些估计用不到的就没加……反正慢慢调吧，遇到问题再说。灰常感谢三水的帮助。

再来张拍的照片吧。没加光耦试了下，没发现什么问题，不知道还有没有必要加，加的话是不是要加个上拉电阻？一般加多大的合适呢？

黑马

taotao71 发表于 2012-3-28 22:02
恭喜，恭喜！总算站起来了

是啊，不过肯定还要慢慢调参数，现在只用了P控制，积分和微分都没加

不过我犯了一个严重的错误，接线插口实在不应该放在上边啊，干扰太严重了，看来还得考虑改结构

黑马

有个疑问，自平衡车如果加点负载物，重心和转动惯量肯定会变，网上的自平衡车是自整定了呢还是只是加的负载还不足以让系统失稳？

黑马

哈哈第一次试机竟然就站起来了，只是调节范围还比较小，视频……上载中，手机拍的……好暗

黑马

接下来就要慢慢整定数据了……哈哈

弘毅

{:soso_e142:} 注解好详细~~

taotao71

恭喜，恭喜！总算站起来了

黑马

弘毅 发表于 2012-3-28 21:58
注解好详细~~

haha, 就当是学习笔记了，也方便DX们帮我挑挑毛病

黑马

有视频有真相……虽然拍得超级不给力

http://v.qq.com/boke/play/9VuIgjUYc6k.html

开心就好

光耦就是为了隔离用，有时候脱机的瞬间电流可能会影响到单片机！你说的上拉是哪部份？ 如果只是为了高电平的花，一般我们习惯用10k或者4.7k上拉电阻

迷你强

{:soso_e102:}，好帅的车车

黑马

开心就好 发表于 2012-3-30 14:54
光耦就是为了隔离用，有时候脱机的瞬间电流可能会影响到单片机！你说的上拉是哪部份？ 如果只是为了高电平的 ...

谢谢~~我用的是一款连续舵机，现在看还没太大问题。4pin的光耦，三极管端要形成高低电平肯定得接上拉或者下拉的电阻吧

黑马

迷你强 发表于 2012-3-30 16:22
，好帅的车车

给点建议呗

三水

{:soso_e142:}哇！恭喜恭喜。一般网上的大多没用自整定，我看了几个开源的都是PI就够了，没有失稳的原因估计是重心还在可控范围内吧，因为他们设置不在可控范围内就停车了。

哈哈，你用的也是舵机来测试的？我之前也用舵机测试过，只要P调节就站住了。。现在也准备重新做做

黑马

三水 发表于 2012-3-30 20:17
哇！恭喜恭喜。一般网上的大多没用自整定，我看了几个开源的都是PI就够了，没有失稳的原因估计 ...

不过感觉转速不太给力，扭矩倒是很大，减速比太大。

最好玩的是有时候没调好，它会很执着的一下一下的撞桌腿，笑喷了{:soso__5448211862396791711_1:}