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