Diff: main.cpp

Revision:

6:800a745c7f2e

Parent:

5:3fffb364744b

Child:

7:19ebd0851f49

--- a/main.cpp	Wed Jul 29 06:52:39 2020 +0000
+++ b/main.cpp	Thu Jul 30 02:18:48 2020 +0000
@@ -57,7 +57,7 @@
 int beforeMode;             // 前のモード
 int flag_sp = 0;             // スピード変化フラグ
 Timer viewTimer;            // スピ―ド変更時に3秒計測
-float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
+float motorSpeed[9] = {0.7, 0.8, 0.9, 0.75, 0.85, 0.95, 0.8, 0.9, 1.0};
                             // モーター速度設定(後半はライントレース用)
 Mutex  mutex;                       
 // ポートp15を赤外線受信モジュールの接続先に指定
@@ -74,10 +74,10 @@
 int flag_t = 0;
 
 // ライントレース
-int sensArray[32] = {0,0,0,0,0,0,0,0,   // ライントレースセンサパターン
-                    0,0,0,0,0,0,0,0,
-                    0,0,0,0,0,0,0,0,
-                    0,0,0,0,0,0,0,0};
+int sensArray[32] = {1,6,2,4,1,1,2,1,   // ライントレースセンサパターン
+                    3,1,1,1,3,1,1,1,
+                    7,1,1,1,1,1,1,1,
+                    5,1,1,1,1,1,1,1};
 
 /* 障害物検知用の変数設定 */
 Timer timer;            // 距離計測用タイマ
@@ -142,7 +142,7 @@
                         mode = beforeMode;
                         break;
                     case 0x40be34cb://レグザリンク
-                        pc.printf("mode = INE_TRACE\r\n");
+                        pc.printf("mode = LINE_TRACE\r\n");
                         mode=LINE_TRACE;
                         display();
                         break;
@@ -197,9 +197,6 @@
 void motor(void const *argument){
     while(true){
         pc.printf("motor\r\n");
-        if(flag_sp >VERYFAST){
-            flag_sp -= 3;
-        }
         switch(run){
             case ADVANCE:
                 motorR1 = motorSpeed[flag_sp];
@@ -232,13 +229,16 @@
                 motorL2 = LOW;
                 break;
         }
+        if(flag_sp > VERYFAST){
+            flag_sp -= 3 * (flag_sp / 3);
+        }
         pc.printf("                motor\r\n");
         ThisThread::sleep_for(30);
     }
 }
 void changeSpeed(){
-    if(flag_sp == VERYFAST){
-        flag_sp = NORMAL;
+    if(flag_sp%3 == 2){
+        flag_sp -= 2;
         
     }else{
         flag_sp = flag_sp + 1;   
@@ -247,14 +247,14 @@
 void trace(void const *argument){
     while(true){ 
         if(mode==LINE_TRACE){        
-            pc.printf("line trace\r\n");
+            //pc.printf("line trace\r\n");
             // センサー値読み取り
             int sensor1 = ss1;
             int sensor2 = ss2;
             int sensor3 = ss3;
             int sensor4 = ss4;
             int sensor5 = ss5;
-        
+            pc.printf("%d  %d  %d  %d  %d  \r\n",sensor1,sensor2,sensor3,sensor4,sensor5); 
             int sensD = 0;
             int sensorNum;
         
@@ -269,23 +269,38 @@
             switch(sensorNum){
                 case 1:
                     run = ADVANCE;
+                    pc.printf("ADVANCE");
                     break;
                 case 2:
                     run = RIGHT;
+                    pc.printf("RIGHT");
                     break;
                 case 3:
                     run = LEFT;
+                    pc.printf("LEFT");
                     break;
                 case 4:
-                    flag_sp += 3;
+                    flag_sp %= 3 + 3;
                     run = RIGHT;
+                    pc.printf("RIGHT");
                     break;
                 case 5:
-                    flag_sp += 3;
+                    flag_sp %= 3 + 3;
                     run = LEFT;
+                    pc.printf("LEFT");
+                    break;
+                case 6:
+                    flag_sp %= 3 + 6;
+                    run = RIGHT;
+                    pc.printf("RIGHT");
+                    break;
+                case 7:
+                    flag_sp %= 3 + 6;
+                    run = LEFT;
+                    pc.printf("LEFT");
                     break;
             }
-            pc.printf("                          line trace\r\n");
+            //pc.printf("                          line trace\r\n");
             ThisThread::sleep_for(30);
         }else{          
             ThisThread::sleep_for(1);
@@ -404,7 +419,7 @@
     }   
 }
 int watch(){
-    led1=0;
+    
     pc.printf("watch\r\n");
     trig = 0;
     ThisThread::sleep_for(5);           // 5ms待つ
@@ -429,6 +444,7 @@
     timer.reset();                  // 距離計測タイマーリセット
     //ThisThread::sleep_for(30);      // 30ms待つ
     pc.printf("私はDTである。%d\r\n",DT);
+    led1=0;
     return DT;
 }
 
@@ -550,8 +566,11 @@
         pc.printf("                                                                              bChange1\r\n");
         b = (int)(((battery.read() * 3.3 - MIN_V)/0.67)*10+0.5)*10;
         //pc.printf("%f",battery.read());
-        if(b < 0){//すべての機能停止(今はなし)
+        if(b <= 0){//すべての機能停止(今はなし)
             b = 0;
+            //lcd.setBacklight(TextLCD::LightOff);
+            //run = STOP;
+            //exit(1);//電池残量が5%未満の時、LCDを消灯し、モーターを停止し、プログラムを終了する。
         }else if(b > 50){
             b = 50;
         }