超詳細雙輪平衡小車原理分析附STM32源碼

ID:380054 · 發(fā)表于 2018-9-19 23:06

手中有些關于雙輪平衡小車的資料，特與大家分享，一同學習進步。

一、平衡小車原理

平衡原理
平衡小車是通過兩個電機運動下實現(xiàn)小車不倒下直立行走的多功能智能小車，在外力的推拉下，小車依然保持不倒下。這么一說可能還沒有很直觀的了解究竟什么是平衡小車，不過這個平衡小車實現(xiàn)的原理其實是在人們生活中的經(jīng)驗得來的。如果通過簡單的練習，一般人可以通過自己的手指把木棒直立而不倒的放在指尖上，所以練習的時候，需要學會的兩個條件：一是放在指尖上可以移動，二是通過眼睛觀察木棒的傾斜角度和傾斜趨勢（角速度）。通過手指的移動去抵消木棒傾斜的角度和趨勢，使得木棒能直立不倒。這樣的條件是不可以缺一的，實際上加入這兩個條件，控制過程中就是負反饋機制。

而世界上沒有任何一個人可以蒙眼不看，就可以直立木棒的，因為沒有眼睛的負反饋，就不知道筆的傾斜角度和趨勢。這整個過程可以用一個執(zhí)行式表達：

平衡小車也是這樣的過程，通過負反饋實現(xiàn)平衡。與上面保持木棒直立比較則相對簡單，因為小車有兩個輪子著地，車體只會在輪子滾動的方向上發(fā)生傾斜。控制輪子轉動，抵消在一個維度上傾斜的趨勢便可以保持車體平衡了。

所以根據(jù)上述的原理，通過測量小車的傾角和傾角速度控制小車車輪的加速度來消除小車的傾角。因此，小車傾角以及傾角速度的測量成為控制小車直立的關鍵。我們的亞博智能平衡小車使用了測量傾角和傾角速度的集成傳感器陀螺儀-MPU6050（模塊詳細介紹在亞博智能平衡小車光盤資料3.硬件資料中）。

二、角度（物理分析 PD 算法）

控制平衡小車，使得它作加速運動。這樣站在小車上（非慣性系，以車輪作為坐標原點）分析倒立擺受力，它就會受到額外的慣性力，該力與車輪的加速度方向相反，大小成正比。這樣倒立擺(如圖 2)所受到的回復力為：公式 1
F = mg sin θ-ma cos θ≈mg θ-mk1θ 式1 中，由于θ很小，所以進行了線性化。假設負反饋控制是車輪加速度 a 與偏角θ成正比，比例為 k1。如果比例k1>g，（g 是重力加速度）那么回復力的方向便于位移方向相反了。

而為了讓倒立擺能夠盡快回到垂直位置穩(wěn)定下來，還需要增加阻尼力。增加的阻尼力與偏角的速度成正比，方向相反，因此公式1可改為：F = mg θ-mk1 θ -mk2 θ` 按照上述倒立擺的模型，可得出控制小車車輪加速度的算法：
a =k1θ+k2θ` 式中θ為小車角度,θ`為角速度。k1 k2都是比例系數(shù) 根據(jù)上述內容，建立速度的比例微分負反饋控制，根據(jù)基本控制理論討論小車通過閉環(huán)控制保持穩(wěn)定的條件（這里需要對控制理論有基本了解）。假設外力干擾引起車模產(chǎn)生角加速度x（t）。沿著垂直于車模地盤方向進行受力分析，可以得到車模傾角與車輪運動加速度以及外力干擾加速度a（t）x（t）之間的運動方程。如圖3所示。

圖3

在角度反饋控制中，與角度成比例的控制量是稱為比例控制；與角速度成比例的控制量稱為微分控制（角速度是角度的微分）。因此上面系數(shù) k1,k2 分別稱為比例和微分控制參數(shù)。其中微分參數(shù)相當于阻尼力，可以有效抑制車模震蕩。通過微分抑制控制震蕩的思想在后面的速度和方向控制中也同樣適用。總結控制車模直立穩(wěn)定的條件如下：
（1）能夠精確測量車模傾角θ的大小和角速度θ'的大��；

（2）可以控制車輪的加速度。

上述控制實際結果是小車與地面不是嚴格垂直，而是存在一個對應的傾角。在重力的作用下，小車會朝著一個方面加速前進。為了保持小車的靜止或者勻速運動需要消除這個安裝誤差。在實際小車制作過程中需要進行機械調整和軟件參數(shù)設置。另外需要通過軟件中的速度控制來實現(xiàn)速度的穩(wěn)定性。在小車角度控制中出現(xiàn)的小車傾角偏差，使得小車在傾斜的方向上產(chǎn)生加速。這個結果可以用來進行小車的速度控制。下面將利用這個原理來調節(jié)小車的速度。

三、測速（物理模型建立數(shù)學模型傳遞函數(shù) PD 算法）假設小車在上面直立控制調節(jié)下已經(jīng)能夠保持平衡了，但是由于
安裝誤差，傳感器實際測量的角度與車模角度有偏差，因此小車實際不是保持與地面垂直，而是存在一個傾角。在重力的作用下，小車就會朝傾斜的方向加速前進。控制速度只要通過控制小車的傾角就可以實現(xiàn)了。具體實現(xiàn)需要解決三個問題：
（1）如何測量小車速度？

（2）如何通過小車直立控制實現(xiàn)小車傾角的改變？

（3）如何根據(jù)速度誤差控制小車傾角？第一個問題可以通過安裝在電機輸出軸上的光碼盤來測量得到
小車的車輪速度。如圖 4 所示。利用控制單片機的計數(shù)器測量在固定時間間隔內速度脈沖信號的個數(shù)可以反映電機的轉速。

圖4

第二個問題可以通過角度控制給定值來解決。給定小車直立控制的設定值，在角度控制調節(jié)下，小車將會自動維持在一個角度。通過前面小車直立控制算法可以知道，小車傾角最終是跟蹤重力加速度Z 軸的角度。因此小車的傾角給定值與重力加速度Z軸角度相減，便可以最終決定小車的傾角
第三個問題分析起來相對比較困難，遠比直觀進行速度負反饋分析復雜。首先對一個簡單例子進行分析。假設小車開始保持靜止，然后增加給定速度，為此需要小車往前傾斜以便獲得加速度。在小車直立控制下，為了能夠有一個往前的傾斜角度，車輪需要往后運動，這樣會引起車輪速度下降（因為車輪往負方向運動了）。由于負反饋，使得小車往前傾角需要更大。如此循環(huán)，小車很快就會傾倒。原本利用負反饋進行速度控制反而成了“正”反饋。
為什么負反饋控制在這兒失靈了呢？原來在直立控制下的小車速度與小車傾角之間傳遞函數(shù)具有非最小相位特性（在此省略了分析），在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。
為了保證系統(tǒng)穩(wěn)定，往往取的小車傾角控制時間常數(shù)Tz很大。這樣便會引起系統(tǒng)產(chǎn)生兩個共軛極點，而且極點的實部變得很小，使得系統(tǒng)的速度控制會產(chǎn)生的震蕩現(xiàn)象。這個現(xiàn)象在實際參數(shù)整定的時候可以觀察到。那么如何消除速度控制過程中的震蕩呢？要解決控制震蕩問題，在前面的小車角度控制中已經(jīng)有了經(jīng)驗，那就是在控制反饋中增加速度微分控制。但由于車輪的速度反饋信號中往往存在著噪聲，對速度進行微分運算會進一步加大噪聲的影響。為此需要對上面控制方法進行改進。原系統(tǒng)中傾角調整過程時間常數(shù) 往往很大，因此可以將該系統(tǒng)近似為一個積分環(huán)節(jié)。將原來的微分環(huán) 節(jié)和這個積分環(huán)節(jié)合并，形成一個比例控制環(huán)節(jié)。這樣可以保持系統(tǒng) 控制傳遞函數(shù)不變，同時避免了微分計算。
但在控制反饋中，只是使用反饋信號的比例和微分，沒有利誤差積分，所以最終這個速度控制是有殘差的控制。但是直接引入誤差積分控制環(huán)節(jié)，會增加系統(tǒng)的復雜度，為此就不再增加積分控制，而是通過與角度控制相結合后在進行改進。

要求小車在原地停止，速度為0。但是由于采用的是比例控制，如果此時陀螺儀有漂移，或者加速度傳感器安裝有誤差，最終小車傾角不會最終調整到0，小車會朝著傾斜的方向恒速運行下去。注意此時車模不會像沒有速度控制那樣加速運行了，但是速度不會最終為0。為了消除這個誤差，可以將小車傾角設定量直接積分補償在角度控制輸出中，這樣就會徹底消除速度控制誤差。第二點，由于加入了速度控制，它可以補償陀螺儀和重力加速度的漂移和誤差。所以此時重力加速度傳感器實際上沒有必要了。
此時小車在控制啟動的時候，需要保持小車的垂直狀態(tài)。此時陀螺儀的積分角度也初始化為0。當然如果電路中已經(jīng)包括了重力加速度傳感器，也可以保留這部分，從而提高小車的穩(wěn)定性。在后面的最終給定的控制方案中，保留了這部分的控制回路。

四、轉向控制（PD 算法）通過左右電機速度差驅動小車轉向消除小車距離道路中心的偏
差。通過調整小車的方向，再加上車前行運動，可以逐步消除小車距離中心線的距離差別。這個過程是一個積分過程，因此小車差動控制一般只需要進行簡單的比例控制就可以完成小車方向控制。但是由于小車本身安裝有電池等比較重的物體，具有很大的轉動慣量，在調整過程中會出現(xiàn)小車轉向過沖現(xiàn)象，如果不加以抑制，會使得小車過度轉向而倒下。根據(jù)前面角度和速度控制的經(jīng)驗，為了消除小車方向控制中的過沖，需要增加微分控制。

五、全方案整合通過上面介紹，將車模直立行走主要的控制算法集中起來，如圖5

圖5

為了實現(xiàn)小車直立行走，需要采集如下信號：

（1）小車傾角速度陀螺儀信號，獲得小車的傾角和角速度。

（2）重力加速度信號

（z軸信號），補償陀螺儀的漂移。該信號可以省略，有速度控制替代。
（3）小車電機轉速脈沖信號，獲得小車運動速度，進行速度控制。

（4）小車轉動速度陀螺儀信號，獲得小車轉向角速度，進行方向控制。

在小車控制中的直立、速度和方向控制三個環(huán)節(jié)中，都使用了比例微分（PD）控制，這三種控制算法的輸出量最終通過疊加通過電機運動來完成。
（1）小車直立控制：使用小車傾角的PD（比例、微分）控制；

g_fAngleControlOut = g_fCarAngle * g_fCarAngle_P + \

gyro[0] * g_fCarAngle_D ;

（2）小車速度控制：使用PD（比例、微分）控制；

g_fSpeedControlOutNew = (CAR_SPEED_SET - g_fCarSpeed) *

g_fCarSpeed_P +\

(CAR_POSITION_SET - g_fCarPosition) * g_fCarSpeed_I;

（3）小車方向控制：使用PD（比例、微分）控制。

speednow=-speedtarget*3.4 -gyro[2]*0.0015 ;

可通過單片機軟件實現(xiàn)上述控制算法。在上面控制過程中，車模的角度控制和方向控制都是直接將輸出電壓疊加后控制電機的轉速實現(xiàn)的。而車模的速度控制本質上是通過調節(jié) 車模的傾角實現(xiàn)的，由于車模是一個非最小相位系統(tǒng)，因此該反饋控制如果比例和速度過大，很容易形成正反饋，使得車模失控，造成系統(tǒng)的不穩(wěn)定性。因此速度的調節(jié)過程需要非常緩慢和平滑。

六、PID 算法

圖 6
控制相關的軟件函數(shù)包括：
1.AngleCalculate：小車傾角計算函數(shù)。根據(jù)采集到的陀螺儀和重力加速度傳感器的數(shù)值計算小車角度和角速度。如果這部分的算法由外部一個運放實現(xiàn)，那么采集得到的直接是小車的角度和角速度，這部分算法可以省略。該函數(shù)是每 5 毫秒調用一次。
2.AngelControl：小車直立控制函數(shù)。根據(jù)小車角度和角速度計算小車電機的控制量。直立控制是 5 毫秒調用一次。
3.SpeedControl：小車速度控制函數(shù)。根據(jù)小車采集到的電機轉速和速度設定值，計算電機的控制量。該函數(shù)是 100 毫秒調用一次。
4.SpeedControlOutput：速度輸出平滑函數(shù)。由于速度是每 100 毫秒進行一次計算。為了使得速度控制更加平滑，該函數(shù)將速度輸出變化量平均分配到 20步 5 毫秒的控制周期中。
5.DirectionControlOutput：方向控制函數(shù)輸出平滑函數(shù)。將方向控制的輸出變化量平均分配到 2 步 5 毫秒的控制周期中。
6.MotorOutput：電機輸出量匯集函數(shù)。根據(jù)前面的直立控制、速度控制和方向控制所得到的控制量進行疊加，分別得到左右兩個電極的輸出電壓控制量。對疊加后的輸出量進行飽
和處理。函數(shù)調用周期 5 毫秒。在此請大家注意速度控制量疊加的極性是負。
7.MotorSpeedOut：電機 PWM 輸出計算函數(shù)。根據(jù)左右兩個電極的輸出控制量的正負極性，疊加上一個小的死區(qū)數(shù)值，克服車模機械靜態(tài)摩擦力。函數(shù)調用周期 5 毫秒。
8.SetMotorVoltage：PWM 輸出函數(shù)：根據(jù)兩個電機的輸出量，計算出 PWM 控制寄存器的數(shù)值，設置四個 PWM 控制寄存器的數(shù)值。函數(shù)調用周期 1 毫秒。
以上 9 個函數(shù)都是在 1 毫秒中斷服務中進行被相互調用的。下圖顯示了這些函數(shù)之間的調用
與參數(shù)傳遞關系。在個函數(shù)附近也表明了調用的周期。
9.Chaoshengbo：加入超聲波壁障模塊：根據(jù)前方障礙物的距離檢測，一旦檢測到后，通過直接 PWM 值輸出（ g_fchaoshengbooutput），相障礙物反方上運動，無需算法實現(xiàn)。每
30 毫秒調用一次。

圖 7

（注：以上函數(shù)框圖為未添加旋轉及超聲波，添加方法與融合速度方式一樣）

七、程序（只給出一部分內容）

（1）時序總算法

1

單片機源程序如下:

#include "upstandingcar.h"
#include "I2C_MPU6050.h"
#include "MOTOR.h"
#include "led.h"
#include "USART.H"
#include "MPU6050.H"
#include "UltrasonicWave.h"
#include "stm32f10x_gpio.h"
#include "math.h"
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
/*****************************************/
u8 newLineReceived = 0;
u8 inputString[80] = {0};
u8 ProtocolString[80] = {0};
/*小車運行狀態(tài)枚舉*/
enum{
enSTOP = 0,
enRUN,
enBACK,
enLEFT,
enRIGHT,
enTLEFT,
enTRIGHT
}enCarState;
#define run_car '1'//按鍵前
#define back_car '2'//按鍵后
#define left_car '3'//按鍵左
#define right_car '4'//按鍵右
#define stop_car '0'//按鍵停
int g_newcarstate = enSTOP; // 1前2后3左4右0停止
char returntemp[] = "$0,0,0,0,0,0,0,0,0,0,0,0cm,8.2V#";
char piddisplay[50] ="$AP";
char manydisplay[80] ={0};
char updata[80] ={0};
/*****************多數(shù)據(jù)************************/
u8 BST_u8MainEventCount; //主循環(huán)判斷計數(shù) 在SysTick_Handler(void)中使用每1ms加1
u8 BST_u8SpeedControlCount; //速度控制循環(huán)計數(shù) 在SysTick_Handler(void)中使用每5ms加1
u8 BST_u8SpeedControlPeriod;
u8 BST_u8DirectionControlPeriod;
u8 BST_u8DirectionControlCount; //轉向控制循環(huán)計數(shù) 在SysTick_Handler(void)中使用每5ms加1
u8 BST_u8trig;
u8 ucBluetoothValue; //藍牙接收數(shù)據(jù)
float volt = 12.0;
/******電機控制參數(shù)******/
float BST_fSpeedControlOut; //速度控制PWM
float BST_fSpeedControlOutOld;
float BST_fSpeedControlOutNew;
float BST_fAngleControlOut;
float BST_fLeftMotorOut;
float BST_fRightMotorOut;
float BST_fCarAngle; //角度控制PWM
float gyro_z;
float gyrx;
float gy0;
/*-----角度環(huán)和速度環(huán)PID控制參數(shù)-----*///以下參考為重點調試參考，同電池電壓有關，建議充好電再調試
float BST_fCarAngle_P =91.3;// 91.3 //調大小時會左右擺，調大時會振動請調到基本能夠站立 P=91.3是用于給小車在運動過程使用
float BST_fCarAngle_D =0.21; // 0.001 0.002 0.004 0.008 0.0010 0.011 調小時反應慢，調大時會干擾
float BST_fCarSpeed_P=5.1;
float BST_fCarSpeed_I=0.10;
const double PID_Original[4] ={91.3, 0.21, 5.1, 0.10};
char alldata[80];
char *iap;
/******速度控制參數(shù)******/
s16 BST_s16LeftMotorPulse; //左電機脈沖數(shù)
s16 BST_s16RightMotorPulse; //右電機脈沖數(shù)
s32 BST_s32LeftMotorPulseOld;
s32 BST_s32RightMotorPulseOld;
s32 BST_s32LeftMotorPulseSigma; //50ms左電機疊加值
s32 BST_s32RightMotorPulseSigma; //50ms右電機疊加值
float BST_fCarSpeed; //測速碼盤得出的車速
float BST_fCarSpeedOld;
float BST_fCarPosition; //測速碼盤通過計算得到的小車位移
/*-----懸停參數(shù)-----*/
int leftstop=0;
int rightstop=0;
int stopflag=0;
/******超聲波********/
float fchaoshengbo = 0; //超聲波輸出量
float juli = 0; //超聲波距離
/******藍牙控制參數(shù)******/
float BST_fBluetoothSpeed; //藍牙控制車速
float BST_fBluetoothDirectionNew; //用于平緩輸出車速使用
float BST_fBluetoothDirectionSL; //左轉標志位由于PWM轉向輸出使用判斷輸出方式固需要標志位
float BST_fBluetoothDirectionSR; //右轉標志位由于PWM轉向輸出使用判斷輸出方式固需要標志位
int chaoflag=0;
int x,y1,z1,y2,z2,flagbt;
int g_autoup = 0;
int g_uptimes = 5000; //自動上報間隔
char charkp[10],charkd[10],charksp[10],charksi[10];
char lspeed[10],rspeed[10],daccel[10],dgyro[10],csb[10],vi[10];
char kp,kd,ksp,ksi;
float dac = 0,dgy = 0;
/************旋轉*****************/
float BST_fBluetoothDirectionL; //左旋轉標志位由于PWM旋轉輸出使用判斷輸出方式固需要標志位
float BST_fBluetoothDirectionR; //右旋轉標志位由于PWM旋轉輸出使用判斷輸出方式固需要標志位
int driectionxco=800;
//******卡爾曼參數(shù)************
float Q_angle=0.001;
float Q_gyro=0.003;
float R_angle=0.5;
float dt=0.005; //dt為kalman濾波器采樣時間;
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
void SendAutoUp(void);
/**********延時子函數(shù)****************/
void delay_nms(u16 time)
{
u16 i=0;
while(time--)
{
i=12000; //自己定義
while(i--) ;
}
}
/***********************************/
/***************************************************************
** 函數(shù)名稱: CarUpstandInit
** 功能描述: 全局變量初始化函數(shù)
***************************************************************/
void CarUpstandInit(void)
{
BST_s16LeftMotorPulse = BST_s16RightMotorPulse = 0; //左右脈沖值初始化
BST_s32LeftMotorPulseSigma = BST_s32RightMotorPulseSigma = 0; //疊加脈沖數(shù) 初始化
BST_fCarSpeed = BST_fCarSpeedOld = 0; //平衡小車車速初始化
BST_fCarPosition = 0; //平衡小車位移量初始化
BST_fCarAngle = 0; //平衡小車車速初始化
BST_fAngleControlOut = BST_fSpeedControlOut = BST_fBluetoothDirectionNew = 0; //角度PWM、車速PWM、藍牙控制PWM 初始化
BST_fLeftMotorOut = BST_fRightMotorOut = 0; //左右車輪PWM輸出值初始化
BST_fBluetoothSpeed = 0; //藍牙控制車速值初始化
BST_fBluetoothDirectionL =BST_fBluetoothDirectionR= 0; //藍牙控制左右旋轉標志位初始化
BST_fBluetoothDirectionSL =BST_fBluetoothDirectionSR= 0; //藍牙控制左右轉向標志位初始化
BST_u8MainEventCount=0; //用于5ms定時器子程序SysTick_Handler(void)中總中斷計數(shù)位
BST_u8SpeedControlCount=0; //用于5ms定時器子程序SysTick_Handler(void)中50ms速度平衡融入計數(shù)位
BST_u8SpeedControlPeriod=0; //用于5ms定時器子程序SysTick_Handler(void)中50ms速度平衡融入計數(shù)位
fchaoshengbo=0; //用于5ms定時器子程序SysTick_Handler(void)中超聲波平衡融入計數(shù)位
}
void ResetPID()
{
if(BST_fCarAngle_P != PID_Original[0])
{
BST_fCarAngle_P = PID_Original[0];
}
if(BST_fCarAngle_D != PID_Original[1])
{
BST_fCarAngle_D = PID_Original[1];
}
if(BST_fCarSpeed_P != PID_Original[2])
{
BST_fCarSpeed_P = PID_Original[2];
}
if(BST_fCarSpeed_I != PID_Original[3])
{
BST_fCarSpeed_I = PID_Original[3];
}
}
/***************************************************************
** 函數(shù)名稱: AngleControl
** 功能描述: 角度環(huán)控制函數(shù)
***************************************************************/
void AngleControl(void)
{
if(flagbt==1)
{
BST_fCarAngle_P=0;
BST_fCarAngle_P=y1*1.71875;
}
if(flagbt==2)
{
BST_fCarAngle_D=0;
BST_fCarAngle_D=(z1-64)*0.15625;
}
dac=accel[2];
dgy=gyro[2];
BST_fCarAngle = Roll - CAR_ZERO_ANGLE; //DMP ROLL滾動方向角度與預設小車傾斜角度值的差得出角度
BST_fAngleControlOut = BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ; //角度PD控制
}
/***************************************************************
** 函數(shù)名稱: SetMotorVoltageAndDirection
** 功能描述: 電機轉速
***************************************************************/
void SetMotorVoltageAndDirection(s16 s16LeftVoltage,s16 s16RightVoltage)
{
u16 u16LeftMotorValue;
u16 u16RightMotorValue;
if(s16LeftVoltage<0) //當左電機PWM輸出為負時 PB14設為正 PB15設為負（PB14 15 分別控制TB6612fng驅動芯片，邏輯0 1可控制左電機正轉反轉）
{
GPIO_SetBits(GPIOB, GPIO_Pin_14 );
GPIO_ResetBits(GPIOB, GPIO_Pin_15 );
s16LeftVoltage = (-s16LeftVoltage);
}
else
{
GPIO_SetBits(GPIOB, GPIO_Pin_15 ); //當左電機PWM輸出為正時 PB14設為負 PB15設為正（PB14 15 分別控制TB6612fng驅動芯片，邏輯0 1可控制左電機正轉反轉）
GPIO_ResetBits(GPIOB, GPIO_Pin_14 );
s16LeftVoltage = s16LeftVoltage;
}
if(s16RightVoltage<0)
{ //當右電機PWM輸出為負時 PB12設為正 PB13設為負（PB12 13 分別控制TB6612fng驅動芯片，邏輯0 1可控制左電機正轉反轉）
GPIO_SetBits(GPIOB, GPIO_Pin_13 );
GPIO_ResetBits(GPIOB, GPIO_Pin_12 );
s16RightVoltage = (-s16RightVoltage);
}
else //當右電機PWM輸出為正時 PB12設為負 PB13設為正（PB12 13 分別控制TB6612fng驅動芯片，邏輯0 1可控制左電機正轉反轉）
{
GPIO_SetBits(GPIOB, GPIO_Pin_12 );
GPIO_ResetBits(GPIOB, GPIO_Pin_13 );
s16RightVoltage = s16RightVoltage;
}
u16RightMotorValue= (u16)s16RightVoltage;
u16LeftMotorValue = (u16)s16LeftVoltage;
TIM_SetCompare3(TIM2,u16LeftMotorValue); //TIM2與 u16RightMotorValue對比，不相同則翻轉波形，調節(jié)PWM占空比
TIM_SetCompare4(TIM2,u16RightMotorValue); //TIM3與 u16LeftMotorValue對比，不相同則翻轉波形，調節(jié)PWM占空比
#if 1 /*判斷車輛是否懸停或跌倒，調試用*/
if(Pitch>10||Pitch<-10&BST_fBluetoothDirectionSR==0&BST_fBluetoothDirectionSL==0)
{
TIM_SetCompare3(TIM2,0);
TIM_SetCompare4(TIM2,0);
stopflag=1;
}
else stopflag=0;
if(BST_fCarAngle > 50 || BST_fCarAngle < (-50))
{
TIM_SetCompare3(TIM2,0);
TIM_SetCompare4(TIM2,0);
stopflag=1;
}
else stopflag=0;
#endif
}
/***************************************************************
** 函數(shù)名稱: MotorOutput
** 功能描述: 電機輸出函數(shù)
將直立控制、速度控制、方向控制的輸出量進行疊加,并加
入死區(qū)常量，對輸出飽和作出處理。
***************************************************************/
void MotorOutput(void) //電機PWM輸出函數(shù)
{
//右電機轉向PWM控制融合平衡角度、速度輸出
BST_fLeftMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew + BST_fBluetoothDirectionNew;//+directionl - BST_fBluetoothDirectionNew; //左電機轉向PWM控制融合平衡角度、速度輸出
BST_fRightMotorOut = BST_fAngleControlOut +BST_fSpeedControlOutNew - BST_fBluetoothDirectionNew;//-directionl+ BST_fBluetoothDirectionNew; //右電機轉向PWM控制融合平衡角度、速度輸出
if((s16)BST_fLeftMotorOut > MOTOR_OUT_MAX) BST_fLeftMotorOut = MOTOR_OUT_MAX;
if((s16)BST_fLeftMotorOut < MOTOR_OUT_MIN) BST_fLeftMotorOut = MOTOR_OUT_MIN;
if((s16)BST_fRightMotorOut > MOTOR_OUT_MAX) BST_fRightMotorOut = MOTOR_OUT_MAX;
if((s16)BST_fRightMotorOut < MOTOR_OUT_MIN) BST_fRightMotorOut = MOTOR_OUT_MIN;
SetMotorVoltageAndDirection((s16)BST_fLeftMotorOut,(s16)BST_fRightMotorOut);
}
void GetMotorPulse(void) //采集電機速度脈沖
{
//////////////////////////////////此部分為外部中斷計算脈沖/////////////////////////////////////
uint16_t u16TempLeft;
uint16_t u16TempRight;
u16TempLeft = TIM_GetCounter(TIM3); // TIM3定時器計算調用
u16TempRight= TIM_GetCounter(TIM4); // TIM4定時器計算調用
leftstop=u16TempLeft;
rightstop=u16TempRight;
TIM_SetCounter(TIM3,0);//TIM3->CNT = 0;
TIM_SetCounter(TIM4,0);//TIM4->CNT = 0; //清零
BST_s16LeftMotorPulse=u16TempLeft;
BST_s16RightMotorPulse=(-u16TempRight);
BST_s32LeftMotorPulseSigma +=BST_s16LeftMotorPulse; //脈沖值疊加 40ms疊加值
BST_s32RightMotorPulseSigma +=BST_s16RightMotorPulse; //脈沖值疊加 40ms疊加值
}
/***************************************************************
** 函數(shù)名稱: SpeedControl
** 功能描述: 速度環(huán)控制函數(shù)
***************************************************************/
void SpeedControl(void)
{
BST_fCarSpeed = (BST_s32LeftMotorPulseSigma + BST_s32RightMotorPulseSigma );// * 0.5 ; //左右電機脈沖數(shù)平均值作為小車當前車速
BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0; //全局變量注意及時清零
BST_fCarSpeedOld *= 0.7;
BST_fCarSpeedOld +=BST_fCarSpeed*0.3;
BST_fCarPosition += BST_fCarSpeedOld; //路程即速度積分 1/11 3:03
BST_fCarPosition += BST_fBluetoothSpeed; //融合藍牙給定速度
BST_fCarPosition += fchaoshengbo; //融合超聲波給定速度
if(stopflag==1)
{
BST_fCarPosition=0;
}
//積分上限設限//
if((s32)BST_fCarPosition > CAR_POSITION_MAX) BST_fCarPosition = CAR_POSITION_MAX;
if((s32)BST_fCarPosition < CAR_POSITION_MIN) BST_fCarPosition = CAR_POSITION_MIN;
if(flagbt==3)
{
BST_fCarSpeed_P=0;
BST_fCarSpeed_P=(y2-128)*0.46875;
}
if(flagbt==4)
{
BST_fCarSpeed_I=0;
BST_fCarSpeed_I=(z2-192)*0.15625;
}
BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法速度*P +位移*I=速度PWM輸出
}
/***************************************************************
** 函數(shù)名稱: BluetoothControl
** 功能描述: 藍牙控制函數(shù)
手機發(fā)送內容
前：0x01 后：0x02
左：0x04 右：0x03
停止：0x07
功能鍵：（旋轉）
左旋轉:0x05 右旋轉：0x06
停轉：0x07
** 輸　入:
** 輸　出:
switch (x)
{
case 0x00 : BST_fCarAngle_P=BST_fCarAngle_D=BST_fCarSpeed_P=BST_fCarSpeed_I=0;
case 0x01 : BST_fBluetoothSpeed = 3000 ;chaoflag=1; break; //向前速度 250
case 0x02 : BST_fBluetoothSpeed = (-3000);chaoflag=1; break; //后退速度 -250
case 0x03 : BST_fBluetoothDirectionNew= -300; chaoflag=1;break ;//左旋
case 0x04 : BST_fBluetoothDirectionNew= 300; chaoflag=1;break ;//右旋轉
case 0x05 : BST_fBluetoothDirectionNew= driectionxco; chaoflag=1;break ;//左旋
case 0x06 : BST_fBluetoothDirectionNew= -driectionxco; chaoflag=1;break ;//右旋轉
case 0x07 : BST_fBluetoothDirectionL =0; BST_fBluetoothDirectionR = 0; BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0;fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0; break; //停
case 0x08 : BST_fBluetoothDirectionSL =0; BST_fBluetoothDirectionSR = 0; fchaoshengbo=0;BST_fBluetoothDirectionNew=0;chaoflag=0;break; //停旋轉
case 0x09 : BST_fBluetoothSpeed = 0 ; break;
case 0x0A : flagbt=1;break;
case 0x0B : flagbt=2;break;
case 0x0C : flagbt=3;break;
case 0x0D : flagbt=4;break;
default : BST_fBluetoothSpeed = 0;flagbt=0; BST_fBluetoothDirectionL=BST_fBluetoothDirectionR = 0;BST_fBluetoothDirectionSR=BST_fBluetoothDirectionSL=0;chaoflag=0;break;
}
***************************************************************/
int num = 0;
u8 startBit = 0;
int int9num =0;
void USART3_IRQHandler(void)
{
u8 uartvalue = 0;
if (USART_GetFlagStatus(USART3, USART_FLAG_ORE) != RESET)//注意！不能使用if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)來判斷
{
USART_ClearFlag(USART3, USART_FLAG_ORE); //讀SR其實就是清除標志
USART_ReceiveData(USART3);
}
if(USART_GetFlagStatus(USART3,USART_FLAG_RXNE)!=RESET)
{
USART_ClearITPendingBit(USART3, USART_IT_RXNE);
uartvalue = USART3->DR;
if(uartvalue == '
)
{
startBit = 1;
num = 0;
}
if(startBit == 1)
{
inputString[num] = uartvalue;
}
if (startBit == 1 && uartvalue == '#')
{
newLineReceived = 1;
startBit = 0;
int9num = num;
}
num++;
if(num >= 80)
{
num = 0;
startBit = 0;
newLineReceived = 0;
}
}
}
/**********************超聲波距離計算***************************/
void chaoshengbo(void)
{
if(chaoflag==0)
{
juli=TIM_GetCounter(TIM1)*5*34/200.0;
if(juli <= 4.00) //判斷若距離小于8cm，小車輸出向后PWM值。
{
fchaoshengbo= (-300);
}
else if(juli >= 5 & juli <= 8)
{
fchaoshengbo=500;
}
else
{
fchaoshengbo=0; //距離大于8cm ，超聲波PWM輸出為0
}
}
//寄生此上報數(shù)據(jù)
//增加自動上報 10ms進一次，故10*50ms = 500ms
//SendAutoUp();
}
/*計算上報的數(shù)據(jù)*/
void CalcUpData()
{
float ls, rs, sLence;
short s_Acc, s_Gyro;
if(g_autoup == 1)
{
//CLI();
ls = BST_fLeftMotorOut;
rs = BST_fRightMotorOut;
s_Acc = accel[1];
s_Gyro = gyro[0];
sLence = juli;
//SEI();
dac=(s_Acc/16384.0f)*9.8f;
dgy=((s_Gyro-128.1f)/131.0f);
ls=ls/3.91;
rs=rs/3.91;
memset(manydisplay, 0x00, 80);
memcpy(manydisplay, "$LV", 4);
memset(lspeed, 0x00, sizeof(lspeed));
memset(rspeed, 0x00, sizeof(rspeed));
memset(daccel, 0x00, sizeof(daccel));
memset(dgyro, 0x00, sizeof(dgyro));
memset(csb, 0x00, sizeof(csb));
memset(vi, 0x00, sizeof(vi));
if((ls <= 1000) && (ls >= -1000))
sprintf(lspeed,"%3.2f",ls);
else
{
//sprintf(str,"$AutoUpError!ls=%3.2f#",ls);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((rs <= 1000) && (rs >= -1000))
sprintf(rspeed,"%3.2f",rs);
else
{
//sprintf(str,"$AutoUpError!rs=%3.2f#",rs);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((dac > -20) && (dac < 20))
sprintf(daccel,"%3.2f",dac);
else
{
//sprintf(str,"$AutoUpError!dac=%3.2f#",dac);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((dgy > -3000) && (dgy < 3000))
sprintf(dgyro,"%3.2f",dgy);
else
{
//sprintf(str,"$AutoUpError!dgy=%3.2f#",dgy);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((sLence >= 0) && (sLence < 10000))
sprintf(csb,"%3.2f",sLence);
else
{
//sprintf(str,"$AutoUpError!juli=%3.2f#",juli);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
if((volt >= 0) && (volt < 20))
sprintf(vi,"%3.2f",volt);
else
{
//sprintf(str,"$AutoUpError!volt=%3.2f#",volt);
//UART3_Send_Char(str); //返回協(xié)議數(shù)據(jù)包
return;
}
strcat(manydisplay,lspeed);
strcat(manydisplay,",RV");
strcat(manydisplay,rspeed);
strcat(manydisplay,",AC");
strcat(manydisplay,daccel);
strcat(manydisplay,",GY");
strcat(manydisplay,dgyro);
strcat(manydisplay,",CSB");
strcat(manydisplay,csb);
strcat(manydisplay,",VT");
strcat(manydisplay,vi);
strcat(manydisplay,"#");
memset(updata, 0x00, 80);
memcpy(updata, manydisplay, 80);
}
}
/*
自動上報
*/
void SendAutoUp(void)
{
g_uptimes --;
if ((g_autoup == 1) && (g_uptimes == 0))
{
CalcUpData();
UART3_Send_Char(updata); //返回協(xié)議數(shù)據(jù)包
}
if(g_uptimes == 0)
g_uptimes = 5000;
}
int StringFind(const char *pSrc, const char *pDst)
{
int i, j;
for (i=0; pSrc[i]!='\0'; i++)
{
if(pSrc[i]!=pDst[0])
continue;
j = 0;
while(pDst[j]!='\0' && pSrc[i+j]!='\0')
{
j++;
if(pDst[j]!=pSrc[i+j])
break;
}
if(pDst[j]=='\0')
return i;
}
return -1;
}
void CarStateOut(void)
{
switch (g_newcarstate)
{
case enSTOP: //停止
{
BST_fBluetoothSpeed = 0;
fchaoshengbo=0;
BST_fBluetoothDirectionNew=0;
chaoflag=0;
} break;
case enRUN: //向前速度 800
{
BST_fBluetoothDirectionNew= 0;
//BST_fSpeedControlOutNew=0;
BST_fBluetoothSpeed = 800 ;
chaoflag=1;
}break;
case enLEFT://左轉
{
BST_fBluetoothDirectionNew= -300;
chaoflag=1;
}break;
case enRIGHT: //右轉
{
BST_fBluetoothDirectionNew= 300;
chaoflag=1;
}break;
case enBACK: //后退速度 -800
{
BST_fBluetoothDirectionNew= 0;
//BST_fSpeedControlOutNew=0;
BST_fBluetoothSpeed = (-800);
chaoflag=1;
}break;
case enTLEFT: //左旋
{
BST_fBluetoothDirectionNew = -driectionxco;
chaoflag=1;
}break;
case enTRIGHT: //右旋
{
BST_fBluetoothDirectionNew = driectionxco;
chaoflag=1;
}break;
default: BST_fBluetoothSpeed = 0; break; //停止
}
}
void ProtocolGetPID(void)
{
memset(piddisplay, 0x00, sizeof(piddisplay));
memcpy(piddisplay, "$AP", 4);
if(BST_fCarAngle_P >= 0 && BST_fCarAngle_P <= 100)
{
sprintf(charkp,"%3.2f",BST_fCarAngle_P);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarAngle_D >= 0 && BST_fCarAngle_D <= 100)
{
sprintf(charkd,"%3.2f",BST_fCarAngle_D);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarSpeed_P >= 0 && BST_fCarSpeed_P <= 100)
{
sprintf(charksp,"%3.2f",BST_fCarSpeed_P);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if(BST_fCarSpeed_I >= 0 && BST_fCarSpeed_I <= 100)
{
sprintf(charksi,"%3.2f",BST_fCarSpeed_I);
}
else
{
UART3_Send_Char("$GetPIDError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
strcat(piddisplay,charkp);
strcat(piddisplay,",AD");
strcat(piddisplay,charkd);
strcat(piddisplay,",VP");
strcat(piddisplay,charksp);
strcat(piddisplay,",VI");
strcat(piddisplay,charksi);
strcat(piddisplay,"#");
UART3_Send_Char(piddisplay); //返回協(xié)議數(shù)據(jù)包
}
void ProtocolCpyData(void)
{
memcpy(ProtocolString, inputString, num+1);
memset(inputString, 0x00, sizeof(inputString));
}
/***************************************************************************
串口協(xié)議數(shù)據(jù)解析
***************************************************************************/
void Protocol(void)
{
//USART_ITConfig(USART3, USART_IT_RXNE, DISABLE);//禁能接收中斷
//判斷數(shù)據(jù)包有效性
switch (ProtocolString[1])
{
case run_car: g_newcarstate = enRUN; UART3_Send_Char(returntemp);break;
case back_car: g_newcarstate = enBACK; UART3_Send_Char(returntemp);break;
case left_car: g_newcarstate = enLEFT; UART3_Send_Char(returntemp);break;
case right_car: g_newcarstate = enRIGHT; UART3_Send_Char(returntemp);break;
case stop_car: g_newcarstate = enSTOP; UART3_Send_Char(returntemp);break;
default: g_newcarstate = enSTOP; break;
}
/*防止數(shù)據(jù)丟包*/
if(strlen((const char *)ProtocolString)<21)
{
newLineReceived = 0;
memset(ProtocolString, 0x00, sizeof(ProtocolString));
//USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);//使能接收中斷
UART3_Send_Char("$ReceivePackError#"); //返回協(xié)議數(shù)據(jù)包
return;
}
if (ProtocolString[3] == '1') //左搖
{
g_newcarstate = enTLEFT;
UART3_Send_Char(returntemp); //返回協(xié)議數(shù)據(jù)包
}
if (ProtocolString[3] == '2') //右搖
{
g_newcarstate = enTRIGHT;
UART3_Send_Char(returntemp); //返回協(xié)議數(shù)據(jù)包
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

所有資料51hei提供下載:
源碼：

STM32平衡車.rar (697.14 KB, 下載次數(shù): 1306)

平衡小車原理分析.pdf (622.92 KB, 下載次數(shù): 1081)

STM32總算法流程圖.doc (19.03 KB, 下載次數(shù): 726)

ID:328014 · 發(fā)表于 2018-9-20 04:26

好資料，51黑有你更精彩!!!
講得比較通俗易懂，很好
補第2個無法下載的文件:

平衡小車原理分析.7z (545.26 KB, 下載次數(shù): 267)

ID:348697 · 發(fā)表于 2018-10-13 19:30

很不錯，正要學平衡車來著

ID:409435 · 發(fā)表于 2018-10-14 14:21

好資料

ID:400629 · 發(fā)表于 2018-10-15 10:41

內容豐富！資料很齊全，謝謝樓主的分享！

ID:315083 · 發(fā)表于 2019-1-18 21:30

頂上，謝謝樓主分享

ID:471436 · 發(fā)表于 2019-2-19 09:38

請問，您自己做出來過嗎？

ID:69072 · 發(fā)表于 2019-2-19 13:13

這種詳細解釋的優(yōu)秀帖非常好

ID:478263 · 發(fā)表于 2019-2-19 21:05

這個資料講的很細

ID:407977 · 發(fā)表于 2019-3-1 21:25

謝謝樓主分享

ID:407977 · 發(fā)表于 2019-3-1 21:25

謝謝樓主分享，小弟正在學習這個。

ID:394789 · 發(fā)表于 2019-3-6 15:54

好資料

ID:380985 · 發(fā)表于 2019-3-27 23:58

好資料。。。。

ID:496582 · 發(fā)表于 2019-4-25 07:03

謝謝樓主

ID:536258 · 發(fā)表于 2019-5-12 16:56

謝謝分享

ID:72134 · 發(fā)表于 2019-5-12 18:41

有空一定好好研究一下

ID:537504 · 發(fā)表于 2019-5-13 23:17

這個源碼應該用什么軟件打開呢？

ID:302850 · 發(fā)表于 2019-5-13 23:24

mark一些，等有金幣了下載！

ID:397503 · 發(fā)表于 2019-5-28 11:20

很棒的資料

ID:260463 · 發(fā)表于 2019-6-2 09:16

好資料，講的通俗易懂

ID:560891 · 發(fā)表于 2019-6-12 14:31

不錯，最近在學習，準備做個小車

ID:574674 · 發(fā)表于 2019-6-29 17:48

謝謝，分享

ID:356717 · 發(fā)表于 2019-7-3 08:37

感謝樓主

ID:486894 · 發(fā)表于 2019-7-4 19:37

下載了顯示資料不存在，這下好了，黑幣不夠了

ID:20345 · 發(fā)表于 2019-7-4 23:39

優(yōu)秀帖非常好！

ID:282095 · 發(fā)表于 2019-7-6 22:21

感謝分享

ID:82588 · 發(fā)表于 2019-7-10 22:36

分享是一種美德，謝謝

ID:585945 · 發(fā)表于 2019-7-18 17:26

感謝樓主！！資料十分有用

ID:278096 · 發(fā)表于 2019-7-19 08:18

第二個文件下載不了，已經(jīng)浪費我10個黑幣了

ID:511461 · 發(fā)表于 2019-7-20 17:16

為什么資料不存在

ID:560018 · 發(fā)表于 2019-7-22 15:20

感謝分享

ID:482599 · 發(fā)表于 2019-8-6 19:45

謝謝分享

ID:365823 · 發(fā)表于 2019-9-1 20:59

感謝分享

ID:623135 · 發(fā)表于 2019-10-13 02:59

很不錯。。。

ID:142891 · 發(fā)表于 2019-10-13 10:47

原理分析下不了。

ID:65480 · 發(fā)表于 2019-10-13 15:27

謝謝分享！學習一下。

ID:605571 · 發(fā)表于 2019-10-23 17:27

樓主你的平衡小車能急剎車嗎，如果能你是怎么克服慣性的

ID:629247 · 發(fā)表于 2019-10-25 09:38

厲害了，絕對的厲害！

ID:629540 · 發(fā)表于 2019-11-3 18:02

怎么下載鏈接不存在？

ID:516350 · 發(fā)表于 2019-11-4 14:14

剛好在做這個，非常感謝

帳號		自動登錄	找回密碼
密碼			立即注冊

超詳細雙輪平衡小車原理分析附STM32源碼

評分

瀏覽過的版塊

超詳細雙輪平衡小車原理分析 附STM32源碼

評分

瀏覽過的版塊

超詳細雙輪平衡小車原理分析附STM32源碼