|
|
論壇潛行1個月，終于做出我的平衡小車了。以下為我的制作經(jīng)驗。希望大家共同學習進步。
一、以下為物料清單：
NO 名稱 數(shù)量 功能
1 電機JGA25-371 2Pcs 提供動力
2 電機驅(qū)動模塊L298n 1Pcs 驅(qū)動馬達
3 Arduino nano V3.0 1Pcs 主控制晶片
4 HC-06藍牙通訊模塊 1Pcs 與藍牙設(shè)備通訊
5 JY521-MP6050 陀螺儀加速計 1Pcs 提供平衡依據(jù)
6 機構(gòu)件 若干 拼接
二、圖片
三、算法
1.有參考過卡爾曼濾波，加Arduino 內(nèi)置PID庫并使用Processing 圖形工具在線調(diào)試PID參數(shù)方法，小車運動趨勢有，但是無法站立，并且參數(shù)始終沒調(diào)好。
2.參考高通濾波方法，效果出奇的好。調(diào)試了下參數(shù)，就站起來了。
四、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;
- /********************角度參數(shù)**************/
- float Gyro_y; //Y軸陀螺儀數(shù)據(jù)暫存
- 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 編碼器碼盤計數(shù)加一
- void Code_left(){ if(Output>=0){count_left += 1;} else{count_left -= 1;}}// 編碼器碼盤計數(shù)加一
- /////////////////////////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//弧度轉(zhuǎn)換為度,
- 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 圖形工具在線調(diào)試PID參數(shù)教程。http://blog.sina.com.cn/s/blog_69bcf45201019bp8.html
2.PVCBOT【9號】忐忑者·自平衡雙輪小車。http://blog.163.com/pvc_robot/bl ... 643220113334818803/
3.基于Arduino+MPU6050+Tp-link 703n平衡小車。http://m.izizhuan.cn/bbs/dpj-47875-1.html
4.虛擬NXT的NXTway-GS自行平衡兩輪機器人教程 http://bbs.cmnxt.com/thread-7697-1-1.html
5.第二個Arduino小車 兩輪自平衡（Arduino PID方式）http://m.izizhuan.cn/bbs/dpj-48038-1.html
6.碉堡的雙輪平衡小車（蕭大哥）http://m.izizhuan.cn/bbs/dpj-48039-1.html
7.總算實現(xiàn)了 PID 調(diào)速（重點推薦）http://m.izizhuan.cn/bbs/dpj-48040-1.html
--------------------------------------------------
PID_v1.h庫：
PID_v1.rar
(6.3 KB, 下載次數(shù): 44)
2016-4-11 02:46 上傳
點擊文件名下載附件
附件為MP6050標準庫，example中有兩種方式：
1.普通方式
2.DMP方式，此方式需要占用中斷資源
MPU6050.rar
(79.67 KB, 下載次數(shù): 25)
2016-4-11 02:46 上傳
點擊文件名下載附件
|
評分
-
查看全部評分
|
