Arduino 自平衡小车，制作经验记录与分享，愿有兴趣人士，一起学习探讨。
本帖最后由 smxyk123 于 2014-5-24 09:20 编辑论坛潜行1个月，终于做出我的平衡小车了。以下为我的制作经验。希望大家共同学习进步。
一、以下为物料清单：
NO 名稱 數量 功能
1 電機JGA25-371 2Pcs 提供動力
2 電機驅動模塊L298n 1Pcs 驅動馬達
3 Arduino nano V3.0 1Pcs 主控制晶片
4 HC-06藍牙通訊模塊 1Pcs 與藍牙設備通訊
5 JY521-MP6050 陀螺儀加速計 1Pcs 提供平衡依據
6 機構件 若干 拼接
二、图片
三、算法
1.有参考过卡尔曼滤波，加Arduino 内置PID库并使用Processing 图形工具在线调试PID参数方法，小车运动趋势有，但是无法站立，并且参数始终没调好。
2.参考高通滤波方法，效果出奇的好。调试了下参数，就站起来了。
四、code分享。
1.请大家重点看MP6050 initial 及公式。如有疑问，愿意解答。//copyright by kaiser 20140423 V1.0
void loop()
{
//////////////////////////////////////MP6050//////////////////////////////////////////////////////////////////
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle_Calcu();
Serial.print("Angle");Serial.println(Angle);
if (abs(Angle)<45) /////////////////up-right judge
{
Input=Angle;
//myPID.SetTunings(Kp, Ki, Kd);
//myPID.Compute();
Output=Kp*Angle+Kd*Gyro_y;
Serial.print("Output");Serial.println(Output);
SetMotorVoltage(Output,Output);
}
else{SetMotorVoltage(0,0);//角度大于45度 停止PWM输出}
if(millis()>serialTime)
{
SerialReceive();
SerialSend();
serialTime+=500;
}
}
}
2.我把程序在更新下，最终版上传。
//copyright by kaiser 20140521 V1.0
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "Wire.h" //serial
#include "I2Cdev.h" //IIC
#include "MPU6050.h" //acc&gyro Sensor
//Define Variables we'll be connecting to
char val='s';int Speed_need=0;int Turn_need=0;
float speeds,speeds_filter, positions;
float diff_speeds,diff_speeds_all;
////////////////////PID parameter///////////////////////////////////////
double Output=0;
float Kp=10,Kd=0.06,Ksp = 2.8,Ksi = 0.11; //
////////////////////////////////////////////////
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
/********************角度参数**************/
float Gyro_y; //Y轴陀螺仪数据暂存
float Angle_ax; //由加速度计算的倾斜角度
float Angle; //小车最终倾斜角度
//int Setpoint=0;
////////////////////////////////////////Pin assignment///////////////////////////////////////
int PinA_right= 2; //interrupt 0
int PinA_left= 3; //interrupt 1
int E_left =6;//ENA
int M_right =7;
int M_right2=8;
int E_right =5; //ENB
int M_left =9;
int M_left2 =10;
//////////////////////////////////////////////////////////////////////////////
int PWM_right=0; int PWM_left=0;
int PWM_left_least=87; int PWM_right_least=88;//left:77,right:78
///////////////////////////////interrupt for Speed/////////////////////////////////
int count_right =0;
int count_left=0;
int old_time=0;
int flag;
void Code_right(){if(Output>=0){count_right += 1;}else{count_right -= 1;} }//if only +,can't stand up 编码器码盘计数加一
void Code_left(){if(Output>=0){count_left += 1;} else{count_left -= 1;}}// 编码器码盘计数加一
/////////////////////////Right&Left&Stop///////////////////////////////////////////////
void SetMotorVoltage(int nLeftVol, int nRightVol) {
if(nLeftVol >=0)
{digitalWrite(M_left,LOW);
digitalWrite(M_left2,HIGH);
} else {
digitalWrite(M_left,HIGH);
digitalWrite(M_left2,LOW);
nLeftVol=-nLeftVol;
}
if(nRightVol >= 0) {
digitalWrite(M_right,LOW);
digitalWrite(M_right2,HIGH);
} else {
digitalWrite(M_right,HIGH);
digitalWrite(M_right2,LOW);
nRightVol=-nRightVol;
}
if(nLeftVol>255) { nLeftVol = 255 ; } //防止PWM值超过255
if(nRightVol>255) { nRightVol = 255 ; }//防止PWM值超过255
analogWrite(E_left,nLeftVol);
analogWrite(E_right,nRightVol);
}
/////////////////////////////////////////////////////////////////////////////////////////////
void Angle_Calcu(void) {
Angle_ax = (ax+1942)/238.2 ; //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy-119.58)/16.4; //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ，负号为方向处理
//Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;
//Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
}
void setup()
{
Wire.begin();
Serial.begin(9600);
///////////////////////////////////////////////////////////////////
accelgyro.reset();//reset
delay(1);
accelgyro.setClockSource(MPU6050_CLOCK_PLL_YGYRO);//PLL with Y Gyro reference*/
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//0x1B Value 0x18 2000°/s
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);//0x1C Value 0x18 16g
accelgyro.setDLPFMode(MPU6050_DLPF_BW_5);//0x06 means acc 5Hz delay19.0ms Gyro 5Hz delay 18.6ms
accelgyro.setTempSensorEnabled(true);//disable temsensor
accelgyro.setSleepEnabled(false);
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
////////////////////////pin mode////////////////////////////////////////
pinMode(E_right, OUTPUT); pinMode(M_right, OUTPUT);pinMode(M_right2, OUTPUT);//right
pinMode(E_left, OUTPUT);pinMode(M_left, OUTPUT);pinMode(M_left2, OUTPUT);//left
pinMode(PinA_right,INPUT);pinMode(PinA_left,INPUT);// in 0 in 1
Serial.println("Pin mode ...");
/////////////////////////////interrupt/////////////////////////////////////////////
attachInterrupt(0, Code_right, FALLING);attachInterrupt(1, Code_left, FALLING);
}
void loop()
{
if (Serial.available() > 0){val = Serial.read();Serial.println(val);}
switch(val){
case 'a':Speed_need=30;Turn_need=0;positions=80;break;//Go
case 'b':Speed_need=10;Turn_need=-10;positions=10;break;//right
case 'c':Speed_need=10;Turn_need=10;positions=10;break;//left
case 'd':Speed_need=0;Turn_need=0;positions=0;break;
default:Speed_need=0;Turn_need=0;positions=0;break;}//stop
//Speed_need=30;Turn_need=0;positions=80;
//SetMotorVoltage(255,255);
//Kp=15,Kd=0.09,Ksp = 2.8,Ksi = 0.11;
//Serial.print("count_left");Serial.println(count_left);
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle_Calcu();
//Serial.print("Angle");Serial.println(Angle);
PWM_Calcu();
//if(millis()-old_time>=500){ Serial.print("count_right");Serial.print(count_right);Serial.print("count_left");Serial.println(count_left);old_time=millis();count_right=0;count_left=0;}
}
void PWM_Calcu(void)
{
if (abs(Angle)>40)
{SetMotorVoltage(0,0);}//角度大于45度 停止PWM输出
else
{ //Speed_need=30;Turn_need=0;positions=80;
//////////////////////
speeds=(count_left + count_right)*0.5;
diff_speeds = count_left - count_right;
diff_speeds_all += diff_speeds;
speeds_filter *=0.85;//一阶互补滤波
speeds_filter +=speeds*0.15;
positions += speeds_filter;
positions += Speed_need;
positions = constrain(positions, -2300, 2300);//抗积分饱和
////////////////////
Output=Kp*Angle+Kd*Gyro_y+ Ksp*speeds_filter + Ksi*positions ;
//Serial.print("Output");Serial.println(Output);
if(Turn_need==0){PWM_right=Output-diff_speeds_all;
PWM_left=Output+diff_speeds_all;}
PWM_right=Output+Turn_need;
PWM_left=Output-Turn_need;
if(PWM_right>=0){PWM_right+=PWM_right_least;}else{PWM_right-=PWM_right_least;}
if(PWM_left>=0){PWM_left+=PWM_left_least;}else{PWM_left-=PWM_left_least;}
SetMotorVoltage(PWM_left,PWM_right);}
count_left = 0;
count_right = 0;
}
//////////////////////////////////////////////////////////
五、吃水不忘挖井人，以下为我的参考文章，前辈们还是要非常尊敬的。
1.Processing 图形工具在线调试PID参数教程。http://blog.sina.com.cn/s/blog_69bcf45201019bp8.html
2.PVCBOT【9号】忐忑者·自平衡双轮小车。http://blog.163.com/pvc_robot/blog/static/17527643220113334818803/
3.基于Arduino+MPU6050+Tp-link 703n平衡小车。http://www.geek-workshop.com/thread-8991-1-1.html
4.虚拟NXT的NXTway-GS自行平衡两轮机器人教程 http://bbs.cmnxt.com/thread-7697-1-1.html
5.第二个Arduino小车 两轮自平衡（Arduino PID方式）。http://www.geek-workshop.com/thread-467-1-1.html
6.碉堡的双轮平衡小车（萧大哥） http://www.geek-workshop.com/thread-9005-1-1.html
7.总算实现了 PID 调速（重点推荐）http://www.embedream.com/ndxs/2011-04-23/121.html 真羨慕....我買回來已經幾個星期, 最忙沒時間攪, 還未解決 電機開動時 mpu6050 不穩定的問題, 單是站立也不行.
想請教樓主, 你的電路是如何連接的?是否全部共地的? Super169 发表于 2014-5-23 14:40 static/image/common/back.gif
真羨慕....我買回來已經幾個星期, 最忙沒時間攪, 還未解決 電機開動時 mpu6050 不穩定的問題, 單是站立也不 ...
是全部公地， 很棒的资源，收藏！很棒的资源，收藏！ 我想单独看你的6050模块！ 还有还有，这个PID_v1.h库求共享！ wetnt 发表于 2014-5-23 16:07 static/image/common/back.gif
很棒的资源，收藏！很棒的资源，收藏！
谢谢！ tgyfish 发表于 2014-5-24 01:45 static/image/common/back.gif
还有还有，这个PID_v1.h库求共享！
PS：这个也可以在官网Arduino.cc上找到 若天寒 发表于 2014-5-23 21:25 static/image/common/back.gif
楼主上面是完整程序吗？？之前做过一个，不是很好
这个是我第一版能站起来的程式。可以发现，里面很多被Mark掉了。 tgyfish 发表于 2014-5-24 01:42 static/image/common/back.gif
我想单独看你的6050模块！
附件为MP6050标准库，example中有两种方式：
1.普通方式
2.DMP方式，此方式需要占用中断资源
感谢楼主，收藏待用 smxyk123 发表于 2014-5-24 08:57 static/image/common/back.gif
附件为MP6050标准库，example中有两种方式：
1.普通方式
2.DMP方式，此方式需要占用中断资源
我就这么没节操的收下了，感谢楼主！ tgyfish 发表于 2014-5-24 15:49 static/image/common/back.gif
我就这么没节操的收下了，感谢楼主！
No thanks! 若天寒 发表于 2014-5-24 11:05 static/image/common/back.gif
楼主：Processing 图形工具在线调试PID参数教程我看了之后又疑问，请教一下，我在第十步的时候出现了问题， ...
com 口通讯一定是一个口，如下图黄色部分是设置的地方，你可以调试调试。
若天寒 发表于 2014-5-24 10:08 static/image/common/back.gif
我做过第一个不成功，想请教楼主角度是如何得出的
请看上图，我的接线方式为，Y轴与双轮电机轴平行，X轴与之垂直。Z轴为重力方向。
角度很简单得出：
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Angle_ax = (ax+1942)/238.2 ; //去除零点偏移1942,//16384*3.14/1.2*180//弧度转换为度,
Gyro_y = -(gy-119.58)/16.4; //去除零点偏移119,2000deg/s 16.4 LSB/(deg/s)250---131 ，负号为方向处理
//Serial.print("Angle_ax,Angle_gy");Serial.print(Angle_ax);Serial.println(Angle_gy);
Angle=0.97*(Angle+Gyro_y*0.0005)+0.03*Angle_ax;