標題: KEA128直立小車源碼 [打印本頁]
作者: zjj123    時間: 2018-6-30 18:10
標題: KEA128直立小車源碼
kea128的庫


單片機源程序如下:
  1. #include "headfile.h"

  3. /*系統(tǒng)變量初始化部分*/
  4. void PIDStructurePointerInit(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff ){ //PID初始值給定

  6.   PID_Balance->Kp = 8;
  7.   PID_Balance->Kd = 1.2;
  8.   PID_Balance->out = 0;
  9.   
  10.   PID_Speed->Kp = 10;
  11.   PID_Speed->Ki = 0.05;
  12.   PID_Speed->out   = 0;
  13.   
  14.   PID_Path->Kp = 2.0;
  15.   PID_Path->Kd = 0.0;
  16.   PID_Path->offset = 1.0;
  17.   PID_Path->out    = 0;
  18.   
  19.   PID_Diff->Kp = 15.0;
  20.   PID_Diff->Kd = 2.0;
  21.   PID_Diff->Ki = 0.0;
  22.   
  23.   PID_Diff->offset = 0.35;
  24.   PID_Diff->out    = 0;
  25.   
  26.   for(uint8 n=0; n<10; n++){
  27.    
  28.     PID_Balance->err_k[n] = 0;
  29.     PID_Speed->err_k[n]   = 0;
  30.     PID_Path->err_k[n]    = 0;
  31.     PID_Diff->err_k[n]    = 0;  
  32.     PID_Speed->out_k[n]   = 0;
  33.   };
  34.   
  35. };


  38. void SensorStructurePointerInit(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro, Angle_Type_Def *Angle, Speed_Type_Def *Speed, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){ //結構體指針初始化成員

  40.   
  41.   /*Gyro*/  
  42.   Gyro->X_Final=0;
  43.   Gyro->X_Offset=0;
  44.   Gyro->X_Value[0]=0;
  45.   Gyro->X_Value[1]=0;
  46.   Gyro->X_Value[2]=0;

  48.   Gyro->Z_Final=0;
  49.   Gyro->Z_Offset=0;
  50.   Gyro->Z_Value[0]=0;
  51.   Gyro->Z_Value[1]=0;
  52.   Gyro->Z_Value[2]=0;
  53.   
  54.   InductorA->LeftFinal=0;
  55.   InductorA->RightFinal=0;
  56.   InductorA->LeftValue[0]=0;
  57.   InductorA->LeftValue[1]=0;
  58.   InductorA->LeftValue[2]=0;
  59.   
  60.   InductorB->LeftFinal=0;
  61.   InductorB->RightFinal=0;
  62.   InductorB->LeftValue[0]=0;
  63.   InductorB->LeftValue[1]=0;
  64.   InductorB->LeftValue[2]=0;

  66.   //角度目標值  也就是平衡值  沒有轉換成角度值 但是是線性關系。
  67.   Angle->PiontValue = 2300;
  68.   
  69.   //速度目標值  沒有轉換成真實速度  與速度線性關系
  70.   Speed->PiontSpeed = 2000;

  72.   
  73.   Speed->PiontSpeedMax=5000;

  75.   Speed->PiontSpeedMin=1500;

  77.   
  78.   
  79.   
  80.   //此處應照車放在賽道上 電感在導線正上方 最大值 電感離導線最遠點為最小值
  81.   InductorA->LeftMax  = 1200;
  82.   InductorA->LeftMin  = 125;
  83.   InductorA->RightMax = 1200;
  84.   InductorA->RightMin = 125;
  85.   
  86.   
  87.   InductorB->LeftMax = 1200;
  88.   InductorB->LeftMin = 125;
  89.   InductorB->RightMax = 1200;
  90.   InductorB->RightMin = 125;
  91.   

  93.   
  94. };


  97. void CarBalanceSystemInit(Gyro_Type_Def *Gyro){//系統(tǒng)初始化

  99.   
  100.   //uart_init(UART1,9600);

  102.   gpio_init(H2,GPO,1);
  103.   
  104.   //關于ADC管腳接的東西 在headfile.h里有定義
  105.   adc_init(ADC0_SE0);       
  106.   adc_init(ADC0_SE1);       
  107.   adc_init(ADC0_SE2);       
  108.   adc_init(ADC0_SE3);       
  109.   adc_init(ADC0_SE4);       
  110.   adc_init(ADC0_SE5);       
  111.   adc_init(ADC0_SE6);
  112.   adc_init(ADC0_SE7);
  113.   
  114.   adc_init(ADC0_SE8);               
  115.   adc_init(ADC0_SE9);       
  116.   adc_init(ADC0_SE10);       
  117.   adc_init(ADC0_SE11);       
  118.   adc_init(ADC0_SE12);       
  119.   adc_init(ADC0_SE13);       
  120.   adc_init(ADC0_SE14);       
  121.   adc_init(ADC0_SE15);
  122.   

  124.   
  125.   
  126.   OLED_Init();
  127.   
  128.   OLED_ShowString(0,0,"SysInit Finish!");
  129.   OLED_ShowString(0,16,"Car balance2.0V");
  130.   OLED_Refresh_Gram();

  132.   // 使用數字陀螺儀  要初始化IIC
  133.   //IIC_init();
  134.   
  135.   
  136.   //此處陀螺儀中值獲取很重要   不過數字陀螺儀默認就是中值 無需獲取  
  137.   // 如果數字陀螺儀中值為0  那么  此處 直接賦值0即可
  138.   Gyro->Z_Offset = adc_once(ADC0_SE2, ADC_12bit);
  139.   Gyro->X_Offset = adc_once(ADC0_SE3, ADC_12bit);
  140.   
  141.   
  142.   systick_delay_ms(10);

  144.   //左電機
  145.   ftm_pwm_init(ftm2,ftm_ch2,13000,0);       
  146.   ftm_pwm_init(ftm2,ftm_ch3,13000,0);               
  147.   
  148.   //右電機  
  149.   ftm_pwm_init(ftm2,ftm_ch4,13000,0);               
  150.   ftm_pwm_init(ftm2,ftm_ch5,13000,0);       
  151.   

  153.   systick_delay_ms(10);  
  154.   
  155.   //ftm的計數引腳可查看KEA128_port_cfg.h文件內的FTM0_COUNT_PIN與FTM1_COUNT_PIN的宏定義得知
  156.   //一個ftm同一時間只能實現(xiàn)一種功能，如果用于測速就不能用于輸出PWM       
  157.   ///////////////////////////////////////////////////////////////////////////////////////////////
  158.   ftm_count_init(ftm0);   //對E0引腳輸入的脈沖進行計數    E0接編碼器LSB   
  159.   gpio_init(E1,GPI,0);    //用于判斷方向                  C5接編碼器DIR
  160.   port_pull(E1);          //IO上拉
  161.   
  162.   ftm_count_init(ftm1);   //對E7引腳輸入的脈沖進行計數    E7接編碼器LSB
  163.   gpio_init(H6,GPI,0);    //用于判斷方向                  H5接編碼器DIR
  164.   port_pull(H6);          //IO上拉
  165.   //////////////////////////////////////////////////////////////////////////////////////////////


  168.   //定時器中斷
  169.   pit_init(pit0,0x9000);//1.539ms
  170.   
  171.   systick_delay_ms(10);  
  172.   
  173.   

  175. };



  179. //絕對值函數   必須要取地址放入形參
  180. void AbsoluteValue(float *Value, float *Return){
  181.   
  182.   if( (*Value) <0) (*Return) = -1*(*Value);
  183.   else *Value = *Return;
  184. }

  186. /*原始數據獲取部分*/
  187. void GetAccelerationValue(Acc_Type_Def *Acc){ //獲取ACC原始數據
  188.   
  189.   Acc->Z_Value[0] = Acc->Z_Value[1];
  190.   Acc->Z_Value[1] = Acc->Z_Value[2];
  191.   Acc->Z_Value[2] = ACC_Z;//本句是獲取當前加速度  上兩句都是 上次值和上上次值

  193.   //利用多次值進行濾波  參數自行調整看效果
  194.   Acc->Z_Final = 0.33*Acc->Z_Value[2]+0.33*Acc->Z_Value[1]+0.33*Acc->Z_Value[0];
  195.   
  196. };

  198. void GetAngleSpeedValue(Gyro_Type_Def *Gyro){  //獲取Gyro原始數據
  199.   
  200.   Gyro->Z_Value[0] = Gyro->Z_Value[1];
  201.   Gyro->Z_Value[1] = Gyro->Z_Value[2];
  202.   Gyro->Z_Value[2] = GYRO_Z - Gyro->Z_Offset;//如果是數字陀螺儀請給Gyro->Z_Offset賦值  在初始化的時候
  203.   
  204.   Gyro->X_Value[0] = Gyro->X_Value[1]; //前前次
  205.   Gyro->X_Value[1] = Gyro->X_Value[2]; //前次
  206.   Gyro->X_Value[2] = GYRO_X - Gyro->X_Offset; //本次
  207.   
  208.   
  209. };


  212. void GetInductorValue(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB){ //獲取inductor原始數據
  213.   
  214.   
  215.   InductorA->LeftValue[0] = InductorA->LeftValue[1];
  216.   InductorA->LeftValue[1] = InductorA->LeftValue[2];
  217.   InductorA->LeftValue[2] = L1;//本句是獲取當前電感值  L1在haedfile.h文件中有定義
  218.   
  219.   InductorA->RightValue[0] = InductorA->RightValue[1];
  220.   InductorA->RightValue[1] = InductorA->RightValue[2];
  221.   InductorA->RightValue[2] = L2;//本句是獲取當前電感值  L2在haedfile.h文件中有定義
  222.   
  223.   InductorB->LeftValue[0] = InductorB->LeftValue[1];
  224.   InductorB->LeftValue[1] = InductorB->LeftValue[2];
  225.   InductorB->LeftValue[2] = L3;//本句是獲取當前電感值  L3在haedfile.h文件中有定義

  227.   InductorB->RightValue[0] = InductorB->RightValue[1];
  228.   InductorB->RightValue[1] = InductorB->RightValue[2];
  229.   InductorB->RightValue[2] = L4;//本句是獲取當前電感值  L4在haedfile.h文件中有定義
  230.   
  231.   
  232. };

  234. void GetSpeedValue(Speed_Type_Def *Speed){ //獲取Speed原始數據
  235.   
  236.   
  237.   for(uint8 n=0; n<15 ; n++){  //循環(huán)誤差保存
  238.    
  239.     //存儲前14次的值  
  240.     Speed->ActualLiftPulse[n] = Speed->ActualLiftPulse[n+1]; //0~14位賦值
  241.    
  242.     if( n==14 ){ //當前值 15次  最新值
  243.       Speed->ActualLiftPulse[n+1] = ftm_count_get(ftm1)*(-1); //本次 15位賦值  
  244.       
  245.       ftm_count_clean(ftm1);  //清除ftm1計數值  
  246.     }//此處一定要修改好 根據你們實際的硬件  對應好左右輪子  因為編碼器是對著安裝的  那么
  247.     //在車子往前走的時候  左右是相反的  所以要把其中一個負值編碼器變?yōu)檎?br />
  248.     //兩個電機存在同樣的問題  都是對著安裝的  
  249.   }
  250.   
  251.   
  252.   
  253.   for(uint8 n=0; n<15 ; n++){  //循環(huán)保存
  254.    
  255.     Speed->ActualRightPulse[n] = Speed->ActualRightPulse[n+1]; //0~14位賦值
  256.    
  257.     if( n==14 ){
  258.       Speed->ActualRightPulse[n+1] = ftm_count_get(ftm0);  //本次 15位賦值
  259.       ftm_count_clean(ftm0);  //清除ftm0計數值  
  260.     }
  261.    
  262.   }

  264.   if(gpio_get(H6)) //檢測 ftm1的轉向   H6  E1是針對左右編碼器的  要弄清楚 不要到時候改過以后
  265.     //左邊編碼器的方向  去控制右邊編碼器的 正負號   那就完蛋了
  266.     Speed->ActualLiftPulse[15] = Speed->ActualLiftPulse[15];
  267.   else               
  268.     Speed->ActualLiftPulse[15] = -Speed->ActualLiftPulse[15];            
  269.   
  270.   
  271.   if(gpio_get(E1))  //檢測 ftm0的轉向  
  272.     Speed->ActualRightPulse[15] = Speed->ActualRightPulse[15];
  273.   else               
  274.     Speed->ActualRightPulse[15] = -Speed->ActualRightPulse[15]; //這個負號取決于你車向前走時  編碼器值是否為正      
  275.   
  276. };


  279. void GetButtonStatus(Button_Type_Def *Button){ //Button原始數據獲取
  280.   
  281.   Button->Up[0] = Button->Up[1];
  282.   Button->Up[1] = Button->Up[2];
  283.   Button->Up[2] = Button->Up[3];
  284.   Button->Up[3] = Button->Up[4];
  285.   Button->Up[4] = UP;//第四次是最新的值  按鍵值  在haedfile.h文件中有定義
  286.   
  287.   Button->Down[0] = Button->Down[1];
  288.   Button->Down[1] = Button->Down[2];
  289.   Button->Down[2] = Button->Down[3];
  290.   Button->Down[3] = Button->Down[4];
  291.   Button->Down[4] = DOWN;//第四次是最新的值  按鍵值  在haedfile.h文件中有定義
  292.   
  293.   Button->Life[0] = Button->Life[1];
  294.   Button->Life[1] = Button->Life[2];
  295.   Button->Life[2] = Button->Life[3];
  296.   Button->Life[3] = Button->Life[4];
  297.   Button->Life[4] = LIFE;//第四次是最新的值  按鍵值  在haedfile.h文件中有定義
  298.   
  299.   Button->Right[0] = Button->Right[1];
  300.   Button->Right[1] = Button->Right[2];
  301.   Button->Right[2] = Button->Right[3];
  302.   Button->Right[3] = Button->Right[4];
  303.   Button->Right[4] = RIGHT;//第四次是最新的值  按鍵值  在haedfile.h文件中有定義
  304.   
  305.   Button->Enter[0] = Button->Enter[1];
  306.   Button->Enter[1] = Button->Enter[2];
  307.   Button->Enter[2] = Button->Enter[3];
  308.   Button->Enter[3] = Button->Enter[4];
  309.   Button->Enter[4] = ENTER;//第四次是最新的值  按鍵值  在haedfile.h文件中有定義

  311. };


  314. void GetSwitchStatus(Switch_Type_Def *Switch){ //開關原始數據獲取
  315.   
  316.   Switch->Bm1[0] = Switch->Bm1[1];
  317.   Switch->Bm1[1] = Switch->Bm1[2];
  318.   Switch->Bm1[2] = SW1; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義
  319.   
  320.   Switch->Bm2[0] = Switch->Bm2[1];
  321.   Switch->Bm2[1] = Switch->Bm2[2];
  322.   Switch->Bm2[2] = SW2; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義
  323.   
  324.   Switch->Bm3[0] = Switch->Bm3[1];
  325.   Switch->Bm3[1] = Switch->Bm3[2];
  326.   Switch->Bm3[2] = SW3; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義
  327.   
  328.   Switch->Bm4[0] = Switch->Bm4[1];
  329.   Switch->Bm4[1] = Switch->Bm4[2];
  330.   Switch->Bm4[2] = SW4; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義

  332.   Switch->Bm5[0] = Switch->Bm5[1];
  333.   Switch->Bm5[1] = Switch->Bm5[2];
  334.   Switch->Bm5[2] = SW5; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義
  335.   
  336.   Switch->Bm6[0] = Switch->Bm6[1];
  337.   Switch->Bm6[1] = Switch->Bm6[2];
  338.   Switch->Bm6[2] = SW6; //撥碼開關  第四次是最新的值  在haedfile.h文件中有定義
  339. };

  341. /*原始數據處理部分*/
  342. void GyroHighPassFilter(Gyro_Type_Def *Gyro){ //Gyro濾波

  344.   float K =0.58;//權值濾波  看情況調節(jié)
  345.   
  346.   Gyro->Z_Value[2] = K * Gyro->Z_Value[2] +  (1-K) * Gyro->Z_Value[1];
  347.   
  348.   Gyro->Z_Final = Gyro->Z_Value[2];


  351.   Gyro->X_Value[2] = K * Gyro->X_Value[2] +  (1-K) * Gyro->X_Value[1];
  352.   
  353.   Gyro->X_Final = Gyro->X_Value[2];
  354.   

  356.   //權值濾波 去除高頻干擾
  357. };


  360. void AngleHardwareFilter(Angle_Type_Def *Angle){ //角度濾波
  361.   //本函數是直接采集角度的  只有模擬陀螺儀和加速度計進行硬件融合直接解算角度值
  362.   
  363.   Angle->ActualValue[0] = Angle->ActualValue[1];
  364.   Angle->ActualValue[1] = Angle->ActualValue[2];
  365.   Angle->ActualValue[2] = ANGLE; //此處是獲取 最新角度值  
  366.   
  367.   //濾波 Angle->FinalValue為最終值
  368.   Angle->FinalValue = 0.6*Angle->ActualValue[2] + 0.2*Angle->ActualValue[1] + 0.2*Angle->ActualValue[0];
  369.   

  371. };

  373. void AngleSoftwareFilter(Acc_Type_Def *Acc, Gyro_Type_Def *Gyro , Angle_Type_Def *Angle){//角度濾波
  374. //本函數是適用于數字陀螺儀 通過軟件互補融合濾波解算角度  
  375.   
  376.   Acc->Z_Final = 0.9*Acc->Z_Value[2] + 0.05*Acc->Z_Value[1] + 0.05*Acc->Z_Value[0];//權值低通濾波
  377.   
  378.   Gyro->Z_Final = 0.92*Gyro->Z_Value[2] + 0.05*Gyro->Z_Value[1] + 0.03*Gyro->Z_Value[0];//權值低通濾波
  379.   
  380.   //加速度  角速度采集
  381.   
  382.   
  383.   //此處參數需要根據  清華方案里的方法去調節(jié)參數  看波形法
  384.   Angle->FinalValue = 0.92*(Angle->FinalValue + 0.05*Gyro->Z_Final) + 0.08*(Acc->Z_Final);//融合濾波

  386.   
  387. };


  390. void InductorSoftwareFilter(Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Angle_Type_Def *Angle, Switch_Type_Def *Switch){//電感值歸一濾波

  392.   /*權值濾波 警告無視  */
  393.   InductorA->LeftFinal = 0.8*InductorA->LeftValue[2] + 0.15*InductorA->LeftValue[1] + 0.05*InductorA->LeftValue[0];//-InductorA->LeftMin;
  394.   InductorA->RightFinal = 0.8*InductorA->RightValue[2] + 0.15*InductorA->RightValue[1] + 0.05*InductorA->RightValue[0];//-InductorA->RightMin;
  395.   
  396.   InductorB->LeftFinal = 0.8*InductorB->LeftValue[2] + 0.15*InductorB->LeftValue[1] + 0.05*InductorB->LeftValue[0];
  397.   InductorB->RightFinal = 0.8*InductorB->RightValue[2] + 0.15*InductorB->RightValue[1] + 0.05*InductorB->RightValue[0];




  402.   /*數據歸一化到0~1000*/
  403.   InductorA->LeftFinal  =  1000*( (float)(InductorA->LeftFinal-InductorA->LeftMin)/(InductorA->LeftMax-InductorA->LeftMin) );
  404.   InductorA->RightFinal  = 1000*( (float)(InductorA->RightFinal-InductorA->RightMin)/(InductorA->RightMax-InductorA->RightMin) );
  405.   
  406.   InductorB->LeftFinal  =  1000*( (float)(InductorB->LeftFinal-InductorB->LeftMin)/(InductorB->LeftMax-InductorB->LeftMin) );
  407.   InductorB->RightFinal  = 1000*( (float)(InductorB->RightFinal-InductorB->RightMin)/(InductorB->RightMax-InductorB->RightMin) );
  408.   
  409.   if(InductorA->LeftFinal>999)InductorA->LeftFinal=999;
  410.   if(InductorA->LeftFinal<  0)InductorA->LeftFinal=  0;
  411.   if(InductorA->RightFinal>999)InductorA->RightFinal=999;
  412.   if(InductorA->RightFinal< 0)InductorA->RightFinal=   0;
  413.   
  414.   
  415.   if(InductorB->LeftFinal>999)InductorB->LeftFinal=999;
  416.   if(InductorB->LeftFinal<  0)InductorB->LeftFinal=  0;
  417.   if(InductorB->RightFinal>999)InductorB->RightFinal=999;
  418.   if(InductorB->RightFinal< 0)InductorB->RightFinal=   0;
  419.   
  420.   //電感AB  A 是靠外側的電感  B是靠內側電感    RL就是左右
  421.   
  422.   InductorA->AndValues = InductorA->LeftFinal+InductorA->RightFinal;//電感和值
  423.   
  424.   InductorA->LeftDerivative = InductorA->LeftValue[2] - InductorA->LeftValue[0]; //左點感變化率
  425.   
  426.   InductorA->RightDerivative= InductorA->RightValue[2] - InductorA->RightValue[0];//右電感變化率
  427.   
  428.   InductorB->LeftDerivative = InductorB->LeftValue[2] - InductorB->LeftValue[0];
  429.   
  430. };


  433. void SpeedSoftwareFilter(Speed_Type_Def *Speed){   //速度低通濾波
  434.    
  435.   /*警告無視*/
  436.   Speed->ActualLiftPulse[15]=0.33*Speed->ActualLiftPulse[15] + 0.33*Speed->ActualLiftPulse[14] + 0.33*Speed->ActualLiftPulse[13];
  437.   Speed->ActualRightPulse[15]=0.33*Speed->ActualRightPulse[15] + 0.33*Speed->ActualRightPulse[14] + 0.33*Speed->ActualRightPulse[13];
  438.   // 6 2 2
  439.   
  440.   //實際速度 = R+L
  441.   Speed->ActualSpeed[0] = Speed->ActualSpeed[1];
  442.   Speed->ActualSpeed[1] = Speed->ActualSpeed[2];
  443.   Speed->ActualSpeed[2] =
  444.     (Speed->ActualRightPulse[0]+Speed->ActualLiftPulse[0])+
  445.     (Speed->ActualRightPulse[1]+Speed->ActualLiftPulse[1])+                    
  446.     (Speed->ActualRightPulse[2]+Speed->ActualLiftPulse[2])+
  447.     (Speed->ActualRightPulse[3]+Speed->ActualLiftPulse[3])+
  448.     (Speed->ActualRightPulse[4]+Speed->ActualLiftPulse[4])+
  449.     (Speed->ActualRightPulse[5]+Speed->ActualLiftPulse[5])+
  450.     (Speed->ActualRightPulse[6]+Speed->ActualLiftPulse[6])+
  451.     (Speed->ActualRightPulse[7]+Speed->ActualLiftPulse[7])+
  452.     (Speed->ActualRightPulse[8]+Speed->ActualLiftPulse[8])+
  453.     (Speed->ActualRightPulse[9]+Speed->ActualLiftPulse[9])+
  454.     (Speed->ActualRightPulse[10]+Speed->ActualLiftPulse[10])+
  455.     (Speed->ActualRightPulse[11]+Speed->ActualLiftPulse[11])+
  456.     (Speed->ActualRightPulse[12]+Speed->ActualLiftPulse[12])+
  457.     (Speed->ActualRightPulse[13]+Speed->ActualLiftPulse[13])+
  458.     (Speed->ActualRightPulse[14]+Speed->ActualLiftPulse[14])+
  459.     (Speed->ActualRightPulse[15]+Speed->ActualLiftPulse[15]);
  460.   
  461.   //濾波
  462.   Speed->ActualSpeed[2] = 0.85*Speed->ActualSpeed[2]+0.1*Speed->ActualSpeed[1]+0.05*Speed->ActualSpeed[0];
  463.   //7 2 1
  464.   
  465.   
  466.   //實際差速  = L-R
  467.   Speed->ActualDifferential[0] = Speed->ActualDifferential[1];
  468.   Speed->ActualDifferential[1] = Speed->ActualDifferential[2];
  469.   Speed->ActualDifferential[2] = Speed->ActualLiftPulse[15]-Speed->ActualRightPulse[15];
  470.   Speed->ActualDifferential[2] = 0.8*Speed->ActualDifferential[2]+0.15*Speed->ActualDifferential[1]+0.05*Speed->ActualDifferential[0];
  471.   //實際差速
  472.   
  473.   
  474.   Speed->ActualInstantaneous[0] = Speed->ActualInstantaneous[1];  
  475.   Speed->ActualInstantaneous[1] = Speed->ActualInstantaneous[2];
  476.   Speed->ActualInstantaneous[2] = (Speed->ActualLiftPulse[15]+Speed->ActualRightPulse[15] + (Speed->ActualLiftPulse[14]+Speed->ActualRightPulse[14])) ;
  477.   //瞬時速度計算
  478.   
  479.   Speed->ActualAcceleration = Speed->ActualInstantaneous[2] - Speed->ActualInstantaneous[0];
  480.   //加速度計算
  481.   
  482.   
  483.   
  484. };


  487. void ButtonLowPassFilter(Button_Type_Def *Button){ //按鍵低通濾波
  488.   
  489.   Button->Up_Final = Button->Up[0] || Button->Up[1] || Button->Up[2] || Button->Up[3] || Button->Up[4];
  490.   //有一個1  最終值就是1 全部為0 最終值才為0
  491.   if(Button->Up_Final==1) Button->Up_Final = 0;//取反    以1為有效值
  492.   else Button->Up_Final = 1;
  493.   
  494.   
  495.   Button->Down_Final = Button->Down[0] || Button->Down[1] || Button->Down[2] || Button->Down[3] || Button->Down[4];
  496.   //有一個1  最終值就是1 全部為0 最終值才為0
  497.   if(Button->Down_Final==1) Button->Down_Final = 0;//取反    以1為有效值
  498.   else Button->Down_Final = 1;
  499.   
  500.   
  501.   Button->Life_Final = Button->Life[0] || Button->Life[1] || Button->Life[2] || Button->Life[3] || Button->Life[4];
  502.   //有一個1  最終值就是1 全部為0 最終值才為0
  503.   if(Button->Life_Final==1) Button->Life_Final = 0;//取反    以1為有效值
  504.   else Button->Life_Final = 1;
  505.   
  506.   
  507.   Button->Right_Final = Button->Right[0] || Button->Right[1] || Button->Right[2] || Button->Right[3] || Button->Right[4];
  508.   //有一個1  最終值就是1 全部為0 最終值才為0
  509.   if(Button->Right_Final==1) Button->Right_Final = 0;//取反    以1為有效值
  510.   else Button->Right_Final = 1;
  511.   
  512.   
  513.   Button->Enter_Final = Button->Enter[0] || Button->Enter[1] || Button->Enter[2] || Button->Enter[3] || Button->Enter[4];
  514.   //有一個1  最終值就是1 全部為0 最終值才為0
  515.   if(Button->Enter_Final==1) Button->Enter_Final = 0;//取反    以1為有效值
  516.   else Button->Enter_Final = 1;
  517.   
  518. };

  520. void SwitchLowPassFilter(Switch_Type_Def *Switch){ //開關低通濾波
  521.   
  522.   Switch->Bm1_Final = Switch->Bm1[0] || Switch->Bm1[1] || Switch->Bm1[2];
  523.   if(Switch->Bm1_Final==1) Switch->Bm1_Final = 0;//取反    以1為有效值
  524.   else Switch->Bm1_Final = 1;
  525.   
  526.   Switch->Bm2_Final = Switch->Bm2[0] || Switch->Bm2[1] || Switch->Bm2[2];
  527.   if(Switch->Bm2_Final==1) Switch->Bm2_Final = 0;//取反    以1為有效值
  528.   else Switch->Bm2_Final = 1;
  529.   
  530.   Switch->Bm3_Final = Switch->Bm3[0] || Switch->Bm3[1] || Switch->Bm3[2];
  531.   if(Switch->Bm3_Final==1) Switch->Bm3_Final = 0;//取反    以1為有效值
  532.   else Switch->Bm3_Final = 1;
  533.   
  534.   Switch->Bm4_Final = Switch->Bm4[0] || Switch->Bm4[1] || Switch->Bm4[2];
  535.   if(Switch->Bm4_Final==1) Switch->Bm4_Final = 0;//取反    以1為有效值
  536.   else Switch->Bm4_Final = 1;
  537.   
  538.   Switch->Bm5_Final = Switch->Bm5[0] || Switch->Bm5[1] || Switch->Bm5[2];
  539.   if(Switch->Bm5_Final==1) Switch->Bm5_Final = 0;//取反    以1為有效值
  540.   else Switch->Bm5_Final = 1;
  541.   
  542.   Switch->Bm6_Final = Switch->Bm6[0] || Switch->Bm6[1] || Switch->Bm6[2];
  543.   if(Switch->Bm6_Final==1) Switch->Bm6_Final = 0;//取反    以1為有效值
  544.   else Switch->Bm6_Final = 1;
  545. };


  548. void OutOfControlSignal(Speed_Type_Def *Speed, PID_Type_Def *PID){   //速度失控信號檢測
  549.   
  550.   if( (Speed->ActualSpeed[2]-Speed->ActualSpeed[1]) > 400  &&  Speed->ActualSpeed[2]>10000){
  551.     //加速度過大 車輪空轉

  553.    
  554.     ftm_pwm_duty(ftm2,ftm_ch2,0);
  555.     ftm_pwm_duty(ftm2,ftm_ch3,0);
  556.     ftm_pwm_duty(ftm2,ftm_ch4,0);
  557.     ftm_pwm_duty(ftm2,ftm_ch5,0);
  558. //   
  559.    
  560.    
  561.     while(1){
  562.       
  563. //     led(LED0,LED_ON);  //根據你們的硬件設置報警燈
  564.       
  565.       systick_delay_ms(80);
  566.       
  567. //      led(LED0,LED_OFF);
  568.       
  569.       systick_delay_ms(80);
  570.    
  571.     };

  573.   }

  575. };



  579. /*被控制量處理部分*/
  580. void BalanceFeedbackControl(PID_Type_Def *PID, Angle_Type_Def *Angle, Gyro_Type_Def *Gyro, Ramp_Type_Def *Ramp){ //直立反饋控制
  581.   
  582.   PID->err = (Angle->PiontValue-Angle->FinalValue);//硬件
  583.   //偏差  =  角度目標-角度反饋值
  584.   
  585.   
  586.   PID->out =   1000 + PID->Kp * PID->err + PID->Kd * Gyro->Z_Final;
  587.   //1000是為了給一個PWM中點   Gyro->Z_Final就是 直立的角速度  也就是D控制  PD
  588.   
  589.   //一般情況下  調直立時   先給一個P參數  D給0   就能立起來   如果發(fā)現(xiàn)不能立瘋轉  
  590.   //多半是 pid方向相反    把(Angle->PiontValue-Angle->FinalValue)  里面調換下位置  即可
  591.   
  592.   

  594. };


  597. void DirectionFeedbackControl(PID_Type_Def *PID, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Gyro_Type_Def *Gyro, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp){ //方向反饋控制

  599.   
  600.   InductorA->RightFinal = PID->offset*InductorA->RightFinal;  
  601.   InductorA->LeftFinal = (2-PID->offset)*InductorA->LeftFinal;
  602.   //調整零偏
  603.   

  605.   //主要是在用 A電感
  606.   
  607.   PID->err = 10.0*(sqrt(1.0*InductorA->RightFinal)-sqrt(1.0*InductorA->LeftFinal) )/ (InductorA->RightFinal+InductorA->LeftFinal);
  608.   
  609.   
  610.   
  611.    
  612.   if( InductorA->AndValues < 25 && Ramp->Flag[0]!=1) {//丟線找回 且不在坡道
  613.    
  614.     PID->err += 0.55*PID->err_k[6]; //找回強度

  616.   }
  617.   else if( InductorA->AndValues < 50 && Ramp->Flag[0]!=1) {//丟線找回
  618.    
  619.     PID->err += 0.50*PID->err_k[6]; //找回強度
  620.   
  621.   }
  622.   else if( InductorA->AndValues < 75 && Ramp->Flag[0]!=1) {//丟線找回
  623.    
  624.     PID->err += 0.45*PID->err_k[7]; //找回強度
  625.   
  626.   }
  627.   else if( InductorA->AndValues < 100 && Ramp->Flag[0]!=1) {//丟線找回
  628.    
  629.     PID->err += 0.40*PID->err_k[7]; //找回強度
  630.   
  631.   }
  632.   else if( InductorA->AndValues < 125 && Ramp->Flag[0]!=1) {//丟線找回
  633.    
  634.     PID->err += 0.35*PID->err_k[8]; //找回強度
  635.   
  636.   }
  637.   else if( InductorA->AndValues< 150 && Ramp->Flag[0]!=1) {//丟線找回
  638.    
  639.     PID->err += 0.30*PID->err_k[8]; //找回強度
  640.   
  641.   }
  642.   else if( InductorA->AndValues < 175 && Ramp->Flag[0]!=1) {//丟線找回
  643.    
  644.     PID->err += 0.25*PID->err_k[9]; //找回強度9
  645.   
  646.   }
  647.   else if( InductorA->AndValues < 200 && Ramp->Flag[0]!=1) {//丟線找回200
  648.    
  649.     PID->err += 0.2*PID->err_k[9]; //找回強度9
  650.   
  651.   }
  652.   
  653.   if(InductorA->AndValues<150)  PID->err = 0;
  654.   


  657.   PID->out = PID->Kp * PID->err * Speed->ActualInstantaneous[2] + PID->Kd * Gyro->X_Final;
  658.   //轉向PID輸出計算 Gyro->X_Final; 是轉向角速度 也是D控制
  659.   
  660.   
  661.   Speed->PiontDifferential = (int16)(PID->out);  //給定差速環(huán)目標值
  662.   //把轉向環(huán)  計算出來的輸出值  給差速目標環(huán)
  663.   
  664.   for(uint8 n=0; n<9 ; n++){  //循環(huán)誤差保存
  665.    
  666.     PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
  667.    
  668.     if( n==8 )
  669.       PID->err_k[n+1] = PID->err;  //本次 9位賦值
  670.    
  671.   }
  672.   
  673. };


  676. void DifferentialFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Gyro_Type_Def *Gyro){//差速反饋控制
  677.      
  678.   //PID->offset只是一個比例分配  PID->offset=2     
  679.   
  680.   int16 SpeedDifferentialValue = PID->offset * Speed->ActualDifferential[2] + (1-PID->offset)*(-0.18*Gyro->X_Final);
  681.   //  得到差速值  是編碼器和   轉向角速度共同決定差速反饋值
  682.   
  683.   
  684.   //Speed->PiontDifferential = 0;
  685.   //差速目標-  差速實際值
  686.   PID->err = ( Speed->PiontDifferential -  SpeedDifferentialValue);

  688.   PID->integral += PID->err;
  689.   
  690.   //積分上下限
  691.   if(PID->integral>50000)PID->integral=50000;
  692.   
  693.   if(PID->integral<-50000)PID->integral=-50000;
  694.   
  695.   if(Speed->ActualSpeed[2]< 50)PID->integral = 0;
  696.   //低速積分清零
  697.   
  698.   PID->out =  PID->Kp * PID->err + PID->Kd * ( SpeedDifferentialValue + PID->err_k[0]);// + PID->Ki * PID->integral;
  699.   //PID計算
  700.   
  701.   for(uint8 n=0; n<9 ; n++){  //循環(huán)誤差保存
  702.    
  703.     PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
  704.    
  705.     if( n==8 )
  706.       PID->err_k[n+1] = PID->err;  //本次 9位賦值
  707.    
  708.   }
  709.   

  711.   PID->err_k[0] = SpeedDifferentialValue;//借用一下PID->err_k[0]咯  存入上次值 差速
  712.   

  714.   //輸出幅度限制   
  715.   if(PID->out>2000)PID->out=2000;//1800
  716.   if(PID->out<-2000)PID->out=-2000;


  719.   
  720. };

  722. void SpeedFeedbackControl(PID_Type_Def *PID, Speed_Type_Def *Speed, Ramp_Type_Def *Ramp, Switch_Type_Def *Switch){ //速度反饋控制
  723.   

  725.   PID->out_k[9] = PID->out;//保存成上一次輸出值
  726.   
  727.   PID->out_k[0] = 0;
  728.   
  729.   
  730.   
  731.   PID->err = Speed->ActualSpeed[2] - Speed->PiontSpeed;
  732.   
  733.   
  734.   PID->integral = PID->err_k[0]+PID->err_k[1]+PID->err_k[2]+PID->err_k[3]+PID->err_k[4]+
  735.   
  736.           PID->err_k[5]+PID->err_k[6]+PID->err_k[7]+PID->err_k[8]+PID->err_k[9]+PID->err;



  740.   PID->out = PID->Kp * PID->err;// + PID->Ki * PID->integral;
  741.   
  742.   PID->out *= 0.05; //縮小速度環(huán)輸出
  743.   
  744.   
  745.   if(PID->out >  4800) PID->out= 4800; //后退最低角度4200
  746.   
  747.   if(PID->out < -1800) PID->out= -1800;//前進最低角度-1800

  749.   

  751.   
  752.   for(uint8 n=0; n<9 ; n++){  //循環(huán)誤差保存
  753.    
  754.     PID->err_k[n] = PID->err_k[n+1]; //0~8位賦值
  755.    
  756.     if( n==8 )
  757.       PID->err_k[n+1] = PID->err;  //本次 9位賦值
  758.    
  759.   }
  760.   

  762. };

  764. void PWMFeedbackControl(PWM_Type_Def *PWM, PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Speed_Type_Def *Speed, Switch_Type_Def *Switch){ //PWM疊加反饋控制
  765.   
  766.   float DiffK=1;

  768.    
  769.   PID_Speed->out_k[0] += (PID_Speed->out - PID_Speed->out_k[9]) * 0.0625;//分段成1/16分增量
  770.   
  771.   //PID_Speed->out_k[9]是上次值 //PID_Speed->out是本次值  PID_Speed->out_k[0] 是平滑輸出增量值
  772.   //疊加的時候把   平滑輸出增量值 + 上次的值 得到16個周期內 每4ms周期應給的值
  773.   
  774.   PWM->PiontValue  = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
  775.   //把所有環(huán)輸出相加
  776.   
  777.   
  778.   PWM->Differential = (int16)(PID_Diff->out);
  779. //差速PWM
  780.   
  781.   
  782.   if(PWM->Differential>0){
  783.    
  784.     PWM->LiftValue = PWM->PiontValue + (DiffK) * PWM->Differential;
  785.   
  786.     PWM->RightValue = PWM->PiontValue - (2-DiffK) * PWM->Differential;      
  787.    
  788.   }
  789.   
  790.   if(PWM->Differential<0){ //差速小于零時  左邊給目標占空比+ 系數*差速
  791.    
  792.     PWM->LiftValue = PWM->PiontValue + (2-DiffK) * PWM->Differential;
  793.   
  794.     PWM->RightValue = PWM->PiontValue - (DiffK) * PWM->Differential;      
  795.    
  796.   }
  797.   
  798.   
  799.   if(PWM->Differential==0){  //差速=0時  左右給一樣的目標占空比
  800.    
  801.     PWM->LiftValue = PWM->PiontValue;
  802.    
  803.     PWM->RightValue = PWM->PiontValue;
  804.   }
  805. };

  807. void ButtonFunctionControl(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Angle_Type_Def *Angle, Speed_Type_Def *Speed, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Button_Type_Def *Button, uint8 *Page){ //按鍵功能實現(xiàn)
  808.   //OLED  按鍵 調節(jié)PID參數用的   
  809.   
  810.   
  811.   if(Button->Life_Final==1)if(*Page>0)(*Page)--;
  812.   
  813.   if(Button->Right_Final==1)if(*Page<135)(*Page)++;

  815.   //page0
  816.   if(*Page<=5){  //UP  kp
  817.    
  818.     if(Button->Up_Final==1 ){

  820.       if(PID_Balance->Kp<30) PID_Balance->Kp = PID_Balance->Kp+0.01;
  821.     }
  822.         
  823.     if(Button->Down_Final==1 ){

  825.       if(PID_Balance->Kp>0) PID_Balance->Kp = PID_Balance->Kp-0.01;
  826.     }

  828.   }

  830.   if(*Page<=10 && *Page>5){  //UP  kd
  831.    
  832.     if(Button->Up_Final==1){
  833.   
  834.       if(PID_Balance->Kd<30) PID_Balance->Kd = PID_Balance->Kd+0.01;
  835.     }
  836.         
  837.     if(Button->Down_Final==1){
  838.    
  839.       if(PID_Balance->Kd>0) PID_Balance->Kd = PID_Balance->Kd-0.01;
  840.     }
  841.    
  842.   }
  843.   
  844.   if(*Page<=15 && *Page>10){  // angle piont
  845.    
  846.     if(Button->Up_Final==1){
  847.   
  848.       if(Angle->PiontValue<2400) Angle->PiontValue = Angle->PiontValue+1;
  849.     }
  850.         
  851.     if(Button->Down_Final==1){
  852.    
  853.       if(Angle->PiontValue>1600) Angle->PiontValue = Angle->PiontValue-1;
  854.     }
  855.    
  856.   }
  857.    
  858.   if(*Page<=20 && *Page>15){  //speed kp
  859.    
  860.    
  861.     if(Button->Up_Final==1){
  862.    
  863.       if(PID_Speed->Kp<30)PID_Speed->Kp = PID_Speed->Kp+0.1;

  865.     }
  866.       
  867.    
  868.     if(Button->Down_Final==1){      
  869.   
  870.       if(PID_Speed->Kp>0)PID_Speed->Kp = PID_Speed->Kp-0.1;
  871.    
  872.     }
  873.    
  874.    
  875.   }

  877.   if(*Page<=25 && *Page>20){  //speed ki
  878.    
  879.    
  880.     if(Button->Up_Final==1){
  881.   
  882.       if(PID_Speed->Ki<10)PID_Speed->Ki = PID_Speed->Ki+0.001;
  883.     }
  884.         
  885.    
  886.     if(Button->Down_Final==1){  

  888.       if(PID_Speed->Ki>0)PID_Speed->Ki = PID_Speed->Ki-0.001;
  889.     }

  891.    
  892.   }
  893.   
  894.   if(*Page<=30 && *Page>25){  //speeed piont
  895.    
  896.     if(Button->Up_Final==1){
  897.   
  898.       if(Speed->PiontSpeed<10000)Speed->PiontSpeed = Speed->PiontSpeed+10;
  899.     }
  900.         
  901.     if(Button->Down_Final==1){

  903.       if(Speed->PiontSpeed>0)Speed->PiontSpeed = Speed->PiontSpeed-10;
  904.     }
  905.    
  906.    
  907.   }

  909.   
  910.   
  911.   
  912.   //page1
  913.   if(*Page<=35 && *Page>30){  //path kp
  914.    
  915.     if(Button->Up_Final==1){

  917.       if(PID_Path->Kp<30)PID_Path->Kp = PID_Path->Kp+0.01;
  918.     }
  919.         
  920.     if(Button->Down_Final==1 ){

  922.       if(PID_Path->Kp>0)PID_Path->Kp = PID_Path->Kp-0.01;
  923.     }

  925.    
  926.   }
  927.   
  928.   if(*Page<=40 && *Page>35){  //path kd
  929.    
  930.    
  931.     if(Button->Up_Final==1){

  933.       if(PID_Path->Kd<30)PID_Path->Kd = PID_Path->Kd+0.01;
  934.     }
  935.         
  936.     if(Button->Down_Final==1){
  937.    
  938.       if(PID_Path->Kd>0)PID_Path->Kd = PID_Path->Kd-0.01;
  939.     }

  941.   }
  942.   
  943.   if(*Page<=45 && *Page>40){  //path piont
  944.    
  945.    
  946.     if(Button->Up_Final==1){
  947.       
  948.       if(PID_Path->offset<2)PID_Path->offset = PID_Path->offset+0.001;
  949.    
  950.     }
  951.       
  952.     if(Button->Down_Final==1){
  953.    
  954.       if(PID_Path->offset>0)PID_Path->offset = PID_Path->offset-0.001;
  955.    
  956.     }
  957.    
  958.    
  959.   }
  960.   
  961.   if(*Page<=50 && *Page>45){  //diff kp
  962.    
  963.     if(Button->Up_Final==1){

  965.       if(PID_Diff->Kp<30)PID_Diff->Kp = PID_Diff->Kp+0.01;
  966.     }
  967.         
  968.     if(Button->Down_Final==1 ){

  970.       if(PID_Diff->Kp>0)PID_Diff->Kp = PID_Diff->Kp-0.01;
  971.     }

  973.    
  974.   }
  975.   
  976.   if(*Page<=55 && *Page>50){  //diff kd
  977.    
  978.     if(Button->Up_Final==1){

  980.       if(PID_Diff->Kd<30)PID_Diff->Kd = PID_Diff->Kd+0.01;
  981.     }
  982.         
  983.     if(Button->Down_Final==1 ){

  985.       if(PID_Diff->Kd>0)PID_Diff->Kd = PID_Diff->Kd-0.01;
  986.     }

  988.    
  989.   }
  990.    
  991.   if(*Page<=60 && *Page>55){  //diff kd
  992.    
  993.     if(Button->Up_Final==1){

  995.       if(PID_Diff->Ki<30)PID_Diff->Ki = PID_Diff->Ki+0.01;
  996.     }
  997.         
  998.     if(Button->Down_Final==1 ){

  1000.       if(PID_Diff->Ki>0)PID_Diff->Ki = PID_Diff->Ki-0.01;
  1001.     }

  1003.    
  1004.   }
  1005.   
  1006.   if(*Page<=65 && *Page>60){  //diff offset
  1007.    
  1008.     if(Button->Up_Final==1){

  1010.       if(PID_Diff->offset<1)PID_Diff->offset = PID_Diff->offset+0.001;
  1011.     }
  1012.         
  1013.     if(Button->Down_Final==1 ){

  1015.       if(PID_Diff->offset>0)PID_Diff->offset = PID_Diff->offset-0.001;
  1016.     }

  1018.    
  1019.   }
  1020.   
  1021.   

  1023.   
  1024.   
  1025.   //page2
  1026.   if(*Page<=70 && *Page>65){ //左電感最大值
  1027.    
  1028.     if(Button->Up_Final==1 && InductorA->LeftMax < 4000)InductorA->LeftMax ++;
  1029.         
  1030.    
  1031.     if(Button->Down_Final==1 && InductorA->LeftMax > 100)InductorA->LeftMax --;
  1032.    
  1033.    
  1034.   }
  1035.   
  1036.   if(*Page<=75 && *Page>70){ //左電感最小值
  1037.    
  1038.    
  1039.     if(Button->Up_Final==1 && InductorA->LeftMin < 4000)InductorA->LeftMin ++;
  1040.         
  1041.    
  1042.     if(Button->Down_Final==1 && InductorA->LeftMin > 100)InductorA->LeftMin --;
  1043.    
  1044.   }
  1045.   
  1046.   if(*Page<=80 && *Page>75){ //右電感最大值
  1047.    
  1048.     if(Button->Up_Final==1 && InductorA->RightMax < 4000)InductorA->RightMax ++;
  1049.         
  1050.    
  1051.     if(Button->Down_Final==1 && InductorA->RightMax > 100)InductorA->RightMax --;
  1052.    
  1053.    
  1054.   }
  1055.   
  1056.   if(*Page<=85 && *Page>80){ //右電感最小值
  1057.    
  1058.    
  1059.     if(Button->Up_Final==1 && InductorA->RightMin< 4000)InductorA->RightMin ++;
  1060.         
  1061.    
  1062.     if(Button->Down_Final==1 && InductorA->RightMin> 100)InductorA->RightMin --;
  1063.    
  1064.   }
  1065.   
  1066.   if(*Page<=90 && *Page>85){ //中間電感最大值
  1067.    
  1068.    
  1069.     if(Button->Up_Final==1 && InductorB->LeftMax< 4000)InductorB->LeftMax ++;
  1070.         
  1071.    
  1072.     if(Button->Down_Final==1 && InductorB->LeftMax> 100)InductorB->LeftMax --;
  1073.    
  1074.   }
  1075.   
  1076.   if(*Page<=95 && *Page>90){//中間電感最小值
  1077.    
  1078.    
  1079.     if(Button->Up_Final==1 && InductorB->LeftMin< 4000)InductorB->LeftMin ++;
  1080.         
  1081.    
  1082.     if(Button->Down_Final==1 && InductorB->LeftMin> 100)InductorB->LeftMin --;
  1083.    
  1084.   }
  1085.   
  1086.   if(*Page<=100 && *Page>95){//最大速度
  1087.    
  1088.    
  1089.     if(Button->Up_Final==1 && Speed->PiontSpeedMax<10000)Speed->PiontSpeedMax ++;
  1090.         
  1091.    
  1092.     if(Button->Down_Final==1 && Speed->PiontSpeedMax >0)Speed->PiontSpeedMax --;
  1093.    
  1094.   }
  1095.   
  1096.   if(*Page<=105 && *Page>100){//最小速度
  1097.   
  1098.     if(Button->Up_Final==1 && Speed->PiontSpeedMin < Speed->PiontSpeedMax)Speed->PiontSpeedMin ++;
  1099.         
  1100.    
  1101.     if(Button->Down_Final==1 && Speed->PiontSpeedMin > 0)Speed->PiontSpeedMin --;
  1102.    
  1103.    
  1104.   }

  1106.   
  1107. //  
  1108. //  //page3
  1109. //  if(*Page<=110 && *Page>105){//減速距離
  1110. //   
  1111. //    if(Button->Up_Final==1 && Speed->SlowDistance < 6000)Speed->SlowDistance ++;
  1112. //        
  1113. //   
  1114. //    if(Button->Down_Final==1 && Speed->SlowDistance > 0)Speed->SlowDistance --;
  1115. //  }
  1116. //  
  1117. //  if(*Page<=115 && *Page>110){//加速距離
  1118. //   
  1119. //    if(Button->Up_Final==1 && Speed->AccDistance < 6000)Speed->AccDistance ++;
  1120. //        
  1121. //   
  1122. //    if(Button->Down_Final==1 && Speed->AccDistance > 0)Speed->AccDistance --;
  1123. //  }
  1124. //  
  1125. //  if(*Page<=120 && *Page>115){//錯誤距離
  1126. //   
  1127. //    if(Button->Up_Final==1 && Speed->FaultDistance < 150000)Speed->FaultDistance+=100;
  1128. //        
  1129. //   
  1130. //    if(Button->Down_Final==1 && Speed->FaultDistance > 100)Speed->FaultDistance-=100;
  1131. //  }
  1132.   
  1133. };


  1136. /*輸出量處理部分*/
  1137. void PrintParameterToOLED(PID_Type_Def *PID_Balance, PID_Type_Def *PID_Speed, PID_Type_Def *PID_Path, PID_Type_Def *PID_Diff, Angle_Type_Def *Angle, Speed_Type_Def *Speed , PWM_Type_Def *PWM, Inductor_Type_Def *InductorA, Inductor_Type_Def *InductorB, Curve_Type_Def *Curve, uint8 Page){    //打印參數到OLED上

  1139.   uint8 mode[10];
  1140.   static uint8 Last=0;
  1141.   
  1142.   if(Page<=30){   
  1143.   
  1144.     if(Page<=5) mode[0] = 0;
  1145.     else mode[0] = 1;
  1146.    
  1147.     if(Page<=10 && Page>5) mode[1] =0;
  1148.     else mode[1] = 1;
  1149.    
  1150.     if(Page<=15 && Page>10) mode[2] =0;
  1151.     else mode[2] = 1;
  1152.    
  1153.     if(Page<=20 && Page>15) mode[3] =0;
  1154.     else mode[3] = 1;
  1155.    
  1156.     if(Page<=25 && Page>20) mode[4] =0;
  1157.     else mode[4] = 1;
  1158.    
  1159.     if(Page<=30 && Page>25) mode[5] =0;
  1160.     else mode[5] = 1;
  1161.    
  1162.     if( Last!=1 ){
  1163.       OLED_Fill(0,0,127,63,0);
  1164.       Last = 1;
  1165.     }
  1166.     OLED_ShowString(0,0,"UP P    D         ");
  1167.     OLED_ShowNum(36,0,(int)(PID_Balance->Kp*10),3,12,mode[0]);  
  1168.     OLED_ShowNum(72,0,(int)(PID_Balance->Kd*10),3,12,mode[1]);
  1169.     OLED_ShowString(0,12 ,"AnglePiont        ");
  1170.     OLED_ShowNum(96,12,(int)(Angle->PiontValue),4,12,mode[2]);  
  1171.    
  1172.     OLED_ShowString(0,24 ,"Speed P   I        ");
  1173.     OLED_ShowNum(56,24,(int)(PID_Speed->Kp*10),3,12,mode[3]);  
  1174.     OLED_ShowNum(96,24,(int)(PID_Speed->Ki*1000),3,12,mode[4]);  
  1175.     OLED_ShowString(0,36 ,"SpeedPiont        ");
  1176.     OLED_ShowNum(96,36,(int)(Speed->PiontSpeed),4,12,mode[5]);
  1177.       

  1179.     OLED_ShowNum(32,48,(int)(InductorA->LeftFinal),3,12,0);  
  1180.     OLED_ShowNum(56,48,(int)(InductorA->RightFinal),3,12,0);
  1181.     OLED_ShowNum(86,48,(int)(InductorB->LeftFinal),3,12,0);
  1182.   }  
  1183.   
  1184.   else if(Page>30 && Page<=65){
  1185.    
  1186.     if(Page<=35 && Page>30) mode[0] = 0;
  1187.     else mode[0] = 1;
  1188.    
  1189.     if(Page<=40 && Page>35) mode[1] =0;
  1190.     else mode[1] = 1;
  1191.    
  1192.     if(Page<=45 && Page>40) mode[2] =0;
  1193.     else mode[2] = 1;
  1194.    
  1195.     if(Page<=50 && Page>45) mode[3] =0;
  1196.     else mode[3] = 1;
  1197.    
  1198.     if(Page<=55 && Page>50) mode[4] =0;
  1199.     else mode[4] = 1;
  1200.    
  1201.     if(Page<=60 && Page>55) mode[5] =0;
  1202.     else mode[5] = 1;
  1203.    
  1204.     if(Page<=65 && Page>60) mode[6] =0;
  1205.     else mode[6] = 1;
  1206.    
  1207.    
  1208.    
  1209.     if( Last!=2 ){
  1210.       OLED_Fill(0,0,127,63,0);
  1211.       Last = 2;
  1212.     }
  1213.    
  1214.     OLED_ShowString(0,0,"Path P    D         ");
  1215.     OLED_ShowNum(48,0,(int)(PID_Path->Kp*10),3,12,mode[0]);  
  1216.     OLED_ShowNum(96,0,(int)(PID_Path->Kd*100),3,12,mode[1]);
  1217.     OLED_ShowString(0,12 ,"PathPiont        ");
  1218.     OLED_ShowNum(84,12,(int)(PID_Path->offset*100),3,12,mode[2]);  
  1219.    
  1220.       
  1221.     OLED_ShowString(0,24 ,"Diff P     D        ");
  1222.     OLED_ShowNum(56,24,(int)(PID_Diff->Kp*10),3,12,mode[3]);  
  1223.     OLED_ShowNum(96,24,(int)(PID_Diff->Kd*10),3,12,mode[4]);  
  1224.     OLED_ShowString(0,36 ,"Diff I     S        ");
  1225.     OLED_ShowNum(56,36,(int)(PID_Diff->Ki*10),3,12,mode[5]);
  1226.     OLED_ShowNum(96,36,(int)(PID_Diff->offset*100),3,12,mode[6]); //差速與陀螺儀融合比率
  1227.       
  1228.       

  1230.     OLED_ShowNum(32,48,(int)(InductorA->LeftFinal),3,12,0);  
  1231.     OLED_ShowNum(56,48,(int)(InductorA->RightFinal),3,12,0);
  1232.     OLED_ShowNum(86,48,(int)(InductorB->LeftFinal),3,12,0);
  1233.       
  1234.   }
  1235.   
  1236.   else if(Page>65 && Page<=105){
  1237.    
  1238.     if(Page<=70 && Page>65) mode[0] = 0;
  1239.     else mode[0] = 1;
  1240.    
  1241.     if(Page<=75 && Page>70) mode[1] =0;
  1242.     else mode[1] = 1;
  1243.    
  1244.     if(Page<=80 && Page>75) mode[2] =0;
  1245.     else mode[2] = 1;
  1246.    
  1247.     if(Page<=85 && Page>80) mode[3] =0;
  1248.     else mode[3] = 1;
  1249.    
  1250.     if(Page<=90 && Page>85) mode[4] =0;
  1251.     else mode[4] = 1;
  1252.    
  1253.     if(Page<=95 && Page>90) mode[5] =0;
  1254.     else mode[5] = 1;
  1255.    
  1256.     if(Page<=100 && Page>95) mode[6] =0;
  1257.     else mode[6] = 1;
  1258.    
  1259.     if(Page<=105 && Page>100) mode[7] =0;
  1260.     else mode[7] = 1;
  1261.    
  1262.    
  1263.     if( Last!=3 ){
  1264.       OLED_Fill(0,0,127,63,0);
  1265.       Last = 3;
  1266.     }
  1267.    
  1268.     OLED_ShowString(0,0   ,"LAMAX");
  1269.     OLED_ShowNum(38,0,(int)(InductorA->LeftMax),4,12,mode[0]);
  1270.     OLED_ShowString(68,0  ,"LAMIN");
  1271.     OLED_ShowNum(108,0,(int)(InductorA->LeftMin),3,12,mode[1]);
  1272.    
  1273.    
  1274.     OLED_ShowString(0,12  ,"RAMAX");
  1275.     OLED_ShowNum(38,12,(int)(InductorA->RightMax),4,12,mode[2]);
  1276.     OLED_ShowString(68,12 ,"RAMIN");
  1277.     OLED_ShowNum(108,12,(int)(InductorA->RightMin),3,12,mode[3]);
  1278.    
  1279.    
  1280.    
  1281.     OLED_ShowString(0,24  ,"ZBMAX");
  1282.     OLED_ShowNum(38,24,(int)(InductorB->LeftMax),4,12,mode[4]);
  1283.     OLED_ShowString(68,24 ,"ZBMIN");
  1284.     OLED_ShowNum(108,24,(int)(InductorB->LeftMin),3,12,mode[5]);
  1285.    
  1286.    
  1287.     OLED_ShowString(0,36 ,"SpeedMax");
  1288.     OLED_ShowNum(84,36,(uint32)(Speed->PiontSpeedMax),4,12,mode[6]);
  1289.    
  1290.     OLED_ShowString(0,48 ,"SpeedMin");
  1291.     OLED_ShowNum(84,48,(uint32)(Speed->PiontSpeedMin),4,12,mode[7]);
  1292.    
  1293.   }
  1294.   
  1295.   OLED_Refresh_Gram();

  1297. };



  1301. void MotorControlFlow(PWM_Type_Def *PWM, Ramp_Type_Def *Ramp){ //電機直接控制
  1302.   

  1304.   //如測試線序 在此處 對*PWM賦值 測試完畢再注釋

  1306.   //PWM->LiftValue = 1200;//左輪正傳  往前走
  1307.   //PWM->LiftValue = 1000;
  1308.   //PWM->LiftValue = 800;
  1309.   
  1310.   //PWM->RightValue = 1200;//右輪正傳 往前走
  1311.   //PWM->RightValue = 1000;
  1312.   //PWM->RightValue = 800;

  1314.   //左邊右邊都給 1000的時候 應該是不轉的   
  1315.   
  1316.   if(PWM->LiftValue>(PWMMAX))PWM->LiftValue = (PWMMAX);
  1317.   if(PWM->LiftValue<(PWMMIN))PWM->LiftValue = (PWMMIN);
  1318.   if(PWM->RightValue>(PWMMAX))PWM->RightValue = (PWMMAX);
  1319.   if(PWM->RightValue<(PWMMIN))PWM->RightValue = (PWMMIN);
  1320.   
  1321.   //此處必須先調整好  保證 PWM值是 >1000時  是前進
  1322.   //PWM值< 1000 是后退   而且 兩輪必須一致
  1323.   //能改硬件 就改驅動線  和 電機引線  例如
  1324.   //給同樣都是  1200  左邊電機慢速往前  右邊慢速往后 這時候 調換右電機線
  1325.   //如果出現(xiàn)如下情況  給1200  理論上是 正轉 20%占空比  但出現(xiàn)轉速很快 說明
  1326.   //驅動線序不對   可以在這里調換 通道  或者 直接改線序
  1327.   
  1328.   if(PWM->LiftValue>1000){ //邊輪前進  1000~2000
  1329.      ftm_pwm_duty(ftm2,ftm_ch2,0);
  1330.      ftm_pwm_duty(ftm2,ftm_ch3,PWM->LiftValue-1000);
  1331.   }
  1332.   else if(PWM->LiftValue<1000){ //左輪是后退  0~1000
  1333.      ftm_pwm_duty(ftm2,ftm_ch2,1000-PWM->LiftValue);
  1334.      ftm_pwm_duty(ftm2,ftm_ch3,0);   
  1335.   }  //如果發(fā)現(xiàn) 此處給定的是  控制 右輪轉動 則調換下硬件驅動線線序  注意 是兩根兩根的調換  
  1336.    

  1338.   if(PWM->RightValue>1000){ //右輪是前進  1000~2000
  1339.      ftm_pwm_duty(ftm2,ftm_ch4,0);
  1340.      ftm_pwm_duty(ftm2,ftm_ch5,PWM->RightValue-1000);
  1341.   }
  1342.   else if(PWM->RightValue<1000){ //右輪是后退  0~1000
  1343.      ftm_pwm_duty(ftm2,ftm_ch4,1000-PWM->RightValue);
  1344.      ftm_pwm_duty(ftm2,ftm_ch5,0);   
  1345.   }   //如果發(fā)現(xiàn) 此處給定的是  控制 左邊輪轉動 則調換下硬件驅動線線序  注意 是兩根兩根的調換   
  1346.   
  1347. };

  1349. uint8 timer3ms = 0, timer4_5ms=0,timer24ms=0 ,timer64ms=0;

  1351. int main(void){
  1352.   
  1353.   //由于本程序閉環(huán)較多  調試過程比較繁瑣  建議一開始的時候 屏蔽掉其他環(huán)
  1354.   //只調節(jié)最基礎的環(huán)節(jié)  平衡車主要就是對于兩個電機的控制  其中有一個函數
  1355.   //MotorControlFlow函數是直接控制電機的  這個函數需要調試好  其要求就是
  1356.   //占空比0~1000退后   1000~2000向前  通過調換驅動線  和 調換電機線  或者
  1357.   //調換程序中 ch2 3 4 5 的  23順序  45順序來  達到最終的效果  兩個電機
  1358.   //給1000以上是向前走   1000一下向后走  2000往前最快   0往后最快
  1359.   
  1360.   //當電機輸出調整好了以后  再調節(jié)編碼器反饋  我們在MotorControlFlow函數里
  1361.   //直接給  PWM->LiftValue = 1200;PWM->RightValue = 1200;  讓兩個電機固定占空比
  1362.   //轉動  這時候 通過IAR在線調試觀察  編碼器測速值 是否正確  是否對應左輪右輪
  1363.   //(左電機轉動對應左編碼器  右電機轉動對應右邊編碼器  不能錯！)
  1364.   //用手阻止一個電機觀察是否是那個電機的編碼器轉速下降  確保 編碼器的方向一致！
  1365.   
  1366.   //編碼器 和 電機調整好以后就方便了很多  接下來就是直立  前提是陀螺儀參數已經整定好
  1367.   //我們調試直立的時候  要把其他環(huán)都關閉 方法如下  函數PWMFeedbackControl 是把所有環(huán)輸出
  1368.   //整合到電機PWM上的函數 其中  PWM->PiontValue  = (int16)(PID_Balance->out)+ (int16)(PID_Speed->out_k[0] + PID_Speed->out_k[9]) ;
  1369.   //這句話是把  三個環(huán)的輸出 疊加在一起  我們只需要保留 直立環(huán)  這樣調節(jié)直立比較順手
  1370.   //其中還有一個 差速 也要給0  對直立就無影響   
  1371.   
  1372.   //當直立調節(jié)完畢后  速度環(huán)和直立的融合是比較難的  直立和速度是并環(huán) 需要精心調試，也可以
  1373.   //跳過速度環(huán)的調試  直接進入轉向環(huán)  與差速環(huán)    他們之間是串級PID的關系  
  1374.   
  1375.   //如果出現(xiàn)中斷  跑飛 或者  卡住在ADC死循環(huán)里  說明堆棧溢出  調整KEA128.ICF文件的  堆棧值 擴大  不超過12K即可
  1376.   


  1379.   
  1380.   get_clk();              //獲取時鐘頻率 必須執(zhí)行
  1381.   
  1382.   uint8 Page=0;
  1383.   
  1384.   PID_Type_Def  PID_Balance, PID_Speed, PID_Path, PID_Diff;
  1385.   
  1386.   RUN_Type_Def       RUN;
  1387.   Stop_Type_Def      Stop;
  1388.   Ramp_Type_Def      Ramp;
  1389.   Curve_Type_Def     Curve;
  1390.   Acc_Type_Def       Acc;
  1391.   Gyro_Type_Def      Gyro;
  1392.   Angle_Type_Def     Angle;
  1393.   PWM_Type_Def       PWM;
  1394.   Speed_Type_Def     Speed;
  1395.   Button_Type_Def    Button;
  1396.   Switch_Type_Def    Switch;
  1397.   Inductor_Type_Def  InductorA, InductorB;

  1399.   
  1400.   SensorStructurePointerInit( &Acc, &Gyro, &Angle, &Speed, &InductorA, &InductorB );//傳感器參數初始化
  1401.   PIDStructurePointerInit( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff );//PID參數初始化
  1402.   CarBalanceSystemInit( &Gyro ); //系統(tǒng)初始化  
  1403.   


  1406.   
  1407.   RUN.Flag[4] = 1;//運行出界標志位
  1408.   
  1409.   RUN.Flag[5] = 0;//啟動電機標志位
  1410.   
  1411.   Stop.Flag[0] = 0;//停止線標志位延時啟動標志位
  1412.   
  1413.   Stop.Flag[1] = 0;//停止標志位
  1414.   
  1415.   Ramp.Flag[0] = 0;//接近坡道標志位
  1416.   
  1417.   Ramp.Flag[1] = 0;//上坡道標志位
  1418.   
  1419.   Curve.Flag[1] = 0;//正差速標志位
  1420.   
  1421.   Curve.Flag[2] = 0;//反差速標志位
  1422.   
  1423.   Curve.Flag[3] = 0;//差速數目
  1424.   
  1425.   while(1){

  1427.     if( timer4_5ms  ){
  1428.       
  1429.         gpio_set(H2,1);//
  1430.       
  1431.       GetAccelerationValue( &Acc );  //加速度原始數據采集
  1432.       GetAngleSpeedValue( &Gyro );   //角速度原始數據采集
  1433.       GetSpeedValue( &Speed );       //獲取原始速度數據
  1434.       GetInductorValue( &InductorA, &InductorB );//電感電壓原始數據采集
  1435.       
  1436.       GyroHighPassFilter( &Gyro );  //角速度濾波處理
  1437.       AngleHardwareFilter( &Angle );//角度濾波處理
  1438.       SpeedSoftwareFilter( &Speed );//速度遞歸權值濾波
  1439.       InductorSoftwareFilter( &InductorA, &InductorB, &Angle, &Switch);//電感濾波處理

  1441.       BalanceFeedbackControl( &PID_Balance, &Angle, &Gyro, &Ramp );//平衡度反饋計算
  1442.       DirectionFeedbackControl( &PID_Path, &InductorA, &InductorB, &Gyro, &Speed, &Ramp );//路徑反饋計算
  1443.       DifferentialFeedbackControl( &PID_Diff, &Speed, &Gyro);//差速反饋計算

  1445.       PWMFeedbackControl( &PWM, &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Speed, &Switch );//PWM疊加 直立 方向 速度PWM
  1446.       MotorControlFlow( &PWM, &Ramp ); //電機PWM輸出控制
  1447.       
  1448.         gpio_set(H2,0);//
  1449.       
  1450.       timer4_5ms = 0;//清除時間標志位
  1451.     }
  1452.    
  1453.     //車子跑起來以后就不會執(zhí)行  因為開銷比較大 影響PID控制周期
  1454.     if( Speed.ActualSpeed[2]<100 && timer24ms && timer4_5ms != 1 ){
  1455.       
  1456.       GetButtonStatus( &Button ); //獲取按鍵狀態(tài)
  1457.       ButtonLowPassFilter( &Button );//判斷按鍵狀態(tài)
  1458.       
  1459.       
  1460.       ButtonFunctionControl( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Angle, &Speed, &InductorA, &InductorB, &Button, &Page );//實現(xiàn)按鍵狀態(tài)
  1461.       
  1462.       GetSwitchStatus( &Switch ); //獲取開關狀態(tài)
  1463.       SwitchLowPassFilter( &Switch );//判斷開關狀態(tài)

  1465.       timer24ms = 0;

  1467.     };
  1468.    
  1469.    
  1470.     if(timer64ms && timer4_5ms != 1 ){  
  1471.       
  1472.       if(Speed.ActualSpeed[2]<100)PrintParameterToOLED( &PID_Balance, &PID_Speed, &PID_Path, &PID_Diff, &Angle, &Speed, &PWM, &InductorA, &InductorB, &Curve, Page );
  1473.       //人機交互界面 高速運行不執(zhí)行
  1474.      
  1475.       SpeedFeedbackControl( &PID_Speed, &Speed, &Ramp, &Switch ); //速度環(huán)反饋控制
  1476.       

  1478.        timer64ms = 0;
  1479.     };
  1480.    
  1481.    
  1482.   }
  1483.   
  1484. }




  1489. uint8 count = 0;

  1491. void PIT_CH0_IRQHandler(void)
  1492. {

  1494.   if( count<84 ) count++;
  1495.   else count = 0;
  1496.    
  1497.   
  1498. //  if( count%2==0 ) timer3ms = 1;
  1499.   
  1500.   if( count%3==0 ) timer4_5ms = 1;
  1501.   
  1502.   if( count%16==0 ) timer24ms = 1;
  1503.   
  1504.   if( count%42==0 ) timer64ms=1;

  1506.   
  1507.   PIT_FlAG_CLR(pit0);//清除中斷標志位
  1508.   

  1510.   
  1511. }
復制代碼

所有資料51hei提供下載:
KEA128直立.zip (4.71 MB, 下載次數: 37)


作者: wjshw    時間: 2018-7-2 21:02
首先感謝分享！參考一下
作者: wjshw    時間: 2018-7-2 21:29
編譯出錯啊~提示缺少一些文件



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1