|
|
前言
今天聊一聊代碼，只有直立功能的代碼。
代碼總體思路
給定一個目標值，單片機通過IIC和mpu6050通信，得知數據后，根據角度環計算出一個PWM值給電機驅動器，從而控制單機轉動。電機轉動，編碼器就會輸出脈沖，單片機通過編碼器模式檢測脈沖值，并把該值代入速度環計算出一個角度值送給角度環，角度環在輸出PWM給電機，以此循環。其實就是上節的框圖，再貼一遍。
程序
程序部分為了減少篇幅，之前講解的編碼器模式及pwm等代碼都不在講解，原理都一樣，只是引腳不同，我只把重點代碼說一下。如果想看這部分代碼的同學，后面我整理之后會放在公眾號。
時序
通過外部中斷檢測mpu6050INT引腳，檢測到信號之后就進行一次pid運算。以此來嚴格控制PID計算周期。可能有些人會有疑問，為什么不放到while中循環檢測？因為放在while循環的話，這個循環是可以被中斷打斷的。小車平衡對實時性要求高，如果在while循環里，姿態矯正時，程序被其他模塊中斷，小車就立不起來了。因此外部中斷的優先級要配置成最高。
代碼如下：
/************************外部中斷，通過mpu6050的INT引腳，嚴格控制PID計算周期 PB5引腳**************************/void MPU6050_EXTI_Init(void){ EXTI_InitTypeDef EXTI_InitStruct; GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO,ENABLE);//開啟時鐘 GPIO_InitStruct.GPIO_Mode=GPIO_Mode_IPU; //上拉輸入 GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5; //PB5 GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz; GPIO_Init(GPIOB,&GPIO_InitStruct); GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);//配置中斷線 EXTI_InitStruct.EXTI_Line=EXTI_Line5;//EXTI中斷/事件線選擇，可選 EXTI0 至 EXTI19 EXTI_InitStruct.EXTI_LineCmd=ENABLE;//使能 EXTI_InitStruct.EXTI_Mode=EXTI_Mode_Interrupt;//EXTI 模式選擇 EXTI_InitStruct.EXTI_Trigger=EXTI_Trigger_Falling;//EXTI 邊沿觸發事件 EXTI_Init(&EXTI_InitStruct);//初始化中斷}//優先級配置void NVIC_Config(void){ NVIC_InitTypeDef NVIC_InitStruct; NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//4級搶占,4級響應。 //外部中斷 NVIC_InitStruct.NVIC_IRQChannel=EXTI9_5_IRQn; NVIC_InitStruct.NVIC_IRQChannelCmd=ENABLE; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority=0; NVIC_InitStruct.NVIC_IRQChannelSubPriority=0; NVIC_Init(&NVIC_InitStruct);}
直立環
原理上節都講過了，必要的地方我在程序中已經注釋了。
/**************************************************************************函數功能：直立PD控制入口參數：角度、角速度返回 值：直立控制PWM作 者：平衡小車之家**************************************************************************/int balance(float Angle,float Gyro){ float Bias; int balance; Bias=Angle-ZHONGZHI; //===求出平衡的角度中值 和機械相關 balance=Balance_Kp*Bias+Gyro*Balance_Kd; //===計算平衡控制的電機PWM PD控制 kp是P系數 kd是D系數 return balance;}
上面角度環的公式是不是和上節中推導的一樣。
Angle - Med_Angle：
Angle 為真實角度Med_Angle為機械中值角度，也就是當小車直立時mpu6050所檢測到的角度，該角度并不一定為0Angle - Med_Angle為真實角度和機械中值角度的偏差
測量機械中值的方法（手動）：
將oled和mpu6050都連接好并調試通過之后，將采樣的Pitch值顯示在oled上。之后讓小車輪子固定不動，用手沿電機旋轉方向先逆時針（順時針）慢慢推動小車上方，讓小車直立，當小車從直立狀態向逆時針方向傾斜時，看看屏幕上顯示的角度是多少，之后同樣的方法測順時針，看看角度是多少，之后兩值相加/2，得出的值就是機械中值。
速度環
/**************************************************************************函數功能：速度PI控制 修改前進后退速度，請修Target_Velocity，比如，改成60就比較慢了入口參數：左輪編碼器、右輪編碼器返回 值：速度控制PWM作 者：平衡小車之家**************************************************************************/int velocity(int encoder_left,int encoder_right){ static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral,Target_Velocity; //=============速度PI控制器=======================// Encoder_Least =(encoder_left+encoder_right)-0; //===獲取最新速度偏差==測量速度（左右編碼器之和）-目標速度（此處為零） Encoder *= 0.7; //===一階低通濾波器 Encoder += Encoder_Least*0.3; //===一階低通濾波器 Encoder_Integral +=Encoder; //===積分出位移 積分時間：10ms //Encoder_Integral=Encoder_Integral-Movement; //===接收遙控器數據，控制前進后退 if(Encoder_Integral>10000) Encoder_Integral=10000; //===積分限幅 if(Encoder_Integral<-10000) Encoder_Integral=-10000; //===積分限幅 Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===速度控制 return Velocity;}
速度環加入低通濾波，目的為了減少速度環的作用，畢竟直立才是最重要的。而且速度環相對于直立環來說是個干擾，因此不能讓速度環作用太大。
轉向環
/**************************************************************************函數功能：轉向控制 修改轉向速度，請修改Turn_Amplitude即可入口參數：左輪編碼器、右輪編碼器、Z軸陀螺儀返回 值：轉向控制PWM作 者：平衡小車之家**************************************************************************/int turn(int encoder_left,int encoder_right,float gyro)//轉向控制{ static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; float Kd=1.3; //=============轉向PD控制器=======================// Turn=-gyro*Kd; //===結合Z軸陀螺儀進行PD控制 return Turn;}
轉向環目的為了保持直線行走。假設小車發生偏移，gyro就不為0，轉向環就會有輸出。
最終控制代碼
int EXTI9_5_IRQHandler(void){ int PWM_out; if(EXTI_GetITStatus(EXTI_Line5)!=0)//一級判定 { if(PBin(5)==0)//二級判定 { EXTI_ClearITPendingBit(EXTI_Line5);//清除中斷標志位 //進入中斷首先讀取數據----編碼器數據和角度數據 Encoder_Left=-Read_Encoder(2); //===讀取編碼器的值 Encoder_Right=Read_Encoder(4); //===讀取編碼器的值 mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺儀 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度 Balance_Pwm =balance(Pitch,gyroy); //===角度環 Velocity_Pwm=velocity(Encoder_Left,Encoder_Right); //===速度環PID控制 記住，速度反饋是正反饋，就是小車快的時候要慢下來就需要再跑快一點 Turn_Pwm =turn(Encoder_Left,Encoder_Right,gyroz); //===轉向環PID控制 PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最終輸出 Moto1=PWM_out+Turn_Pwm; //===計算左輪電機最終PWM Moto2=PWM_out-Turn_Pwm; //===計算右輪電機最終PWM Xianfu_Pwm(); //===PWM限幅 Set_Pwm(Moto1,Moto2); //===賦值給PWM寄存器 } } return 0; }
注意
PWM_out=Balance_Pwm - Balance_Kp*Velocity_Pwm;//最終輸出
由于是串行PID，因此速度環的輸出就是直立環的輸入，如上面的框圖一樣。因此將速度環代入到直立環當中，就可以求出最終輸出。也就是將Velocity_Pwm代入到角度環形參Angle中即可得出最終輸出公式。
mpu6050、oled代碼移植
這兩個硬件相關的原理就不講了，網上有很多視頻講解，這里簡單說下移植步驟吧。
第一步：找到相關的.c 和 .h文件
將這幾個文件復制到你的HAREWARE文件夾中，可以分別單獨建個文件夾，方便查找。如下圖：
之后再keil中添加C文件及設置頭文件查找路徑：
之后進行編譯，就能調用函數了。主函數初始化：
OLED_Init(); OLED_Clear(); MPU_Init(); mpu_dmp_init();
while中一直刷新平衡角：
while(1) { OLED_Float(0,10,Pitch,3); }
讀取角度的函數放在外部中斷中：
mpu_dmp_get_data(&Pitch,&Roll,&Yaw); //角度 MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //陀螺儀 MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //加速度
總結
原理明白之后，在看代碼是不是很簡單，先在天上飄一會。。。
一路過來，其實平衡小車也不難嘛。再飄一會。。。
。。。。下不來了。
|
-
-
平衡小車-基礎版.7z
2023-4-17 03:53 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
247.56 KB, 下載次數: 13, 下載積分: 黑幣 -5
|
