Diff: main.cpp

Revision:

3:2ae6218973be

Parent:

2:38825726cb1b

Child:

4:3f80c0180e2f

--- a/main.cpp	Mon Jul 27 08:49:28 2020 +0000
+++ b/main.cpp	Wed Jul 29 01:24:51 2020 +0000
@@ -9,46 +9,79 @@
 #include <stdint.h>
 #include "platform/mbed_thread.h"
 #include "TextLCD.h"
-#define MIN_V 2.23
-#define MAX_V 3.3
-
-//bChangeのいろいろ
-I2C i2c_lcd(p9, p10);
-TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
-int b = 0;
-AnalogIn battery(p15);
-double test;
 
 Serial pc(USBTX, USBRX);
-// ポートp15を赤外線受信モジュールの接続先に指定
-ReceiverIR ir_rx(p15);
-RemoteIR::Format format;
-uint8_t buf[32];
-uint32_t bitcount;
-uint32_t code;
 
-//障害物回避の設定
-DigitalOut trig(p6);    // 超音波センサのtrigピンをp6に接続
-DigitalIn  echo(p7);    // 超音波センサのechoピンをp7に接続
-PwmOut   servo(p25);    // サーボコントロールピン(p25)
+/* マクロ定義、列挙型定義 */
+#define MIN_V 2.0
+#define MAX_V 2.67
+#define     LOW     0       // 
+#define     HIGH    1       // 
+#define     NORMAL   0       // フラグ値
+#define     FAST   1       // フラグ値
+#define     VERYFAST   2       // フラグ値
 
-enum Mode{
-    ADVANCE,
+enum MODE{
+    READY   = -1,
+    ADVANCE = 1,
     RIGHT,
     LEFT,
     BACK,
     STOP,
     LINE_TRACE,
     AVOIDANCE,
-    READY
+    SPEED,
 };
 
-Mode run;
-Mode mode;
+/* ピン配置 */
+ReceiverIR   ir(p5);        // リモコン操作
+DigitalOut  trig(p6);       // 超音波センサtrigger
+DigitalOut  echo(p7);       // 超音波センサecho
+DigitalIn   ss1(p8);        // ライントレースセンサ(左)
+DigitalIn   ss2(p9);        // ライントレースセンサ
+DigitalIn   ss3(p10);       // ライントレースセンサ
+DigitalIn   ss4(p11);       // ライントレースセンサ
+DigitalIn   ss5(p12);       // ライントレースセンサ(右)
+Serial      esp(p13, p14);  // Wi-Fiモジュール(tx, rx)
+AnalogIn    battery(p15);   // 電池残量読み取り(Max 3.3V)
+PwmOut      motorR2(p21);   // 右モーター後退
+PwmOut      motorR1(p22);   // 右モーター前進
+PwmOut      motorL2(p23);   // 左モーター後退
+PwmOut      motorL1(p24);   // 左モーター前進
+PwmOut      servo(p25);     // サーボ
+I2C         i2c_lcd(p28,p27);   // LCD(tx, rx)
 
-Timer timer;            // 距離計測用タイマー
+/* 変数宣言 */
+int mode;                   // 操作モード
+int run;                    // 走行状態
+int beforeMode;             // 前のモード
+int flag_sp = 0;             // スピード変化フラグ
+Timer viewTimer;            // スピ―ド変更時に3秒計測
+float motorSpeed[6] = {0.7, 0.8, 0.9, 0.8, 0.9, 1.0};
+                            // モーター速度設定(後半はライントレース用)
+Mutex  mutex;                       
+// ポートp15を赤外線受信モジュールの接続先に指定
+RemoteIR::Format format;
+uint8_t buf[32];
+uint32_t bitcount;
+uint32_t code;
+
+//bChangeのいろいろ
+TextLCD_I2C lcd(&i2c_lcd, (0x27 << 1), TextLCD::LCD16x2, TextLCD::HD44780);
+int b = 0;
+double test;
+int flag_b = 0;
+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};
 
 /* 障害物検知用の変数設定 */
+Timer timer;            // 距離計測用タイマ
+int DT;                 // 距離
 int SC;                 // 正面   
 int SL;                 // 左
 int SR;                 // 右
@@ -56,167 +89,93 @@
 int SRD;                // 右前
 int flag = 0;           // 障害物有無のフラグ
 const int limit = 20;   // 障害物の距離のリミット(単位：cm)
-int DT;                 // 距離
+int far;                // 最も遠い距離
 int houkou;             // 進行方向(1:前 2:左 3:右)
-int far;                // 最も遠い距離
 int i;                  // ループ変数
 
-void rimokon(void const *argument);
+
+
+/* プロトタイプ宣言 */
+void decodeIR(void const *argument);
+void motor(void const *argument);
+void changeSpeed();
 void avoidance(void const *argument);
-void motor(void const *argument);
+void trace(void const *argument);
 void watchsurrounding();
 int watch();
 void bChange(void const *argument);
-Thread thread1(rimokon, NULL, osPriorityRealtime);
-Thread thread2(motor, NULL, osPriorityHigh);
-Thread thread3(avoidance, NULL, osPriorityAboveNormal);
-Thread thread4(bChange, NULL, osPriorityBelowNormal);
+void display();
+void lcdBacklight(void const *argument);
+Thread thread1(decodeIR, NULL, osPriorityAboveNormal);
+Thread thread2(motor, NULL, osPriorityNormal);
+Thread thread3(avoidance, NULL, osPriorityLow);
+Thread thread4(trace, NULL, osPriorityLow);
+Thread thread5(bChange, NULL, osPriorityIdle);
+RtosTimer bTimer(lcdBacklight, osTimerPeriodic);
 
-void rimokon(void const *argument){  
+
+void decodeIR(void const *argument){  
     while(1){  
         // 受信待ち
-        if (ir_rx.getState() == ReceiverIR::Received) {
+        if (ir.getState() == ReceiverIR::Received) {
             // コード受信
-            bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+            bitcount = ir.getData(&format, buf, sizeof(buf) * 8);
             if(bitcount > 1){
                 // 受信成功
                 code=0;
                 for(int j=0;j<4;j++){
                     code+=(buf[j]<<(8*(3-j)));
                 }
-                pc.printf("%0x\r\n",code);
+                if(mode!=SPEED){
+                    beforeMode=mode;
+                }
+                
+                //pc.printf("%0x\r\n",code);
                 switch(code){
-                    case 0x40bf0ff0://入力切換
-                        pc.printf("入力切換\r\n");
-                        break;
-                    case 0x40bf12ed://電源
-                        pc.printf("電源\r\n");
-                        break;
-                    case 0x40bf7b84://地アナ
-                        pc.printf("地アナ\r\n");
-                        break;
-                    case 0x40bf7a85://地デジ
-                        pc.printf("地デジ\r\n");
-                        break;
-                    case 0x40bf7c83://BS
-                        pc.printf("BS\r\n");
-                        break;
-                    case 0x40bf7d82://CS
-                        pc.printf("CS\r\n");
-                        break;
-                    
-                    case 0x40bf01fe://1
-                        pc.printf("1\r\n");
-                        break;
-                    case 0x40bf02fd://2
-                        pc.printf("2\r\n");
-                        break;
-                    case 0x40bf03fc://3
-                        pc.printf("3\r\n");
-                        break;
-                    case 0x40bf04fb://4
-                        pc.printf("4\r\n");
-                        break;
-                    case 0x40bf05fa://5
-                        pc.printf("5\r\n");
-                        break;
-                    case 0x40bf06f9://6
-                        pc.printf("6\r\n");
-                        break;
-                    case 0x40bf07f8://7
-                        pc.printf("7\r\n");
-                        break;
-                    case 0x40bf08f7://8
-                        pc.printf("8\r\n");
-                        break;
-                    case 0x40bf09f6://9
-                        pc.printf("9\r\n");
-                        break;
-                    case 0x40bf0af5://10
-                        pc.printf("10\r\n");
-                        break;
-                    case 0x40bf0bf4://11
-                        pc.printf("11\r\n");
-                        break;
-                    case 0x40bf0cf3://12
-                        pc.printf("12\r\n");
-                        break;
-                    case 0x40bf1be4://チャンネル↑
-                        pc.printf("チャンネル↑\r\n");
-                        break;
-                    case 0x40bf1fe0://チャンネル↓
-                        pc.printf("チャンネル↓\r\n");
-                        break;
-                    case 0x40bf1ce3://画面表示
-                        pc.printf("画面表示\r\n");
-                        break;
-                    case 0x40bf10ef://消音
-                        pc.printf("消音\r\n");
-                        break;
                     case 0x40bf27d8://クイック
-                        pc.printf("クイック\r\n");
-                        break;    
-                    case 0x40bf1ae5://音量↑
-                        pc.printf("音量↑\r\n");
-                        break;
-                    case 0x40bf1ee1://音量↓
-                        pc.printf("音量↓\r\n");
+                        pc.printf("mode = SPEED\r\n");
+                        mode = SPEED;
                         break;
                     case 0x40be34cb://レグザリンク
-                        pc.printf("レグザリンク\r\n");
+                        pc.printf("mode = INE_TRACE\r\n");
+                        mode=LINE_TRACE;
+                        display();
                         break;
                     case 0x40bf6e91://番組表
-                        pc.printf("番組表\r\n");
-                        break;
-                    case 0x40bf3bc4://戻る
-                        pc.printf("戻る\r\n");
-                        break;
-                    case 0x40bf3cc3://終了
-                        pc.printf("終了\r\n");
+                        pc.printf("mode = AVOIDANCE\r\n");
+                        mode=AVOIDANCE;
+                        display();
                         break;
                     case 0x40bf3ec1://↑
-                        pc.printf("↑\r\n");
+                        pc.printf("mode = ADVANCE\r\n");
+                        mode = ADVANCE;
+                        run = ADVANCE;
+                        display();
                         break;
                     case 0x40bf3fc0://↓
-                        pc.printf("↓\r\n");
+                        pc.printf("mode = BACK\r\n");
+                        mode = BACK;
+                        run = BACK;
+                        display();
                         break;
                     case 0x40bf5fa0://←
-                        mode=LINE_TRACE;
-                        pc.printf("←\r\n");
+                        pc.printf("mode = LEFT\r\n");
+                        mode = LEFT;
+                        run = LEFT;
+                        display();
                         break;
                     case 0x40bf5ba4://→
-                        mode=AVOIDANCE;
-                        pc.printf("→\r\n");
+                        pc.printf("mode = RIGHT\r\n");
+                        mode = RIGHT;
+                        run = RIGHT;
+                        display();
                         break;
                     case 0x40bf3dc2://決定
-                        pc.printf("決定\r\n");
-                        break;
-                    case 0x40bf738c://青
-                        pc.printf("青\r\n");
-                        break;
-                    case 0x40bf748b://赤
-                        pc.printf("赤\r\n");
-                        break;
-                    case 0x40bf758a://緑
-                        pc.printf("緑\r\n");
-                        break;
-                    case 0x40bf7689://黄
-                        pc.printf("黄\r\n");
+                        pc.printf("mode = STOP\r\n");
+                        mode = STOP;
+                        run = STOP;
+                        display();
                         break;
-                    case 0x43bc14eb://dデータ
-                        pc.printf("dデータ\r\n");
-                        break;
-                    case 0x40bf50af://静止
-                        pc.printf("静止\r\n");
-                        break;
-                    case 0x40bf59a6://おまかせ映像
-                        pc.printf("おまかせ映像\r\n");
-                        break;
-                    case 0x40bf13ec://音声切換
-                        pc.printf("音声切換\r\n");
-                        break;
-                    
-                    
                     default:
                         ;
                 }
@@ -226,97 +185,192 @@
     }        
 }
 void motor(void const *argument){
-    while(1){
-        pc.printf("motor\r\n"); 
-        ThisThread::sleep_for(20);
+    while(true){
+        pc.printf("motor\r\n");
+        if(flag_sp >VERYFAST){
+            flag_sp -= 3;
+        }
+        switch(run){
+            case ADVANCE:
+                motorR1 = motorSpeed[flag_sp];
+                motorR2 = LOW;
+                motorL1 = motorSpeed[flag_sp];
+                motorL2 = LOW;
+                break;
+            case RIGHT:
+                motorR1 = LOW;
+                motorR2 = motorSpeed[flag_sp];
+                motorL1 = motorSpeed[flag_sp];
+                motorL2 = LOW;
+                break;
+            case LEFT:
+                motorR1 = motorSpeed[flag_sp];
+                motorR2 = LOW;
+                motorL1 = LOW;
+                motorL2 = motorSpeed[flag_sp];
+                break;
+            case BACK:
+                motorR1 = LOW;
+                motorR2 = motorSpeed[flag_sp];
+                motorL1 = LOW;
+                motorL2 = motorSpeed[flag_sp];
+                break;
+            case STOP:
+                motorR1 = LOW;
+                motorR2 = LOW;
+                motorL1 = LOW;
+                motorL2 = LOW;
+                break;
+        }
+        pc.printf("                motor\r\n");
+        ThisThread::sleep_for(50);
     }
 }
+void changeSpeed(){
+    if(flag_sp == VERYFAST){
+        flag_sp = NORMAL;
+        
+    }else{
+        flag_sp = flag_sp + 1;   
+    }  
+}
+void trace(void const *argument){
+    while(true){ 
+        if(mode==LINE_TRACE){        
+            pc.printf("line trace\r\n");
+            // センサー値読み取り
+            int sensor1 = ss1;
+            int sensor2 = ss2;
+            int sensor3 = ss3;
+            int sensor4 = ss4;
+            int sensor5 = ss5;
+        
+            int sensD = 0;
+            int sensorNum;
+        
+            if(sensor1 > 0) sensD |= 0x10;
+            if(sensor2 > 0) sensD |= 0x08;
+            if(sensor3 > 0) sensD |= 0x04;
+            if(sensor4 > 0) sensD |= 0x02;
+            if(sensor5 > 0) sensD |= 0x01;
+    
+            sensorNum = sensArray[sensD];
+    
+            switch(sensorNum){
+                case 1:
+                    run = ADVANCE;
+                    break;
+                case 2:
+                    run = RIGHT;
+                    break;
+                case 3:
+                    run = LEFT;
+                    break;
+                case 4:
+                    flag_sp += 3;
+                    run = RIGHT;
+                    break;
+                case 5:
+                    flag_sp += 3;
+                    run = LEFT;
+                    break;
+            }
+            pc.printf("                          line trace\r\n");
+            ThisThread::sleep_for(50);
+        }else{          
+            ThisThread::sleep_for(50);
+        }
+    }
+}
+
 /* 障害物回避走行 */
 void avoidance(void const *argument){
-    
-    while(1){
-    if(mode==AVOIDANCE){
-        pc.printf("avoidance\r\n"); 
-        if(flag == 0){                      // 障害物がない場合
-            run = ADVANCE;                  // 前進
-        }
-     /*   else{                               // 障害物がある場合                   
-            i = 0;
-            if(SC < 15){                    // 正面15cm以内に障害物が現れた場合
-                run = BACK;                 // 後退
-                while(watch() < limit){     // 正面20cm以内に障害物がある間
+    while(1){  
+        if(mode==AVOIDANCE){
+            pc.printf("avoidance\r\n"); 
+            if(flag == 0){                      // 障害物がない場合
+                run = ADVANCE;                  // 前進
+            }
+            /*else{                               // 障害物がある場合                   
+                i = 0;
+                if(SC < 15){                    // 正面15cm以内に障害物が現れた場合
+                    run = BACK;                 // 後退
+                    while(watch() < limit){     // 正面20cm以内に障害物がある間
                     
+                    }
+                    run = STOP;                 // 停止                 
                 }
-                run = STOP;                 // 停止                 
-            }
-            if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
-                run = LEFT;                 // 左折                   
-                while(i < 100){             // 進行方向確認
-                    if(watch() > limit){    
-                        i++;                
+                if(SC < limit && SLD < limit && SL < limit && SRD < limit && SR < limit){   // 全方向に障害物がある場合
+                    run = LEFT;                 // 左折                   
+                    while(i < 100){             // 進行方向確認
+                        if(watch() > limit){    
+                            i++;                
+                        }
+                        else{                   
+                            i = 0;              
+                        }
+                    }
+                    run = STOP;                 // 停止
+                }
+                else {                          // 全方向以外                          
+                    far = SC;                   // 正面を最も遠い距離に設定
+                    houkou = 1;                 // 進行方向を前に設定
+                    if(far < SLD || far < SL){  // 左または左前がより遠い場合
+                        if(SL < SLD){           // 左前が左より遠い場合
+                            far = SLD;          // 左前を最も遠い距離に設定
+                        }
+                        else{                   // 左が左前より遠い場合
+                            far = SL;           // 左を最も遠い距離に設定
+                        }
+                        houkou = 2;             // 進行方向を左に設定
                     }
-                    else{                   
-                        i = 0;              
+                    if(far < SRD || far < SR){  // 右または右前がより遠い場合
+                        if(SR < SRD){           // 右前が右より遠い場合
+                            far = SRD;          // 右前を最も遠い距離に設定
+                        }
+                        else{                   // 右が右前よりも遠い場合
+                            far = SR;           // 右を最も遠い距離に設定
+                        }
+                        houkou = 3;             // 進行方向を右に設定
+                    }
+                    switch(houkou){                         // 進行方向の場合分け
+                        case 1:                             // 前の場合
+                            run = ADVANCE;                  // 前進
+                            thread_sleep_for(100);          // 1秒待つ
+                            break;
+                        case 2:                             // 左の場合
+                            run = LEFT;                     // 左折
+                            while(i < 100){                 // 進行方向確認
+                                if(watch() > (far - 2)){    
+                                    i++;
+                                }
+                                else{
+                                    i = 0;
+                                }
+                            }
+                            run = STOP;                     // 停止
+                            break;
+                        case 3:                             // 右の場合
+                            run = RIGHT;                    // 右折
+                            while(i < 100){                 // 進行方向確認
+                                if(watch() > (far - 2)){
+                                    i++;
+                                }
+                                else{
+                                    i = 0;
+                                }
+                            }
+                            run = STOP;                     // 停止
+                            break;
                     }
                 }
-                run = STOP;                 // 停止
-            }
-            else {                          // 全方向以外                          
-                far = SC;                   // 正面を最も遠い距離に設定
-                houkou = 1;                 // 進行方向を前に設定
-                if(far < SLD || far < SL){  // 左または左前がより遠い場合
-                    if(SL < SLD){           // 左前が左より遠い場合
-                        far = SLD;          // 左前を最も遠い距離に設定
-                    }
-                    else{                   // 左が左前より遠い場合
-                        far = SL;           // 左を最も遠い距離に設定
-                    }
-                    houkou = 2;             // 進行方向を左に設定
-                }
-                if(far < SRD || far < SR){  // 右または右前がより遠い場合
-                    if(SR < SRD){           // 右前が右より遠い場合
-                        far = SRD;          // 右前を最も遠い距離に設定
-                    }
-                    else{                   // 右が右前よりも遠い場合
-                        far = SR;           // 右を最も遠い距離に設定
-                    }
-                    houkou = 3;             // 進行方向を右に設定
-                }
-                switch(houkou){                         // 進行方向の場合分け
-                    case 1:                             // 前の場合
-                        run = ADVANCE;                  // 前進
-                        thread_sleep_for(100);          // 1秒待つ
-                        break;
-                    case 2:                             // 左の場合
-                        run = LEFT;                     // 左折
-                        while(i < 100){                 // 進行方向確認
-                            if(watch() > (far - 2)){    
-                                i++;
-                            }
-                            else{
-                                i = 0;
-                            }
-                        }
-                        run = STOP;                     // 停止
-                        break;
-                    case 3:                             // 右の場合
-                        run = RIGHT;                    // 右折
-                        while(i < 100){                 // 進行方向確認
-                            if(watch() > (far - 2)){
-                                i++;
-                            }
-                            else{
-                                i = 0;
-                            }
-                        }
-                        run = STOP;                     // 停止
-                        break;
-                }
-            }
-        }*/
-        flag = 0;                   // フラグを0にセット
-        watchsurrounding();
-    }
+            }*/
+            flag = 0;                   // フラグを0にセット
+            watchsurrounding();
+            pc.printf("                avoidance\r\n");
+        }else{  
+            ThisThread::sleep_for(50);
+        }
     }   
 }
 int watch(){
@@ -337,7 +391,8 @@
         DT = 400;
     }
     timer.reset();                  // 距離計測タイマーリセット
-    ThisThread::sleep_for(30);      // 30ms待つ
+    //ThisThread::sleep_for(30);      // 30ms待つ
+    pc.printf("                watch\r\n");
     return DT;
 }
 
@@ -378,28 +433,176 @@
     if(SC < limit || SLD < limit || SL < limit || SRD < limit || SR < limit){ // 20cm以内に障害物を検知した場合
         flag = 1;                   // フラグに1をセット
     }
+    pc.printf("                watchsurrounding\r\n");
+}
+
+void defaultView(){
+    lcd.setAddress(0,0);
+    lcd.printf("Battery:");
+    lcd.setAddress(0,1);
+    lcd.printf("Mode:");
 }
 
+void display(){
+        mutex.lock();
+        lcd.setAddress(0,1);
+        switch(mode){
+            case ADVANCE:
+                //strcpy(DispMode,"Mode:Advance");
+                lcd.printf("Mode:Advance    ");
+                break;
+            case RIGHT:
+                //strcpy(DispMode,"Mode:Right");
+                lcd.printf("Mode:Right      ");
+                break;
+            case LEFT:
+                //strcpy(DispMode,"Mode:Left");
+                lcd.printf("Mode:Left       ");
+                break;
+            case BACK:
+                //strcpy(DispMode,"Mode:Back");
+                lcd.printf("Mode:Back       ");
+                break;
+            case STOP:
+                //strcpy(DispMode,"Mode:Stop");
+                lcd.printf("Mode:Stop       ");
+                break;
+            case READY:
+                //strcpy(DispMode,"Mode:Stop");
+                lcd.printf("Mode:Ready      ");
+                break;
+            case LINE_TRACE:
+                //strcpy(DispMode,"Mode:LineTrace");
+                lcd.printf("Mode:LineTrace  ");
+                break;
+            case AVOIDANCE:
+                //strcpy(DispMode,"Mode:Avoidance");
+                lcd.setAddress(0,1);
+                lcd.printf("Mode:Avoidance  ");
+                break;
+            case SPEED:
+                switch(flag_sp){
+                    case(NORMAL):
+                        lcd.printf("Speed:Normal    ");
+                        break;
+                    case(FAST):
+                        lcd.printf("Speed:Fast      ");
+                        break;
+                    case(VERYFAST):
+                        lcd.printf("Speed:VeryFast  ");
+                        break;
+                }
+                viewTimer.reset();
+                viewTimer.start();  
+                break;              
+        }
+        mutex.unlock(); 
+}
+
+void lcdBacklight(void const *argument){
+    if(flag_b == 1){
+        lcd.setBacklight(TextLCD::LightOn);
+    }else{
+        lcd.setBacklight(TextLCD::LightOff);
+    }
+    flag_b = !flag_b;
+}
 
 void bChange(void const *argument){
-    while(1){
-        //lcd.setBacklight(TextLCD::LightOn);
+    /*while(1){
+        pc.printf("                                                                              bChange1\r\n");
+        lcd.setBacklight(TextLCD::LightOn);
         test = battery.read()*MAX_V;
         b = (int)((test - MIN_V)/0.107 + 0.5)*10;
-        lcd.locate(0,0);
+        if(b < 0){
+            b = 0;
+        }else if(b > 100){
+            b = 100;
+        }
+        mutex.lock();
+        lcd.setAddress(0,0);
+        lcd.printf("Battery:%3d%%    ",b);
+        pc.printf("                                                                              bChange2\r\n");
+        mutex.unlock();
+    }*/
+    lcd.setBacklight(TextLCD::LightOn);
+    while(1){
+        pc.printf("                                                                              bChange1\r\n");
+        b = (int)((battery.read()* MAX_V - MIN_V)/0.067 + 0.5)*10;
+        if(b < 0){//すべての機能停止
+            lcd.setBacklight(TextLCD::LightOff);
+            bTimer.stop();
+            exit(1);
+        }else if(b > 100){
+            b = 100;
+        }
+        mutex.lock();
+        lcd.setAddress(0,0);
         lcd.printf("Battery:%3d%%",b);
-        pc.printf("bChange\r\n");
+        pc.printf("                                                                              bChange2\r\n");
+        mutex.unlock();
+        if(b <= 30){
+            if(flag_t == 0){
+                bTimer.start(500);
+                flag_t = 1;
+            }
+        }else{
+            if(flag_t == 1){
+                bTimer.stop();
+                lcd.setBacklight(TextLCD::LightOn);
+                flag_t = 0;
+            }
+        }
     }
+        
 }
 int main() {
-    
-    
+    mode = READY;
+    beforeMode = READY;
+    run = STOP;
+    flag_sp = NORMAL;
+    //defaultView();
     
     while(1){
-        /*if(button.read()==0){
-            rimokon();
-        }    */
-        pc.printf("main\r\n"); 
-        ThisThread::sleep_for(40);
+        pc.printf("                                    main1\r\n");
+        switch(mode){
+            /*case ADVANCE:
+                run = ADVANCE;
+                break;
+            case RIGHT:
+                run = RIGHT;
+                break;
+            case LEFT:
+                run = LEFT;
+                break;
+            case BACK:
+                run = BACK;
+                break;
+            case STOP:
+                run = STOP;
+                break;
+            case READY:
+                run = STOP;
+                break;*/
+            case SPEED:
+                changeSpeed();
+                display();  
+                mode = beforeMode;
+                break;            
+        }
+
+        if(viewTimer.read_ms()>=3000){
+            pc.printf("a");
+            viewTimer.stop();
+            //defaultView();
+            viewTimer.reset();
+            //flaga=0;
+        }
+        pc.printf("                                    main2\r\n");
+        if(viewTimer.read_ms()==0){
+            display();
+        }
+        pc.printf("                                    main3\r\n");
+        ThisThread::sleep_for(10);
     }
 }
\ No newline at end of file