Diff: main.cpp

Revision:

2:0806f6996e8c

Parent:

1:56c272cb4522

--- a/main.cpp	Sat Oct 02 05:14:25 2021 +0000
+++ b/main.cpp	Fri Nov 12 14:58:59 2021 +0000
@@ -1,191 +1,28 @@
 #include "mbed.h"
-#include "SO1602A.h"
-#define SW_ONE(__NUM__) (SWData[__NUM__][0]-SWData[__NUM__][1]==1)
-
-DigitalIn SW[7]= {PA_3,PA_12,PB_5,PA_11,PA_8,PF_1,PF_0};
+#include "conste.h"
 
-DigitalOut POWER[2]= {PA_1,PB_1};
-
-Serial UART(PA_9,PA_10, 9600);
-
-DigitalOut led(LED1);
-Serial telemetry(USBTX, USBRX, 115200);
-
-SO1602A oled(PB_7,PB_6);
+CONSTE conste(PA_0,PA_1, 9600);
 
-Timer emoTimer;
-Timer restTimer;
-Timer newsTimer[3];
-Timer mainTimer;
-int msTimer=0;
+DigitalOut led(LED2);
+Serial telemetry(USBTX, USBRX, 9600);
 
-int newsStatus[3] = {0};
-
-void uartRX()
-{
-    char data = UART.getc();
-    newsStatus[(data>>2)&0b11]=(data&0b11);
-    newsTimer[(data>>2)&0b11].reset();
-}
+/* 操縦権 0…停止 1…手動 1…自動 */
+int operate = 0;
+/* 座標移動追従出来ているか(現在座標と目標座標の大きさは小さいか) */
+int isFollow = 0;
 
 int main()
 {
-
-    POWER[0]=POWER[1]=1;
-    for(int i=0; i<7; i++) SW[i].mode(PullDown);
-
-    mainTimer.reset();
-    mainTimer.start();
-    restTimer.start();
-    emoTimer.start();
-    for(int i=0; i<3; i++) newsTimer[i].start();
-
-    oled.init();
-    oled.setDispFlag(1,0,0);
-//    oled.locate(2,0);
-//    oled.printf("Stop display");
-
-    UART.attach(uartRX, Serial::RxIrq);
+    // 識別番号 0…のあ 1…ねぷちゅーん 2…とりとん 4…リモコン1 6…リモコン2
+    conste.identificationNum = 0;
 
     while(1) {
-
-//        telemetry.printf("%d\n\r", (SW[6]==0?0:1));
-
-        /* SW入力のワンタイム化 */
-        static bool SWData[7][2] = {0};
-        for(int i=0; i<7; i++) {
-            SWData[i][1]=SWData[i][0];
-            SWData[i][0]=SW[i];
-        }
-
-        /* 入力制限モード */
-        static bool restFlag = 0;
-        if(SW_ONE(0)) restFlag=!restFlag;
-
-        /* メインタイマー制御 0-pause 1-start */
-        static bool timerMode=0;
-        if(!SW[1]) timerMode=0;
-        if(!restFlag) {
-            if(SW_ONE(2)) msTimer+=1000*60;
-            if(SW_ONE(3)) msTimer+=1000*10;
-            if(SW_ONE(4)) msTimer+=1000*1;
-
-            if(SW[1]&&SW_ONE(5)) {
-                timerMode=0;
-                msTimer=0;
-            }
-            if(SW[1]&&SW_ONE(6)) timerMode=!timerMode;
-        }
-
-        /* 時間の更新 */
-        static int temp=mainTimer.read_ms();
-        if(timerMode) msTimer+=mainTimer.read_ms()-temp;
-        temp=mainTimer.read_ms();
-        if(msTimer>120e3) {
-            timerMode=0;
-            msTimer=120e3;
-        }
-
-        /* UART送受信関連 */
-        uint8_t sendData[4] = {0};
-        uint8_t checkSum = 0;
-
-        //チェックサム計算
-        for (int i = 0; i < 7; i++)
-            checkSum += msTimer / (int)pow(10.0, (double)i) % 10;
-
-        sendData[3] = 0x80;
-        sendData[3] |= timerMode << 6;
-        sendData[3] |= checkSum & 0x3F;
-
-        //1バイト区切る
-        for(int i = 0; i < 3; i++)
-            sendData[i] = 0x7F & msTimer >> 7 * (2-i);
-
-        for(int i = 0; i < 4; i++){
-            UART.putc(sendData[i]);
-        telemetry.putc(sendData[i]);
-            }
-//        telemetry.printf("%d \n\r",msTimer);
-
-        for(int i=0; i<3; i++)
-            if(newsTimer[i].read_ms()>500)
-                newsStatus[i]=0;
-
-
-
-        /* LCD描画 */
+        // ロボットの状態 1…モード異常 2…座標追従異常 3…正常
+        conste.status = (operate == 1) << 1 | (isFollow == 1);
+        
+        /* 非常停止の時，モーターを止める */
+        if(conste.isEMO) /*MovMotor[i] = 0*/;
 
-//        oled.locate(5,0);
-//        oled.printf("%1d:%02d.%d ", msTimer/60000,msTimer/1000%60,msTimer/100%10);
-//
-//        oled.locate(0,0);
-//        int statious=!SW[1];
-//        if(restFlag&&(SW[2]+SW[3]+SW[4]+SW[5]+SW[6])) restTimer.reset();
-//        if(emoTimer.read()<1&&emoTimer.read_ms()%200<100) statious=0;
-//        if(statious) oled.printf("EMO  ");
-//        else oled.printf("     ");
-//
-//        oled.locate(12,0);
-//        statious=restFlag;
-//        if(!SW[1]&&(SW[5]+SW[6])) emoTimer.reset();
-//        if(restTimer.read()<1&&restTimer.read_ms()%200<100) statious=0;
-//        if(statious) oled.printf("REST");
-//        else oled.printf("    ");
-
-
-//        for(int i=0; i<3; i++) {
-//            oled.locate(i*6,1);
-//            if(newsStatus[i]==0) oled.printf("Lost");
-//            if(newsStatus[i]==1) oled.printf("None");
-//            if(newsStatus[i]==2) oled.printf("Yoki");
-//        }
-//        for(int i=0; i<2; i++) {
-//            oled.locate(i*6+4,1);
-//            oled.printf("  ");
-//        }
-
-        char printLCD[32] = {0};
-
-        int statious=!SW[1];
-        if(restFlag&&(SW[2]+SW[3]+SW[4]+SW[5]+SW[6])) restTimer.reset();
-        if(emoTimer.read()<1&&emoTimer.read_ms()%200<100) statious=0;
-        if(statious) strcat(printLCD,"EMO  ");
-        else strcat(printLCD,"     ");
-        
-        char tmp[17] = {0};
-        sprintf(tmp,"%1d:%02d.%d ", msTimer/60000,msTimer/1000%60,msTimer/100%10);
-        strcat(printLCD,tmp);
-        
-        statious=restFlag;
-        if(!SW[1]&&(SW[5]+SW[6])) emoTimer.reset();
-        if(restTimer.read()<1&&restTimer.read_ms()%200<100) statious=0;
-        if(statious) strcat(printLCD,"REST");
-        else strcat(printLCD,"    ");
-        
-        const char stat[3][5] = {"Lost","None","Yoki"};
-        oled.locate(0,1);
-        sprintf(tmp,"%s  %s  %s",stat[newsStatus[0]],stat[newsStatus[1]],stat[newsStatus[2]]);
-        strcat(printLCD,tmp);
-        
-        oled.locate(0,0);
-        oled.printf(printLCD);
-            
-//        for(int i=0; i<3; i++) {
-//            oled.locate(i*6,1);
-//            if(newsStatus[i]==0) oled.printf("Lost");
-//            if(newsStatus[i]==1) oled.printf("None");
-//            if(newsStatus[i]==2) oled.printf("Yoki");
-//        }
-//        for(int i=0; i<2; i++) {
-//            oled.locate(i*6+4,1);
-//            oled.printf("  ");
-//        }
-//static Timer tim;
-//tim.start();
-//telemetry.printf("%d \n\r",tim.read_ms());
-//tim.reset();
-
-
+        telemetry.printf("%d\n\r",conste.timeMs);
     }
 }