ryou sato / Mbed 2 deprecated PPA

Dependencies:   mbed

Revision:
0:5637e966d2f5
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 19 09:37:34 2021 +0000
@@ -0,0 +1,700 @@
+//メインプログラム・UARTコマンド受信
+#include "mbed.h"
+#include <cstdio>
+#include <string>
+#include "ColorSensor.h"
+
+#define SirialComSpeed 38400                                                        //シリアル通信のBPS設定
+
+#define CommandNumber (sizeof CommandList) / (sizeof(CdListTable))                  //コマンドの数(CommandNumber)
+
+//変数定義
+int servoflag = 0;              //サーボホーン位置情報格納用
+int *pservoflag;                //ポインター
+int off_width = 1720;           //サーボホーン初期位置パルス幅[us]
+int *poff_width;                //ポインター
+int on_width = 1500;            //サーボホーン終端位置パルス幅[us]
+int *pon_width;                 //ポインター
+
+//シリアル関連
+RawSerial SerialCom(SERIAL_TX, SERIAL_RX);                                //シリアル通信
+std::string RcvCharStr = "";                                            //シリアル通信受信文字格納用
+
+//ピン設定
+DigitalOut led(LED1);
+PwmOut servopwm(PA_0);
+InterruptIn det0(PA_4);                       //POWER PKG 設置検知
+InterruptIn det1(PC_1);                       //MAIN PKG 設置検知
+InterruptIn det2(PC_0);                       //レバー降ろし検知
+DigitalOut cmuxen(PA_8);                    //カラーセンサー用マルチプレクサENピン
+BusOut cmuxaddr(PC_7,PA_9);                 //カラーセンサー用マルチプレクサADDRバス
+DigitalOut rmuxen(PA_10);                   //リレー用マルチプレクサENピン
+BusOut rmuxaddr(PB_10,PB_4,PB_5,PB_3);      //リレー用マルチプレクサADDRバス
+DigitalInOut relayctl1(PA_11, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
+DigitalInOut relayctl2(PB_12, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
+DigitalInOut relayctl3(PB_2, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
+DigitalInOut relayctl4(PB_1, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
+DigitalInOut relayctl5(PB_15, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
+DigitalInOut relayctl6(PB_14, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
+DigitalInOut relayctl7(PB_13, PIN_OUTPUT, OpenDrain, 0);              //個別リレー用制御
+DigitalInOut relayctl8(PC_4, PIN_OUTPUT, OpenDrain, 0);               //個別リレー用制御
+
+static std::string none;
+
+//関数宣言
+int help_mode(std::string para);
+int relay_set(std::string para);            //リレー用制御
+void initial();                             //初期化
+int led_lum_check(std::string para);        //カラーセンサー用マルチプレクサ制御 + 輝度確認
+int led_all_check(std::string para);
+int servo_turn(std::string para);
+int pkg_detection(std::string para);
+void main_uart_receive();                   //コマンド文字列受信処理
+void risedet_interrupt();
+void falldet_interrupt();
+
+//クラス名の定義
+CsMode CsMode;
+
+//コマンドの宣言
+typedef struct {
+    std::string Commond;
+    int (* CommandFunction)(std::string para);
+} CdListTable;
+
+//コマンド一覧(左側がコマンド、右側が飛び先の関数)
+CdListTable CommandList[] = {
+    {"help",   help_mode},
+    {"led",   led_lum_check},
+    {"led all",   led_all_check},
+    {"relay",   relay_set},
+    {"servo",   servo_turn},
+    {"det",   pkg_detection},
+};
+
+//ヘルプ用コマンド一覧
+std::string HelpStr[] = {
+    "* Commands List **********************************************************",
+    "relayALLoff            : relay control",
+    "relay1,2{on|off}",
+    "relay3{on|off}",
+    "relay4{on|off}",
+    "relay5{on|off}",
+    "relay21{on|off}",
+    "relay22,23{on|off}",
+    "relay24,25{on|off}",
+    "relay26{on|off}",
+    "relay26on20ms",
+    "relay26on73ms",
+    "relayMUXoff",
+    "relay6on",
+    "relay7on",
+    "relay8on",
+    "relay9on",
+    "relay10on",
+    "relay11on",
+    "relay12on",
+    "relay13on",
+    "relay14on",
+    "relay15on",
+    "relay16on",
+    "relay17on",
+    "relay18on",
+    "relay19on",
+    "relay20on",
+    "servo{on|off}          : servo control",
+    "led{1|2|3}             : LED luminance check",
+    "det                    : pkg,lever detection",
+    "**************************************************************************",
+    "",     //helpの終わりを示す
+};
+
+//ヘルプ表示
+int help_mode(std::string para)
+{
+    int tmpCount = 0;
+
+    //HelpStrを空文字が出てくるまで繰り返し表示
+    while(HelpStr[tmpCount] != "") {
+        SerialCom.printf("%s\r\n",HelpStr[tmpCount].c_str());
+        tmpCount++;
+    }
+    return 0;
+}
+
+//リレー制御
+int relay_set(std::string para)
+{
+    //基板とレバーが未検知の時はコマンドを実行しない
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
+        return 0;
+    }
+
+    //パラメータが
+    if(para.substr(0)=="ALLoff") {
+        relayctl1 = 0;
+        relayctl2 = 0;
+        relayctl3 = 0;
+        relayctl4 = 0;
+        relayctl5 = 0;
+        relayctl6 = 0;
+        relayctl7 = 0;
+        relayctl8 = 0;
+        rmuxen = 1;
+        rmuxaddr.write(0);
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="1,2on") {
+        relayctl1 = 1;
+
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="1,2off") {
+        relayctl1 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="3on") {
+        relayctl2 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="3off") {
+        relayctl2 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="4on") {
+        relayctl3 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="4off") {
+        relayctl3 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="5on") {
+        relayctl4 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="5off") {
+        relayctl4 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="21on") {
+        relayctl5 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="21off") {
+        relayctl5 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="22,23on") {
+        relayctl6 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="22,23off") {
+        relayctl6 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="24,25on") {
+        relayctl7 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="24,25off") {
+        relayctl7 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="26on") {
+        relayctl8 = 1;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="26off") {
+        relayctl8 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="26on20ms") {
+        relayctl8 = 1;
+        wait(0.02);
+        relayctl8 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="26on73ms") {
+        relayctl8 = 1;
+        wait(0.073);
+        relayctl8 = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="MUXoff") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(0);
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="6on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(1);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="7on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(2);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="8on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(3);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="9on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(4);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="10on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(5);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="11on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(6);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="12on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(7);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="13on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(8);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="14on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(9);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="15on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(10);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="16on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(11);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="17on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(12);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="18on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(13);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="19on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(14);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+    } else if (para.substr(0)=="20on") {
+        rmuxen = 1;
+        wait(0.01);
+        rmuxaddr.write(15);
+        wait(0.01);
+        rmuxen = 0;
+        SerialCom.printf ("OK\r\n");
+        return 0;
+        //パラメータが上記以外の場合
+    } else {
+        SerialCom.printf(" ERROR!\r\n");
+        return 0;
+    }
+    //return 0;
+}
+
+//LEDの明るさ取得
+int led_lum_check(std::string para)
+{
+    //基板とレバーが未検知の時はコマンドを実行しない
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
+        return 0;
+    }
+
+    int tmpInt = atoi(para.c_str());                //パラメータを数字に変換
+    cmuxen = 1;                                     //マルチプレクサのENピンをHIGHにする
+    wait(0.01);
+
+    //ADDRバス切替
+    cmuxaddr.write(tmpInt);
+    wait(0.01);
+
+    cmuxen = 0;                                     //マルチプレクサのENピンをLOWにする
+    wait(0.01);
+
+    //選択したLED表示
+    //SerialCom.printf ("LED%d_", tmpInt);
+
+    //LEDの輝度表示
+    CsMode.SensorMain();
+
+    SerialCom.printf ("OK\r\n");
+    return 0;
+}
+
+int pkg_detection(std::string para)
+{
+    int ppkgdet = det0;
+    int mpkgdet = det1;
+    int leverdet = det2;
+
+    SerialCom.printf("%d%d%d\r\n", leverdet, mpkgdet, ppkgdet);
+    SerialCom.printf ("OK\r\n");
+    return 0;
+}
+
+//コマンド文字列解析
+void ReceiveCmdAnalyze()
+{
+    //入力された文字列を1文字ずつ解析(文字列の長さ分まで繰り返す)
+    for(int y=0; y<=RcvCharStr.size(); y++) {
+
+        //コマンド全部と比較し一致しているのがあれば処理開始(前方一致)
+        for (int i=0; i<CommandNumber; i++) {
+            if(RcvCharStr.substr(0,y) == CommandList[i].Commond) {
+
+                //コマンド以降の文字列を引数にして関数呼び出し
+                (* *CommandList[i].CommandFunction)(RcvCharStr.substr(y));
+
+                //文字列のバッファクリア
+                RcvCharStr = "";
+                return;
+            }
+        }
+    }
+    //エラーメッセージ表示
+    SerialCom.printf (" ERROR!\r\n\r\n");
+    RcvCharStr = "";
+    return;
+}
+
+//UART受信
+void main_uart_receive()
+{
+    std::string tmpStr = "";                         //シリアル通信受信文字格納(1文字)
+    tmpStr = SerialCom.getc();                       //受信文字を配列に追加
+    SerialCom.printf("%s",tmpStr.c_str());           //エコーバック出力
+
+    //①改行文字が来た場合コマンド解析へ
+    if(tmpStr == "\r") {
+        SerialCom.printf ("\r\n");                                //TeraTerm改行
+        ReceiveCmdAnalyze();                                                //コマンド解析
+        return;
+
+        //②バックスペースが来た場合は文字列の末尾を削除
+    } else if(tmpStr == "\b") {
+
+        //1文字もない場合は何もしない
+        if(RcvCharStr.size()>0) {
+            RcvCharStr.erase(RcvCharStr.length()-1,1);
+        }
+
+        //③上記以外の場合は受信した文字をRcvCharStrへ格納
+    } else {
+        RcvCharStr +=  tmpStr;
+    }
+}
+
+//サーボモータ制御
+int servo_turn(std::string para)
+{
+    //オフ時のパルス幅は上記で定義
+    //off_width = 1550;
+    //on_width = 1350;
+
+    //基板とレバーが未検知の時はコマンドを実行しない
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
+        return 0;
+    }
+
+    int waitms = 5;       //パルス時間変更後の待ち時間(ms) (動作速度:0.1秒/60度(サーボモータSG92仕様))
+
+    //パラメータがオフ(off)の時
+    if(para.substr(0)=="off") {
+        if(servoflag == 0) {
+            SerialCom.printf(" ERROR! already OFF\r\n");
+            return 0;
+        } else {
+            /*
+            int pulseus = on_width;
+            servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+            while (pulseus <= off_width) {
+                servopwm.pulsewidth_us(pulseus);
+                wait_ms(waitms);
+                pulseus = pulseus + 1;
+            }
+            */
+            int pulseus = off_width;
+            servopwm.period_ms(20);
+            servopwm.pulsewidth_us(pulseus);
+            servoflag = 0;              //サーボホーン位置情報
+        }
+    }
+
+    //パラメータがオン(on)の時
+    else if(para.substr(0)=="on") {
+        //往路
+        if(servoflag == 1) {
+            SerialCom.printf(" ERROR! already ON\r\n");
+            return 0;
+        } else {
+            int pulseus = off_width;
+            servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+            while (pulseus >= on_width) {
+                servopwm.pulsewidth_us(pulseus);
+                wait_ms(waitms);
+                pulseus = pulseus - 1;
+            }
+            servoflag = 1;              //サーボホーン位置情報
+        }
+
+        wait(1);      //スイッチを押し切る時間を確保
+
+        //復路
+        if(servoflag == 0) {
+            SerialCom.printf(" ERROR! already OFF\r\n");
+            return 0;
+        } else {
+            int pulseus = on_width;
+            servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+            while (pulseus <= off_width) {
+                servopwm.pulsewidth_us(pulseus);
+                wait_ms(waitms);
+                pulseus = pulseus + 1;
+            }
+            servoflag = 0;              //サーボホーン位置情報
+
+            SerialCom.printf ("OK\r\n");
+        }
+    }
+    //パラメータがオン時のパルス幅(****us)のとき
+    else {
+        int tmpInt = atoi(para.c_str());
+        int pulseus = off_width;
+        servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+        while (pulseus >= tmpInt) {
+            servopwm.pulsewidth_us(pulseus);
+            wait_ms(waitms);
+            pulseus = pulseus - 1;
+        }
+
+        wait(1);
+
+        pulseus = tmpInt;
+        //servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+        while (pulseus <= off_width) {
+            servopwm.pulsewidth_us(pulseus);
+            wait_ms(waitms);
+            pulseus = pulseus + 1;
+        }
+        SerialCom.printf ("OK\r\n");
+    }
+    return 0;
+}
+
+//リレーとマルチプレクサの初期設定
+void initial()
+{
+    relayctl1 = 0;
+    relayctl2 = 0;
+    relayctl3 = 0;
+    relayctl4 = 0;
+    relayctl5 = 0;
+    relayctl6 = 0;
+    relayctl7 = 0;
+    relayctl8 = 0;
+    rmuxen = 1;
+    rmuxaddr.write(0);
+
+    //LEDのマルチプレクサをOFFに
+    cmuxen = 1;
+
+    return;
+}
+
+//LED全チェック
+int led_all_check(std::string para)
+{
+    //基板とレバーが未検知の時はコマンドを実行しない
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf ("\r\n Error! PKG, lever undetected!\r\n\r\n");
+        return 0;
+    }
+
+    char buf[2];
+
+    //led_lum_checkをLED1-24まで実施
+    for (int x = 1; x <= 3; x++) {
+        snprintf(buf, 12, "%d", x);
+        std::string str(buf, 2);
+        led_lum_check(str);
+    }
+    return 0;
+}
+
+void risedet_interrupt()
+{
+    wait(0.001);
+    led = 0;
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf("\r\n Error! PKG, lever undetected! initializing...\r\n");
+
+        //初期設定
+        initial();
+
+        //サーボがonの場合はoffへ
+        if(servoflag == 1) {
+            servo_turn("off");
+        }
+
+        //PKGとレバーが検知されるのを待つ
+        SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
+        intdet0 = det0;
+        intdet1 = det1;
+        intdet2 = det2;
+        while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+            intdet0 = det0;
+            intdet1 = det1;
+            intdet2 = det2;
+            if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
+                main_uart_receive();
+            }
+        }
+        SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n restart!\r\n\r\n");
+    }
+    led = 1;
+}
+
+int main()
+{
+    led = 0;
+
+    //UART ボーレート設定
+    SerialCom.baud(SirialComSpeed);
+
+    //起動表示
+    SerialCom.printf ("PCU-1L Unitchecker Initializing...\r\n");
+
+    /* 基板上にプルアップを入れたのでコメントアウト
+    //ピン変化割込みポートモード設定
+    det0.mode(PullUp);
+    det1.mode(PullUp);
+    det2.mode(PullUp);
+    */
+
+    //初期設定
+    initial();
+
+    //LEDチェック
+    led_all_check(none);
+
+    //サーボモータ第一初期化
+    pservoflag = &servoflag;        //ポインター(サーボホーン位置情報格納用)
+    poff_width = &off_width;
+    pon_width = &on_width;
+    servopwm.period_ms(20);         //PWMサイクル周期(サーボモータSG92仕様)
+    servopwm.pulsewidth_us(off_width);   //角度が決まる
+
+    //PKGとレバーが検知されるのを待つ
+    int intdet0 = det0;
+    int intdet1 = det1;
+    int intdet2 = det2;
+    if(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+        SerialCom.printf ("\r\n Waiting for PKG installation...\r\n");
+        while(intdet0 == 1 | intdet1 == 1 | intdet2 == 1) {
+            //wait(1);
+            intdet0 = det0;
+            intdet1 = det1;
+            intdet2 = det2;
+            if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
+                main_uart_receive();
+            }
+        }
+        SerialCom.printf("\r\n PKG, lever detected!\r\n\r\n");
+    }
+
+    led = 1;
+
+    //PKG,レバー未検知割込みを起動
+    det0.rise(&risedet_interrupt);
+    det1.rise(&risedet_interrupt);
+    det2.rise(&risedet_interrupt);
+
+    SerialCom.printf (" ready!\r\n\r\n");
+    //メインループ
+    while(1) {
+        if (SerialCom.readable()) {       //読み込む文字があるかどうか判定
+            main_uart_receive();
+        }
+    }
+}