Arduino平衡小車源碼&原理圖【Minibalance For Arduino】 0.png (84.48 KB, 下載次數(shù): 183) 下載附件 2017-7-12 20:50 上傳 把我們提供的庫文件拷貝到您的Arduino安裝目錄下的Libraries文件夾 可在附件中下載 /****************************************************************************    作者：平衡小車之家    產(chǎn)品名稱：Minibalance For Arduino ****************************************************************************/ #include <DATASCOPE.h>      //這是PC端上位機的庫文件 #include <PinChangeInt.h>    //外部中斷 #include <MsTimer2.h>        //定時中斷 #include <KalmanFilter.h>    //卡爾曼濾波 #include "I2Cdev.h"         #include "MPU6050_6Axis_MotionApps20.h"//MPU6050庫文件 #include "Wire.h"   #include <EEPROM.h>          MPU6050 Mpu6050; //實例化一個 MPU6050 對象，對象名稱為 Mpu6050 DATASCOPE data;//實例化一個 上位機 對象，對象名稱為 data KalmanFilter KalFilter;//實例化一個卡爾曼濾波器對象，對象名稱為 KalFilter int16_t ax, ay, az, gx, gy, gz;  //MPU6050的三軸加速度和三軸陀螺儀數(shù)據(jù) #define KEY 3     //按鍵引腳 #define IN1 12   //TB6612FNG驅(qū)動模塊控制信號 共6個 #define IN2 13 #define IN3 7 #define IN4 6 #define PWMA 10 #define PWMB 9 #define ENCODER_L 2  //編碼器采集引腳 每路2個 共4個 #define DIRECTION_L 5 #define ENCODER_R 4 #define DIRECTION_R 8 #define ZHONGZHI 0//小車的機械中值  DIFFERENCE #define DIFFERENCE 2 int Balance_Pwm, Velocity_Pwm, Turn_Pwm;   //直立 速度 轉(zhuǎn)向環(huán)的PWM int Motor1, Motor2;      //電機疊加之后的PWM float Battery_Voltage;   //電池電壓 單位是V volatile long Velocity_L, Velocity_R = 0;   //左右輪編碼器數(shù)據(jù) int Velocity_Left, Velocity_Right = 0;     //左右輪速度 int Flag_Qian, Flag_Hou, Flag_Left, Flag_Right; //遙控相關(guān)變量 int Angle, Show_Data,PID_Send;  //用于顯示的角度和臨時變量 unsigned char Flag_Stop = 1,Send_Count,Flash_Send;  //停止標志位和上位機相關(guān)變量 float Balance_Kp=15,Balance_Kd=0.4,Velocity_Kp=2,Velocity_Ki=0.01; //***************下面是卡爾曼濾波相關(guān)變量***************// float K1 = 0.05; // 對加速度計取值的權(quán)重 float Q_angle = 0.001, Q_gyro = 0.005; float R_angle = 0.5 , C_0 = 1; float dt = 0.005; //注意：dt的取值為濾波器采樣時間 5ms int addr = 0; /************************************************************************** 函數(shù)功能：檢測小車是否被拿起 入口參數(shù)：Z軸加速度 平衡傾角 左輪編碼器 右輪編碼器 返回  值：0：無事件 1：小車被拿起 **************************************************************************/ int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right){   static unsigned int flag, count0, count1, count2;   if (flag == 0) //第一步   {     if (abs(encoder_left) + abs(encoder_right) < 15)         count0++;  //條件1，小車接近靜止     else       count0 = 0;     if (count0 > 10)      flag = 1, count0 = 0;   }   if (flag == 1) //進入第二步   {     if (++count1 > 400)       count1 = 0, flag = 0;                         //超時不再等待2000ms     if (Acceleration > 27000 && (Angle > (-14 + ZHONGZHI)) && (Angle < (14 + ZHONGZHI)))  flag = 2; //條件2，小車是在0度附近被拿起   }   if (flag == 2)  //第三步   {     if (++count2 > 200)       count2 = 0, flag = 0;       //超時不再等待1000ms     if (abs(encoder_left + encoder_right) > 300)           //條件3，小車的輪胎因為正反饋達到最大的轉(zhuǎn)速            {         flag = 0;  return 1;       }                                             }   return 0; } /************************************************************************** 函數(shù)功能：檢測小車是否被放下 作者：平衡小車之家 入口參數(shù)： 平衡傾角 左輪編碼器 右輪編碼器 返回  值：0：無事件 1：小車放置并啟動 **************************************************************************/ int Put_Down(float Angle, int encoder_left, int encoder_right){   static u16 flag, count;   if (Flag_Stop == 0)         return 0;                   //防止誤檢   if (flag == 0)   {     if (Angle > (-10 + ZHONGZHI) && Angle < (10 + ZHONGZHI) && encoder_left == 0 && encoder_right == 0)      flag = 1; //條件1，小車是在0度附近的   }   if (flag == 1)   {     if (++count > 100)       count = 0, flag = 0;  //超時不再等待 500ms     if (encoder_left > 12 && encoder_right > 12 && encoder_left < 80 && encoder_right < 80) //條件2，小車的輪胎在未上電的時候被人為轉(zhuǎn)動     {       flag = 0;       flag = 0;       return 1;    //檢測到小車被放下     }   }   return 0; } /************************************************************************** 函數(shù)功能：異常關(guān)閉電機 作者：平衡小車之家 入口參數(shù)：傾角和電池電壓 返回  值：1：異常  0：正常 **************************************************************************/ unsigned char Turn_Off(float angle, float voltage) {   unsigned char temp;   if (angle < -40 || angle > 40 || 1 == Flag_Stop || voltage < 11.1) //電池電壓低于11.1V關(guān)閉電機 //===傾角大于40度關(guān)閉電機//===Flag_Stop置1關(guān)閉電機   {                                                                             temp = 1;                                               analogWrite(PWMA, 0);  //PWM輸出為0     analogWrite(PWMB, 0); //PWM輸出為0   }   else    temp = 0;   //不存在異常，返回0   return temp; } /************************************************************************** 函數(shù)功能：虛擬示波器往上位機發(fā)送數(shù)據(jù) 作者：平衡小車之家 入口參數(shù)：無 返回  值：無 **************************************************************************/ void DataScope(void) {   int i;   data.DataScope_Get_Channel_Data(Angle, 1);  //顯示第一個數(shù)據(jù)，角度   data.DataScope_Get_Channel_Data(Velocity_Left, 2);//顯示第二個數(shù)據(jù)，左輪速度  也就是每40ms輸出的脈沖計數(shù)   data.DataScope_Get_Channel_Data(Velocity_Right, 3);//顯示第三個數(shù)據(jù)，右輪速度 也就是每40ms輸出的脈沖計數(shù)   data.DataScope_Get_Channel_Data(Battery_Voltage, 4);//顯示第四個數(shù)據(jù)，電池電壓，單位V   Send_Count = data.DataScope_Data_Generate(4);   for ( i = 0 ; i < Send_Count; i++)   {     Serial.write(DataScope_OutPut_Buffer[i]);     }   delay(50);  //上位機必須嚴格控制發(fā)送時序 } /************************************************************************** 函數(shù)功能：按鍵掃描  作者：平衡小車之家 入口參數(shù)：無 返回  值：按鍵狀態(tài)，1：單擊事件，0：無事件。 **************************************************************************/ unsigned char My_click(void){   static unsigned char flag_key = 1; //按鍵按松開標志   unsigned char Key;      Key = digitalRead(KEY);   //讀取按鍵狀態(tài)   if (flag_key && Key == 0) //如果發(fā)生單擊事件   {     flag_key = 0;     return 1;            // 單擊事件   }   else if (1 == Key)     flag_key = 1;   return 0;//無按鍵按下 } /************************************************************************** 函數(shù)功能：直立PD控制  作者：平衡小車之家 入口參數(shù)：角度、角速度 返回  值：直立控制PWM **************************************************************************/ int balance(float Angle, float Gyro) {   float Bias;   int balance;   Bias = Angle - 0;   //===求出平衡的角度中值 和機械相關(guān)   balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===計算平衡控制的電機PWM  PD控制   kp是P系數(shù) kd是D系數(shù)   return balance; } /************************************************************************** 函數(shù)功能：速度PI控制 作者：平衡小車之家 入口參數(shù)：左輪編碼器、右輪編碼器 返回  值：速度控制PWM **************************************************************************/ int velocity(int encoder_left, int encoder_right) {   static float Velocity, Encoder_Least, Encoder, Movement;   static float Encoder_Integral, Target_Velocity;   float kp = 2, ki = kp / 200;    //PI參數(shù)   if       ( Flag_Qian == 1)Movement = 600;   else   if ( Flag_Hou == 1)Movement = -600;   else    //這里是停止的時候反轉(zhuǎn)，讓小車盡快停下來   {     Movement = 0;     if (Encoder_Integral > 300)   Encoder_Integral -= 200;     if (Encoder_Integral < -300)  Encoder_Integral += 200;   }   //=============速度PI控制器=======================//   Encoder_Least = (encoder_left + encoder_right) - 0;               //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零）   Encoder *= 0.7;                                                   //===一階低通濾波器   Encoder += Encoder_Least * 0.3;                                   //===一階低通濾波器   Encoder_Integral += Encoder;                                      //===積分出位移 積分時間：40ms   Encoder_Integral = Encoder_Integral - Movement;                   //===接收遙控器數(shù)據(jù)，控制前進后退   if (Encoder_Integral > 21000)    Encoder_Integral = 21000;        //===積分限幅   if (Encoder_Integral < -21000) Encoder_Integral = -21000;         //===積分限幅   Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                  //===速度控制   if (Turn_Off(KalFilter.angle, Battery_Voltage) == 1 || Flag_Stop == 1)    Encoder_Integral = 0;//小車停止的時候積分清零   return Velocity; } /************************************************************************** 函數(shù)功能：轉(zhuǎn)向控制 作者：平衡小車之家 入口參數(shù)：Z軸陀螺儀 返回  值：轉(zhuǎn)向控制PWM **************************************************************************/ int turn(float gyro)//轉(zhuǎn)向控制 {   static float Turn_Target, Turn, Turn_Convert = 3;   float Turn_Amplitude = 80, Kp = 2, Kd = 0.001;  //PD參數(shù)   if (1 == Flag_Left)             Turn_Target += Turn_Convert;  //根據(jù)遙控指令改變轉(zhuǎn)向偏差   else if (1 == Flag_Right)       Turn_Target -= Turn_Convert;//根據(jù)遙控指令改變轉(zhuǎn)向偏差   else Turn_Target = 0;   if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===轉(zhuǎn)向速度限幅   if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;   Turn = -Turn_Target * Kp + gyro * Kd;         //===結(jié)合Z軸陀螺儀進行PD控制   return Turn; } /************************************************************************** 函數(shù)功能：賦值給PWM寄存器 作者：平衡小車之家 入口參數(shù)：左輪PWM、右輪PWM 返回  值：無 **************************************************************************/ void Set_Pwm(int moto1, int moto2) {   if (moto1 > 0)     digitalWrite(IN1, HIGH),      digitalWrite(IN2, LOW);  //TB6612的電平控制   else             digitalWrite(IN1, LOW),       digitalWrite(IN2, HIGH); //TB6612的電平控制   analogWrite(PWMA, abs(moto1)); //賦值給PWM寄存器   if (moto2 < 0) digitalWrite(IN3, HIGH),     digitalWrite(IN4, LOW); //TB6612的電平控制   else        digitalWrite(IN3, LOW),      digitalWrite(IN4, HIGH); //TB6612的電平控制   analogWrite(PWMB, abs(moto2));//賦值給PWM寄存器 } /************************************************************************** 函數(shù)功能：限制PWM賦值  作者：平衡小車之家 入口參數(shù)：無 返回  值：無 **************************************************************************/ void Xianfu_Pwm(void) {   int Amplitude = 250;  //===PWM滿幅是255 限制在250   if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一個衡量平衡小車電機和機械安裝差異的一個變量。直接作用于輸出，讓小車具有更好的一致性。   if(Flag_Hou==1)   Motor2-=DIFFERENCE-2;   if (Motor1 < -Amplitude) Motor1 = -Amplitude;   if (Motor1 > Amplitude)  Motor1 = Amplitude;   if (Motor2 < -Amplitude) Motor2 = -Amplitude;   if (Motor2 > Amplitude)  Motor2 = Amplitude; } /************************************************************************** 函數(shù)功能：5ms控制函數(shù) 核心代碼 作者：平衡小車之家 入口參數(shù)：無 返回  值：無 **************************************************************************/ void control() {   static int Velocity_Count, Turn_Count, Encoder_Count;   static float Voltage_All,Voltage_Count;   int Temp;   sei();//全局中斷開啟   Mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  //獲取MPU6050陀螺儀和加速度計的數(shù)據(jù)   KalFilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);          //通過卡爾曼濾波獲取角度   Angle = KalFilter.angle;//Angle是一個用于顯示的整形變量   Balance_Pwm = balance(KalFilter.angle, KalFilter.Gyro_x);//直立PD控制 控制周期5ms   if (++Velocity_Count >= 8) //速度控制，控制周期40ms   {     Velocity_Left = Velocity_L;    Velocity_L = 0;  //讀取左輪編碼器數(shù)據(jù)，并清零，這就是通過M法測速（單位時間內(nèi)的脈沖數(shù)）得到速度。     Velocity_Right = Velocity_R;    Velocity_R = 0; //讀取右輪編碼器數(shù)據(jù)，并清零     Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms     Velocity_Count = 0;   }   if (++Turn_Count >= 4)//轉(zhuǎn)向控制，控制周期20ms   {     Turn_Pwm = turn(gz);     Turn_Count = 0;   }   Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm;  //直立速度轉(zhuǎn)向環(huán)的疊加   Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度轉(zhuǎn)向環(huán)的疊加   Xianfu_Pwm();//限幅   if (Pick_Up(az, KalFilter.angle, Velocity_Left, Velocity_Right))   Flag_Stop = 1;  //===如果被拿起就關(guān)閉電機//===檢查是否小車被那起   if (Put_Down(KalFilter.angle, Velocity_Left, Velocity_Right))      Flag_Stop = 0;              //===檢查是否小車被放下   if (Turn_Off(KalFilter.angle, Battery_Voltage) == 0)        Set_Pwm(Motor1, Motor2);//如果不存在異常，賦值給PWM寄存器控制電機   if (My_click()) Flag_Stop = !Flag_Stop;   //中斷剩余的時間掃描一下按鍵狀態(tài)   Temp = analogRead(0);  //采集一下電池電壓   Voltage_Count++;       //平均值計數(shù)器   Voltage_All+=Temp;     //多次采樣累積   if(Voltage_Count==200) Battery_Voltage=Voltage_All*0.05371/200,Voltage_All=0,Voltage_Count=0;//求平均值 } /************************************************************************** 函數(shù)功能：初始化 相當于STM32里面的Main函數(shù) 作者：平衡小車之家 入口參數(shù)：無 返回  值：無 **************************************************************************/ void setup() {   pinMode(IN1, OUTPUT);        //TB6612控制引腳，控制電機1的方向，01為正轉(zhuǎn)，10為反轉(zhuǎn)   pinMode(IN2, OUTPUT);          //TB6612控制引腳，   pinMode(IN3, OUTPUT);          //TB6612控制引腳，控制電機2的方向，01為正轉(zhuǎn)，10為反轉(zhuǎn)   pinMode(IN4, OUTPUT);          //TB6612控制引腳，   pinMode(PWMA, OUTPUT);         //TB6612控制引腳，電機PWM   pinMode(PWMB, OUTPUT);         //TB6612控制引腳，電機PWM   digitalWrite(IN1, 0);          //TB6612控制引腳拉低   digitalWrite(IN2, 0);          //TB6612控制引腳拉低   digitalWrite(IN3, 0);          //TB6612控制引腳拉低   digitalWrite(IN4, 0);          //TB6612控制引腳拉低   analogWrite(PWMA, 0);          //TB6612控制引腳拉低   analogWrite(PWMB, 0);          //TB6612控制引腳拉低   pinMode(2, INPUT);       //編碼器引腳   pinMode(4, INPUT);       //編碼器引腳   pinMode(5, INPUT);       //編碼器引腳   pinMode(8, INPUT);       //編碼器引腳   pinMode(3, INPUT);       //按鍵引腳   Wire.begin();             //加入 IIC 總線   Serial.begin(9600);       //開啟串口，設置波特率為 9600   delay(1500);              //延時等待初始化完成   Mpu6050.initialize();     //初始化MPU6050   delay(20);   if(digitalRead(KEY)==0) {    //讀取EEPROM的參數(shù)   Balance_Kp =  (float)((EEPROM.read(addr+0)*256)+EEPROM.read(addr+1) )/100;   Balance_Kd =  (float)((EEPROM.read(addr+2)*256)+EEPROM.read(addr+3))/100;   Velocity_Kp = (float)((EEPROM.read(addr+4)*256)+EEPROM.read(addr+5))/100;   Velocity_Ki = (float)((EEPROM.read(addr+6)*256)+EEPROM.read(addr+7))/100;   }   MsTimer2::set(5, control);  //使用Timer2設置5ms定時中斷   MsTimer2::start();          //使用中斷使能   attachInterrupt(0, READ_ENCODER_L, CHANGE);           //開啟外部中斷 編碼器接口1   attachPinChangeInterrupt(4, READ_ENCODER_R, CHANGE);  //開啟外部中斷 編碼器接口2 } /************************************************************************** 函數(shù)功能：主循環(huán)程序體 入口參數(shù)：無 返回  值：無 **************************************************************************/ void loop() {   int Voltage_Temp;   static unsigned char flag;   unsigned char Balance_Kp_Temp=0,Balance_Kd_Temp=0,Velocity_Kp_Temp=0,Velocity_Ki_Temp=0;   Voltage_Temp = (Battery_Voltage - 11.1) * 60;  //根據(jù)APP的協(xié)議對電池電壓變量進行處理   if (Voltage_Temp > 100)Voltage_Temp = 100;   if (Voltage_Temp < 0)Voltage_Temp = 0;   if (Flag_Stop == 0)   {     Serial.begin(9600);       //開啟串口，設置波特率為 9600     flag=!flag;       if(PID_Send==1)//發(fā)送PID參數(shù)   {     Serial.print("{C");     Serial.print((int)(Balance_Kp*100));   //左輪編碼器     Serial.print(":");     Serial.print((int)(Balance_Kd*100));  //右輪編碼器 …………限于本文篇幅 余下代碼請從51黑下載附件………… 復制代碼 全部資料下載地址： Minibalance For Arduino平衡小車原理圖.pdf (203.8 KB, 下載次數(shù): 220) 2017-7-12 18:29 上傳 點擊文件名下載附件 下載積分: 黑幣 -5 Arduino平衡小車源碼.zip (145.75 KB, 下載次數(shù): 273) 2017-7-12 18:31 上傳 點擊文件名下載附件 下載積分: 黑幣 -5

評分

參與人數(shù) 6	黑幣 +32	收起 理由
1750936835	+ 5	贊一個！
QQwert	+ 5
nick0411013	+ 1	很給力！
周得利	+ 5	共享資料的黑幣獎勵！
我西爸爸	+ 8
偶游QHD	+ 8	解釋非常詳盡的代碼，非常感謝！

查看全部評分