Diff: main.cpp

Revision:

3:2c2276646a5c

Parent:

2:1ae5e25618e0

Child:

4:ba166f7b2b8f

--- a/main.cpp	Sun Nov 15 15:23:20 2020 +0000
+++ b/main.cpp	Sun Nov 22 12:03:42 2020 +0000
@@ -1,202 +1,281 @@
 #include "mbed.h"
+#include "IWCMD.h"
+
+/* 設定 */
+const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
+// Servo pulsewidth             MIN     MID     MAX
+double Servo_1_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+double Servo_2_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+double Servo_3_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+double Servo_4_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+double Servo_5_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+double Servo_6_pulthwidth_range[3] = {0.0010, 0.0015, 0.0020};
+
+
+/* 時間設定 */
+/* WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD > WDT_PERIOD                      => 接続状態でもWDTが必ず発動．
+*  WDT_PERIOD/2 < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD       => 1or2回接続確認に失敗するとWDTが発動．
+*  WDT_PERIOD/(N+1) < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD/N => N回以上接続確認に失敗するとWDTが発動するかも．
+*/
+const float WAIT_TIME_PM_REPLY = 0.2f;  // PMの接続確認を待機する時間．xbeeの通信時間を考えて十分に長くとるべし．しかしこの時間はPCからの操作を受け付けないのでできるだけ短いほうが望ましい．
+const float WAIT_TIME_PC_CMD   = 3.0f;  // PCからの命令を待機する時間．長いほうがより長い時間PCからのコマンドを受け付けるが，WDTの周期に注意．
+
+
+
 
 /* ピン設定 */
-PwmOut Servo_1(PA_8);
-// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
-DigitalOut Servo_2_MANUAL(PA_0);
-PwmOut Servo_3(PA_6_ALT0);
-PwmOut Servo_4(PA_3);
-PwmOut Servo_5(PB_4);
-PwmOut Servo_6(PA_7_ALT1);
-Serial xbee(PA_9, PA_10);
-Serial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX, 9600);
+Serial xbee(PA_9, PA_10, 9600);
+DigitalOut xbee_reset(PA_1);
 DigitalOut led[] = {
     DigitalOut(PB_1),
     DigitalOut(PF_0),
     DigitalOut(PF_1),
-    };
-DigitalOut xbee_reset(PA_1);
-DigitalOut nucleo_led(PB_3);
+};
 
-
-/* モジュールID */
-const int kModuleID = 1;
+PwmOut Servo_1(PA_8);
+// PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
+DigitalOut Servo_2(PA_0);
+PwmOut Servo_3(PA_6_ALT0);
+PwmOut Servo_4(PA_3);
+PwmOut Servo_5(PB_4);
+PwmOut Servo_6(PA_7_ALT1);
 
 
-/* サーボ */
-//                               MIN    MID     MAX
-double Servo_1_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
-double Servo_2_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
-double Servo_3_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
-double Servo_4_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
-double Servo_5_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
-double Servo_6_pulsewidth[3] = {0.0013, 0.0015, 0.0017};
 
-Ticker tick_MANUAL;
-Timeout timo_MANUAL;
-double Servo_2_pw_current_MANUAL = 0.001; // 適当,いじる必要なし
-void Servo_2_off_MANUAL(){
-    Servo_2_MANUAL = 0;
-}
-void Servo_2_on_MANUAL(){
-    Servo_2_MANUAL = 1;
-    timo_MANUAL.attach(&Servo_2_off_MANUAL, Servo_2_pw_current_MANUAL);
-}
-void Servo_2_period_ms_MANUAL(int p_ms){
-    Servo_2_off_MANUAL();
-    tick_MANUAL.attach(&Servo_2_on_MANUAL, (double)p_ms / 1.0e3);
-}
-void Servo_2_pulsewidth_MANUAL(double p_ms){
-    Servo_2_pw_current_MANUAL = p_ms;
-}
+/* タイマー設定 */
+Timer tim;
+Ticker  s2_pwm_tick;
+Timeout s2_pwm_timo;
 
 
-/* PM */
-void PM_receive(){
-    while(xbee.readable()){
-        pc.putc(xbee.getc());
-    }
-}
-void PM_init(){
-    xbee.baud(9600);
-    xbee_reset = 1;
-}
-void PM_on(){
-    xbee.printf("a");
-}
-void PM_off(){
-    xbee.printf(" ");
-}
-void PM_CM_alive(){    // PMのWDT用，生存信号送信関数(PMのWDT周期以下で送信する必要あり．)
-    xbee.printf("c");
-}
+/* グローバル変数 */
+double s2_pulsewidth_curr = 0.0015;
 
 
-/* PCシリアル通信 */
-void SerialReceive(void)
-{
-    char message = pc.getc();
-    while(pc.readable())  // bufferを綺麗に
-        pc.getc();
+/* プロトタイプ宣言 */
+void pwm_init();
+void xbee_init();
+void PM_checkconnection(int);
+void transmission_xbee2pc_wait_for(float);
+void Servo_2_off();
+void Servo_2_on();
+void Servo_2_pwm_interrupt();
+void Servo_2_period_ms(int);
+void Servo_2_pulsewidth(double);
+void cmd_exe_wait_for(float);
+void cmd_exe(char);
+void reset_all_motors();
+
+
+Timer opt_tim;
 
-    switch (message) {
-        case 'q':
-            Servo_1.pulsewidth(Servo_1_pulsewidth[2]);
-            break;
-        case 'a':
-            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
-            break;
-        case 'z':
-            Servo_1.pulsewidth(Servo_1_pulsewidth[0]);
-            break;
-        case 'w':
-            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[2]);
-            break;
-        case 's':
-            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
-            break;
-        case 'x':
-            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[0]);
-            break;
-        case 'e':
-            Servo_3.pulsewidth(Servo_3_pulsewidth[2]);
-            break;
-        case 'd':
-            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
-            break;
-        case 'c':
-            Servo_3.pulsewidth(Servo_3_pulsewidth[0]);
-            break;
-        case 'r':
-            Servo_4.pulsewidth(Servo_4_pulsewidth[2]);
-            break;
-        case 'f':
-            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
-            break;
-        case 'v':
-            Servo_4.pulsewidth(Servo_4_pulsewidth[0]);
-            break;
-        case 't':
-            Servo_5.pulsewidth(Servo_5_pulsewidth[2]);
-            break;
-        case 'g':
-            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
-            break;
-        case 'b':
-            Servo_5.pulsewidth(Servo_5_pulsewidth[0]);
-            break;
-        case 'y':
-            Servo_6.pulsewidth(Servo_6_pulsewidth[2]);
-            break;
-        case 'h':
-            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
-            break;
-        case 'n':
-            Servo_6.pulsewidth(Servo_6_pulsewidth[0]);
-            break;
-        case 'u':
-            PM_on();
-            break;
-        case 'j':
-            PM_off();
-            break;
-        default :
-            Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
-            Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
-            Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
-            Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
-            Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
-            Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
-            PM_off();
-            break;
+/* メイン文 */
+int main(){
+    // 初期化
+    pwm_init();
+    xbee_init();
+    
+    wait(1);
+    led[0] = 1;
+    
+    // ループ
+    while(1){
+        pc.printf("CM : %02d\n", kModuleID);
+        // PM1接続確認
+        PM_checkconnection(1);                               // PM1に接続確認
+        transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
+        
+        // コマンド実行
+        cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
+        
+        // PM2接続確認
+        PM_checkconnection(2);                               // PM2に接続確認
+        transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
+        
+        // コマンド実行
+        cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f);             // PCからのコマンド待機
     }
-    led[2] =~ led[2];
 }
 
-
-Timer log_loop_timer;
-int main()
-{
+/* PWM初期化 */
+void pwm_init(){
     Servo_1.period_ms(20);
-    Servo_2_period_ms_MANUAL(20);
+    Servo_2_period_ms(20);
     Servo_3.period_ms(20);
     Servo_4.period_ms(20);
     Servo_5.period_ms(20);
     Servo_6.period_ms(20);
-    Servo_1.pulsewidth(Servo_1_pulsewidth[1]);
-    Servo_2_pulsewidth_MANUAL(Servo_2_pulsewidth[1]);
-    Servo_3.pulsewidth(Servo_3_pulsewidth[1]);
-    Servo_4.pulsewidth(Servo_4_pulsewidth[1]);
-    Servo_5.pulsewidth(Servo_5_pulsewidth[1]);
-    Servo_6.pulsewidth(Servo_6_pulsewidth[1]);
-    PM_off();
+    reset_all_motors();
+}
+
+/* xbee初期化 */
+void xbee_init(){
+    xbee_reset = 1;
+}
+
+
+/* PMとの接続確認，PMのWDT更新 */
+void PM_checkconnection(int id){
+    if(id == 1){
+        xbee.putc(IWCMD_PM1_CCO);
+    }else if(id == 2){
+        xbee.putc(IWCMD_PM2_CCO);
+    }
+}
+
 
-    pc.baud(9600);
-    pc.attach(SerialReceive,Serial::RxIrq);
-    PM_init();
+/* wait_time秒間，xbeeからの通信を受信してPCに表示 */
+void transmission_xbee2pc_wait_for(float wait_time){
+    tim.reset();
+    tim.start();
+    while(tim.read() < wait_time){
+        if(xbee.readable()){
+            pc.putc(xbee.getc());
+        }
+    }
+    tim.stop();
+}
+
 
-    // start motion
-    wait(1);
+/* wait_time秒間，PCからの通信を受信してコマンド実行 */
+void cmd_exe_wait_for(float wait_time){
+    tim.reset();
+    tim.start();
+    while(tim.read() < wait_time){
+        if(pc.readable()){
+            cmd_exe(pc.getc());
+        }
+    }
+    tim.stop();
+}
+
 
-    pc.printf("CM : %02d\n", kModuleID);
+/*各コマンド実行内容*/
+// 全てのモータを停止,初期位置に．
+void reset_all_motors(){
+    Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
+    Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
+    Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
+    Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
+    Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
+    Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
+    xbee.putc(IWCMD_PM1_SPS);
+    xbee.putc(IWCMD_PM2_SPS);
+}
+
 
-    wait(1);
-    led[0] = 1;
-   
-    nucleo_led = 0;
-    while(1) {
-        log_loop_timer.reset();
-        log_loop_timer.start();
-        
-        pc.printf("CM : %02d\n", kModuleID);
-        
-        PM_CM_alive();
-        
-        while(log_loop_timer.read() < 0.5f){
-            PM_receive();
-        }
-       
-        nucleo_led =~ nucleo_led;
-        log_loop_timer.stop();
+/* Servo2用手動PWM */
+void Servo_2_off(){
+    Servo_2 = 0;
+}
+void Servo_2_on(){
+    Servo_2 = 1;
+    s2_pwm_timo.attach(&Servo_2_off, s2_pulsewidth_curr);
+}
+void Servo_2_period_ms(int p_ms){
+    Servo_2_off();
+    s2_pwm_tick.attach(&Servo_2_on, (double)p_ms / 1.0e3);
+}
+void Servo_2_pulsewidth(double p){
+    s2_pulsewidth_curr = p;
+}
+//const int s2_division = 100;   // PWM用分割数
+//int s2_duty;                   // PWM用分割数のうちduty個分High出力
+//int s2_period_us;              // PWM用分割数1つあたりの周期
+//
+//void Servo_2_period_ms(int p_ms){
+//    s2_period_us = (p_ms*1000) / s2_division;
+//    s2_pwm_tick.attach_us(&Servo_2_pwm_interrupt, s2_period_us);
+//}
+//void Servo_2_pulsewidth(double pw){
+//    s2_duty = pw*1000000.0 / (double)s2_period_us;
+//}
+//void Servo_2_pwm_interrupt(){
+//    static int t = 0;
+//    if(t == s2_division){
+//        Servo_2 = 0;
+//        t = 0;
+//        return;
+//    }
+//    if(t == s2_division-s2_duty)
+//        Servo_2 = 1;
+//    t++;
+//}
+
+
+/* コマンド実行 */
+void cmd_exe(char cmd){
+    switch(cmd){
+    case IWCMD_CM_SS1_PP: //スラスターサーボ1を正位置
+        Servo_1.pulsewidth(Servo_1_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
+        Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
+        Servo_1.pulsewidth(Servo_1_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
+        Servo_2_pulsewidth(Servo_2_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
+        Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
+        Servo_2_pulsewidth(Servo_2_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
+        Servo_3.pulsewidth(Servo_3_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
+        Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
+        Servo_3.pulsewidth(Servo_3_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
+        Servo_4.pulsewidth(Servo_4_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
+        Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
+        Servo_4.pulsewidth(Servo_4_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
+        Servo_5.pulsewidth(Servo_5_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
+        Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
+        Servo_5.pulsewidth(Servo_5_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
+        Servo_6.pulsewidth(Servo_6_pulthwidth_range[2]);
+        break;
+    case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
+        Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
+        break;
+    case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
+        Servo_6.pulsewidth(Servo_6_pulthwidth_range[0]);
+        break;
+    case IWCMD_CM_PM1_FR: //PM1を正転(IWCMD_PM1_SUU)
+        xbee.putc(IWCMD_PM1_SUU);
+        break;
+    case IWCMD_CM_PM1_ST: //PM1を停止(IWCMD_PM1_SPS)
+        xbee.putc(IWCMD_PM1_SPS);
+        break;
+    case IWCMD_CM_PM2_FR: //PM2を正転(IWCMD_PM2_SUU)
+        xbee.putc(IWCMD_PM2_SUU);
+        break;
+    case IWCMD_CM_PM2_ST: //PM2を停止(IWCMD_PM2_SPS)
+        xbee.putc(IWCMD_PM2_SPS);
+        break;
+    default:
+        reset_all_motors();
+        break;
     }
+    led[2] =~ led[2];
 }
\ No newline at end of file