Revision 1:f382b4244a89, committed 2020-11-08

Comitter:

MCR_Xavier

Date:

Sun Nov 08 03:59:27 2020 +0000

Parent:

0:21afd0549d07

Commit message:

sample

Changed in this revision

--- a/main.cpp	Sat Apr 28 06:26:41 2018 +0000
+++ b/main.cpp	Sun Nov 08 03:59:27 2020 +0000
@@ -40,16 +40,17 @@
 int ErrFlg=0;                           //エラー判定フラグ
 int SensValBuf=0;                       //センサ値のバッファ
 int SensorR,SensorL;                    //ラインセンサ
+float VolumeVal;
 //----------ボリューム値取得-----------------
 int GetVol(void){
     int Val;
-    Val = Volume.read_u16()>>12;
+    Val = Volume.read_u16()>>7;
     return Val;
     }
 //----------センサ値更新-----------------
 void SensUp(void){
-    SensorR = LineR.read_u16()>>8;
-    SensorL = LineL.read_u16()>>8;
+    SensorR = LineR.read_u16()>>4;
+    SensorL = LineL.read_u16()>>4;
     }
 //------------モータ管理--------------------
 void MotorCtrl(void){
@@ -84,11 +85,23 @@
                     }    
 //------------ライントレース--------------------
 void LineTrace(void){
-    int SensVal,CommSpeed=100,PGain=1;
-    
+    long SensVal,CommSpeed=0,sensdelta=0,SensValBuf=0,SensI;
+    //float PGain=6.3,DGain=4;
+    //float PGain=0.18,DGain=1.07;
+    float PGain=0.75,DGain=0,IGain=0.1;
+    //DGain=0;
+    DGain=VolumeVal;
+    //PGain=VolumeVal;
+    //IGain=VolumeVal;
+    //CommSpeed=100*VolumeVal;
     SensVal = SensorR - SensorL;
-    MotorR = int(CommSpeed - (SensVal * PGain));
-    MotorL = int(CommSpeed + (SensVal * PGain));
+    sensdelta = SensVal - SensValBuf;
+    SensI = SensI + SensVal;
+    if(sensdelta < 0 ) sensdelta = sensdelta * (-1);
+    MotorR = int(CommSpeed - (SensVal * PGain) - (sensdelta * DGain) - (SensI * IGain));
+    //MotorL = int(1.19 * (CommSpeed + (SensVal * PGain) - (sensdelta * DGain)));
+    MotorL = int(CommSpeed + (SensVal * PGain) - (sensdelta * DGain) + (SensI * IGain));
+    SensValBuf = SensVal;
     }
 //-------------LED出力------------------
 void led_out(void){
@@ -108,7 +121,7 @@
     MotorCtrl();
     led_out();
     SensUp();
-    LineTrace();
+    //LineTrace();
             }
             
 //----------マイコン初期設定---------------
@@ -132,14 +145,23 @@
 //---------------メイン--------------------
 int main() {
     init();
-    int sensV;
+    float tmpval;
     wait(1);
     while(SW_IN);
     MotorDA = 0;
     while(1) {
-        if(!SW_IN) pc.printf("%5d  %5d  %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR);
-        ledval = sensV;      
-        wait(0.02);
+        //tmpval=GetVol();
+        //VolumeVal=tmpval/100;
+        if(!SW_IN) pc.printf("%5d  %5d  %5d %5d %f \r\n",SensorL,SensorR,MotorL,MotorR,VolumeVal);
+        //ledval = sensV;
+        MotorR  = 300;
+        MotorL  = 300; 
+        ledval ++;      
+        wait(0.5);
+        MotorR  = -300;
+        MotorL  = -300; 
+        ledval ++;      
+        wait(0.5);
 
     }
 }
\ No newline at end of file