倒立擺爲什麼有了角度環還必須有位置環呢？
答案是：和平衡小車類比可得，當只有角度環的時候，倒立擺是可以對擺錘偏向做出pwm的反應的，但是具體要反應合適程度到哪裏，還是應該由位置環來告訴系統，應該儘量穩定在什麼位置範圍。
角度環5ms採集一下 位置環25ms採集一次，電機控制是5ms控制一次。
這裏位置環25ms是這樣的出來的：首先嚐試5ms，但是不論參數怎麼調試都是反應劇烈，所以逐漸加大延長編碼器採集時間，到了30ms的時候根據相同參數比較反應滯後，座椅最後確定25ms。
角度環是根據角度的差值來給定pwm 需要的參數是角位移傳感器的採集的adc的值
int balance(float Angle)
{
float Bias; //傾角偏差
static float Last_Bias,D_Bias; //PID相關變量
int balance; //PWM返回值
Bias=Angle-ZHONGZHI; //求出平衡的角度中值 和機械相關
D_Bias=Bias-Last_Bias; //求出偏差的微分 進行微分控制（這次偏差和上次偏差）
balance=-Balance_KP*Bias-D_Bias*Balance_KD; //===計算傾角控制的電機PWM PD控制
Last_Bias=Bias; //保持上一次的偏差
return balance;
}
位置環是用編碼器測得的位置的差值作爲比例參數，用上一次位置的差值和這一次位置的差值作爲微分參數
int Position(int Encoder)
{
static float Position_PWM,Last_Position,Position_Bias,Position_Differential;
static float Position_Least;
Position_Least =Encoder-Position_Zero; //===
Position_Bias *=0.8;
Position_Bias += Position_Least*0.2; //===一階低通濾波器
Position_Differential=Position_Bias-Last_Position; //這次位置的差值減去上次位置的差值
Last_Position=Position_Bias;
Position_PWM=Position_Bias*Position_KP+Position_Differential*Position_KD; //===位置控制
return Position_PWM;
}
自我總結的調試指南：
注：紅色是目標位置
黑色是編碼器測的實際位置
角度環：p ：加參數加到放手可以直立一會 但是一碰就抖動很厲害
加上d後：加參數加到可以直立不倒 向兩邊或一邊加速，碰一下不抖
位置環：p：因爲電機總得脈衝是7200，然後倒立擺轉一圈編碼器根據正交解碼測得是1024個脈衝，在25ms中斷中大部分時候達到的最大脈衝差值是100左右，所以我們P參數確定的範圍暫且是1~50. 初始位置放在編碼器值是10000的時候（這裏編碼器CNT初始值給的10000），目標位置也是10000，這時候給p=1，我們會觀察到這樣的波形：
紅色是目標位置，黑色是編碼器測得實際位置，因爲有位置環p的存在，所以倒立擺在偏了一小段的時候會有恢復力，但這個恢復力超調太大了，導致倒立擺劇烈來回擺動，所以此時加微分參數D，來減小超調。
微分參數大小的判斷是因爲它乘的是這次位置差-上次位置差，這個差值在25ms中斷中最大位十左右，所以我們暫定微分參數是0~300。先增加到100，這時候波形如下：
黑色是實際編碼器測定位置變化，紅色是目標位置，這時候我們看可以看到超調量減小了並且處於穩態，但是卻遲遲達不到實際值，這說明有靜差，我們可以先不增加積分，再增大比例參數，直到出現這種波形：
黑色是實際波形，紅色是目標波形，這時候可以看出，已經達到目標位置值，並且超調量不是很大，這時候就在增大點微分參數D減小點超調量，使實際波形更接近於目標值就好，最好波形如下：
