Revision 1:c5854b9559f9, committed 2019-06-09

Comitter:

MCR_Xavier

Date:

Sun Jun 09 07:46:22 2019 +0000

Parent:

0:21afd0549d07

Commit message:

sample

Changed in this revision

diff -r 21afd0549d07 -r c5854b9559f9 main.cpp
--- a/main.cpp	Sat Apr 28 06:26:41 2018 +0000
+++ b/main.cpp	Sun Jun 09 07:46:22 2019 +0000
@@ -23,6 +23,7 @@
 
 //割り込み定義
 Ticker flipper;             //汎用タイマー
+Ticker sensget;             //センサー用タイマー
 
 //プロトタイプ宣言
 void    init(void);         //マイコン初期設定
@@ -32,6 +33,7 @@
 void    SensUp(void);       //センサー値更新
 int     GetVol(void);       //ボリューム値取得
 
+
 //グローバル変数の宣言
 int timer1=0;                           //汎用タイマー
 int ledval=1;                           //LED出力値
@@ -40,6 +42,14 @@
 int ErrFlg=0;                           //エラー判定フラグ
 int SensValBuf=0;                       //センサ値のバッファ
 int SensorR,SensorL;                    //ラインセンサ
+int SensR[9],SensL[9];
+double PGainCLB=0;          //Pゲイン調整変数
+double IGainCLB=0;          //Iゲイン調整変数
+double DGainCLB=1.7;          //Dゲイン調整変数
+double SensVal_I,SensVal_IBuf=0;
+char turnFlg=0;
+char Stime;
+int CommSpeed;
 //----------ボリューム値取得-----------------
 int GetVol(void){
     int Val;
@@ -48,9 +58,34 @@
     }
 //----------センサ値更新-----------------
 void SensUp(void){
-    SensorR = LineR.read_u16()>>8;
-    SensorL = LineL.read_u16()>>8;
+int i,j,SensBuf;
+  for (i = 0; i < 9; i++) {
+    for (j = 9; j > i; j--) {
+      if (SensR[j-1] > SensR[j]) {
+        SensBuf = SensR[j-1];
+        SensR[j-1] = SensR[j];
+        SensR[j] = SensBuf;
+      }
+      if (SensL[j-1] > SensL[j]) {
+        SensBuf = SensL[j-1];
+        SensL[j-1] = SensL[j];
+        SensL[j] = SensBuf;
+      }
     }
+  }
+
+    SensorR = SensR[5];
+    SensorL = SensL[5];
+    }
+//----------センサー値の取得---------------
+void sensGet(){
+    if(Stime >= 9) {
+        Stime=0;
+        } 
+    SensR[Stime] = LineR.read_u16();
+    SensL[Stime] = LineL.read_u16();
+    Stime++;
+    }  
 //------------モータ管理--------------------
 void MotorCtrl(void){
     int RMotorVal,LMotorVal;
@@ -73,10 +108,11 @@
              RMotorVal = MotorR;
              MotorR_Rev = FW;}
         
-        if(LMotorVal >= 1000) MotorL = 1000;
-        if(RMotorVal >= 1000) MotorR = 1000;
+        if(LMotorVal >= 100) LMotorVal = 100;
+        if(RMotorVal >= 100) RMotorVal = 100;
         PWM_L.pulsewidth_us(LMotorVal);    //左PWM  (0~1000)
         PWM_R.pulsewidth_us(RMotorVal);    //右PWM  (0~1000)
+        //ledval = int(RMotorVal*0.016);
             }
         
         L_Dir = MotorL_Rev;             //右モータ回転方向（H：CW）
@@ -84,11 +120,29 @@
                     }    
 //------------ライントレース--------------------
 void LineTrace(void){
-    int SensVal,CommSpeed=100,PGain=1;
-    
+    int SensVal,SensVal_D;
+    double PGain=0.026,IGain=0.0002,DGain=0.7;
+    if(SensorR <= 5000 && SensorL <= 5000) CommSpeed=0;
     SensVal = SensorR - SensorL;
-    MotorR = int(CommSpeed - (SensVal * PGain));
-    MotorL = int(CommSpeed + (SensVal * PGain));
+    if(SensVal <= 0 && !turnFlg){//turnL
+     turnFlg=1;
+     SensVal_I = 0;
+        }
+     else if (SensVal >= 0 && turnFlg){//turnR
+      turnFlg=0;
+      SensVal_I = 0;
+         }
+    SensVal_I = SensVal_I + SensVal;
+    SensVal_IBuf = SensVal_IBuf + SensVal;
+    if(SensVal_I >= 100000000) SensVal_I = 100000000;
+    if(SensVal_I <= (-100000000)) SensVal_I = (-100000000);
+    SensVal_D = SensValBuf - SensVal;
+    SensValBuf = SensVal;
+    MotorR = int((CommSpeed - ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB))))/10;
+    MotorL = int(CommSpeed + ((SensVal * PGain)+(SensVal_I*IGain)-(SensVal_D*DGainCLB)))/10;
+    ledval = int(SensVal_IBuf / (-100000));
+    //MotorR = int(CommSpeed - (SensVal * PGainCLB));
+    //MotorL = int(CommSpeed + (SensVal * PGainCLB));
     }
 //-------------LED出力------------------
 void led_out(void){
@@ -104,11 +158,11 @@
 
 //----------タイマー割り込み---------------
 void flip(){
-    timer1++;
     MotorCtrl();
     led_out();
     SensUp();
     LineTrace();
+    timer1++;
             }
             
 //----------マイコン初期設定---------------
@@ -124,22 +178,37 @@
 
 //割り込み処理開始  
 flipper.attach_us(&flip,1000);              //汎用タイマー割り込み
+sensget.attach_us(&sensGet,100);            //センサー用タイマー割り込み 
 //PWM周期設定
-PWM_L.period(0.001);
-PWM_R.period(0.001);
+PWM_L.period(0.0001);
+PWM_R.period(0.0001);
     }
     
 //---------------メイン--------------------
 int main() {
     init();
     int sensV;
+
     wait(1);
-    while(SW_IN);
+    while(SW_IN)
+     {
+        pc.printf("%5d  %5d \r\n",SensorL,SensorR);
+        wait(0.1);
+        }
+    wait(1);
     MotorDA = 0;
+    timer1 = 0;
     while(1) {
-        if(!SW_IN) pc.printf("%5d  %5d  %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR);
-        ledval = sensV;      
-        wait(0.02);
+        if(!SW_IN)
+         {
+             DGainCLB = DGainCLB + 0.1;
+             pc.printf("%5d  %5d  %5d %5d %f %d \r\n",SensorL,SensorR,MotorL,MotorR,DGainCLB,turnFlg);
+             //ledval++;
+             }   
+             if(timer1 <= 1000)SensVal_IBuf = 0;
+             if(SensVal_I <= (-10000) || SensVal_I >= 10000 )CommSpeed = 200;
+              else CommSpeed=490;
+        wait(0.01);
 
     }
 }
\ No newline at end of file