Diff: main.cpp

Revision:

6:1d3689cf7132

Parent:

5:c7734a94b30a

Child:

7:81d62e1be97a

--- a/main.cpp	Sat Nov 21 04:12:08 2020 +0000
+++ b/main.cpp	Sun Nov 22 12:03:56 2020 +0000
@@ -1,77 +1,202 @@
-
 #include "mbed.h"
-
-/* Define */
-//#define USB_DEBUG  // xbeeではなくusbに送信
-#define SIMPLE_USE   // xbeeで情報送信できる情報量に制限
-
-/* ID */
-const int kModuleID = 1;
+#include "IWCMD.h"
 
-/* グローバル変数 */
-int sluster_up = 0;
-
-float battery_voltage;
-float motor_current;
-int sluster_power;
-int pump_power;
-
-/* オフセット */
-const float k_vbat_offset[] = {
+/* 設定 */
+#define      kModuleID    2      // パワーモジュールのID( 1 or 2 )
+const double WDT_PERIOD = 4.0;   // WatchDogTimerの周期[sec.] 最後の接続確認からWDT_PERIOD以上2*WDT_PERIOD未満の時間でWDT発動．
+//#define      USB_DEBUG         // USBデバッグをオン(Xbeeはオフ)
+#define      SIMPLE_LOG          // Xbeeで送信できる情報量に制限
+const float k_vbat_offset[] = {  // 電圧読み取りオフセット
     1.4f,
     0.6f
-    };
-const float k_Im_offset[] = {
+};
+const float k_Im_offset[] = {    // 電流読み取りオフセット
     0.0f,
     0.0f
-    };
+};
+
+
 
 /* ピン設定 */
+#if   kModuleID == 1
+DigitalOut sluster(PA_11);    // default : PA_8
+DigitalOut pump(PA_8);        // default : PA_11
+#elif kModuleID == 2
+DigitalOut sluster(PA_8);     // default : PA_8
+DigitalOut pump(PA_11);       // default : PA_11
+#endif
+
+DigitalOut xbee_nreset(PB_5);
 DigitalOut led[] = {
     DigitalOut(PA_4),
     DigitalOut(PA_5),
     DigitalOut(PA_6),
-    };
+};
 DigitalOut user[] = {
     DigitalOut(PB_1),
     DigitalOut(PF_0),
     DigitalOut(PF_1),
-    };
-DigitalOut xbee_nreset(PB_5);
+};
+
 AnalogIn raw_battery_voltage(PA_1);
-AnalogIn raw_motor_shunt_voltage(PA_0);
-//DigitalOut sluster(PA_8);
-//DigitalOut pump(PA_11);
-DigitalOut pump(PA_8);
-DigitalOut sluster(PA_11);  //修正
+AnalogIn raw_sluster_shunt_voltage(PA_0);
+
 #ifndef USB_DEBUG
-Serial xbee(PA_9, PA_10);
+Serial ser(PA_9, PA_10, 9600);
 #else
-Serial xbee(USBTX, USBRX);
+Serial ser(USBTX, USBRX, 9600);
 #endif
 
 
-/* WatchDogTimer関係 */
-Ticker tick_WDT;
-const double TIME_WDT = 2.0;  // [sec] : WDTの周期設定
-bool flag_WDT = 0;
-void WDT_int(){
-    if(flag_WDT == 1){      // 一定時間反応がなかったためスラスター，ポンプのパワーオフ．
-        sluster_power = 0;
-        pump_power = 0;
-        sluster_up = 0;
+/* タイマ設定 */
+Ticker wdt_tick;
+Ticker pwm_tick;
+Ticker duty_acceleration_tick;
+
+
+/* グローバル変数 */
+int sluster_power = 0;     // スラスターDuty比
+int sluster_up = 0;        // スラスターDuty比上昇率
+int pump_power = 0;        // ポンプDuty比
+bool wdt_flag = 0;         // WatchDogTimerフラグ
+
+
+/* プロトタイプ宣言 */
+void cmd_exe(char);
+void motors_power_off();
+void motors_limitter();
+void pwm_init();
+void pwm_interrupt();
+void duty_acceleration_interrupt();
+void xbee_init();
+void wdt_init();
+void wdt_interrupt();
+void wdt_update();
+float read_battery_voltage();
+float read_sluster_current();
+void receive_checkconnection();
+void iwcmd_pm_suu(int);
+void iwcmd_pm_spu(int);
+void iwcmd_pm_spd(int);
+void iwcmd_pm_ppu(int);
+void iwcmd_pm_ppd(int);
+void iwcmd_pm_sps(int);
+void iwcmd_pm_cco(int);
+
+
+/* メイン */
+int main(){
+    // 初期化
+    pwm_init();
+    wdt_init();
+    xbee_init();
+    
+    wait(1);
+    
+#ifndef SIMPLE_LOG
+    ser.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
+#endif //SIMPLE_LOG
+    
+    wait(1);
+    led[0] = 1;
+    
+    // ループ
+    while(1){
+        if(ser.readable()){
+            char cmd = ser.getc();
+            cmd_exe(cmd);
+        }
     }
-    flag_WDT = 1;
-}
-void WDT_init(){
-    tick_WDT.attach(&WDT_int, TIME_WDT);
 }
 
 
-/* PWM出力用割り込み関数 */
-Ticker pwm;
-void myPWM(void)
-{
+
+/* 各コマンド用関数 */
+//スラスターのDuty比上昇率を正
+void iwcmd_pm_suu(int id){
+    if(id == kModuleID){
+        sluster_up = 35;
+    }
+}
+//スラスターのDuty比を上昇
+void iwcmd_pm_spu(int id){
+    if(id == kModuleID){
+        sluster_power += 5;
+    }
+}
+//スラスターのDuty比を減少
+void iwcmd_pm_spd(int id){
+    if(id == kModuleID){
+        sluster_power -= 5;
+    }
+}
+//ポンプのDuty比を上昇
+void iwcmd_pm_ppu(int id){
+    if(id == kModuleID){
+        pump_power += 5;
+    }
+}
+//ポンプのDuty比を減少
+void iwcmd_pm_ppd(int id){
+    if(id == kModuleID){
+        pump_power -= 5;
+    }
+}
+//スラスターとポンプを停止
+void iwcmd_pm_sps(int id){
+    if(id == kModuleID){
+        motors_power_off();
+    }
+}
+//接続確認
+void iwcmd_pm_cco(int id){
+    if(id == kModuleID){
+        receive_checkconnection();
+    }
+}
+
+
+
+/* スラスターとポンプのパワーをオフにする */
+void motors_power_off(){
+    sluster_power = 0;
+    sluster_up = 0;
+    pump_power = 0;
+}
+
+/* モータのDuty比に制限をかける */
+void motors_limitter(){
+    if (sluster_power > 95){
+        sluster_power = 95;
+    } else if (sluster_power < 0) {
+        sluster_power = 0;
+    }
+    
+    if (pump_power > 95){
+        pump_power = 95;
+    } else if (pump_power < 0) {
+        pump_power = 0;
+    }
+}
+
+
+/* 接続確認受信時 */
+void receive_checkconnection(){
+    wdt_update();
+    ser.printf("PM : %02d, BV : %04.2f\n", kModuleID, read_battery_voltage());
+}
+
+
+/* PWM */
+// PWM初期化
+void pwm_init(){
+    sluster_power = 0;
+    pump_power = 0;
+    pwm_tick.attach_us(&pwm_interrupt, 100);
+    duty_acceleration_tick.attach(&duty_acceleration_interrupt, 0.5);
+}
+// PWM割り込み関数
+void pwm_interrupt(){
     static int t = 0;
     if(t == 100){
         sluster = 0;
@@ -85,101 +210,103 @@
         pump = 1;
     t++;
 }
+// Duty比上昇率が正のとき，上昇させる関数
+void duty_acceleration_interrupt(){
+    if ((sluster_up > 0)&&(sluster_up > sluster_power)){
+        sluster_power+=20;
+    }
+#ifndef SIMPLE_LOG
+    ser.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
+#endif //SIMPLE_LOG
+    led[1] =~ led[1];
+}
+
+/* センシング */
+// バッテリー電圧読み取り
+float read_battery_voltage(){
+    return k_vbat_offset[kModuleID-1] + raw_battery_voltage.read() * 33.0f;
+}
+// スラスター電流読み取り
+float read_sluster_current(){
+    return k_Im_offset[kModuleID-1] + raw_sluster_shunt_voltage * 33.0f;
+}
 
 
-/* xbee or usb シリアル受信時実行関数 */
-void SerialReceive(void)
-{
-    flag_WDT = 0;                               // シリアルの信号を受け取るたびにWDTのフラグを折る．
-    char message = xbee.getc();
-    while(xbee.readable())  // bufferを綺麗に
-        xbee.getc();
-        
-    switch (message) {
-        case 'a':
-            sluster_up = 35;
-            break;
-        case 'u':
-            sluster_power += 5;
-            break;
-        case 'd':
-            sluster_power -= 5;
-            break;
-        case '+':
-            pump_power += 5;
-            break;
-        case '-':
-            pump_power -= 5;
-            break;
-        case 'c':                       // 接続確認用コマンド(WDT生存確認に使用)．何もしない．
-            break;
-        default :
-            sluster_power = 0;
-            pump_power = 0;
-            sluster_up = 0;
-            break;
+/* xbee */
+// xbee初期化
+void xbee_init(){
+    xbee_nreset = 1;
+}
+
+
+/* WatchDogTimer */
+// WDTの初期化
+void wdt_init(){
+    wdt_tick.attach(&wdt_interrupt, WDT_PERIOD);
+}
+//WDTの割り込み関数
+void wdt_interrupt(){
+    if(wdt_flag == 1){
+        motors_power_off();
     }
-    
-    if (sluster_power > 95){
-        sluster_power = 95;
-    } else if (sluster_power < 0) {
-        sluster_power = 0;
-    }
-    
-    if (pump_power > 95){
-        pump_power = 95;
-    } else if (pump_power < 0) {
-        pump_power = 0;
-    }
-    led[2] =~ led[2];
+    wdt_flag = 1;
+}
+// WDTのフラグ更新
+void wdt_update(){
+    wdt_flag = 0;
 }
 
 
-
-/* メイン文 */
-int main()
-{
-    // Initialization
-    pump_power = 0;
-    sluster_power = 0;
-    pwm.attach_us(myPWM, 100); // original PWM function
-    //sluster.period_ms(1);
-    //pump.period_ms(1);
-    xbee_nreset = 1;    // xBee nreset
-    xbee.baud(9600);
-    xbee.attach(SerialReceive,Serial::RxIrq);
-    WDT_init();
-    
-    // start motion
-    wait(1);
-#ifdef USB_DEBUG
-    xbee.printf("2020 Project Iwatobi, PowerModule ID : %02d\n", kModuleID);
-#else
-    xbee.printf("PM : %02d\n", kModuleID);
-#endif
-    wait(1);
-    led[0] = 1;
-    
-    
-    while(1) {
-        //sluster.write(0.01*sluster_power);
-        //pump.write(0.01*pump_power);
-        if ((sluster_up > 0)&&(sluster_up > sluster_power)){
-            sluster_power+=20;
-        }
-        battery_voltage = k_vbat_offset[kModuleID] + raw_battery_voltage.read() * 33.0f;   // 10.0 * 3.3
-        motor_current = k_Im_offset[kModuleID] + raw_motor_shunt_voltage * 33.0f;    // 10.0 * 3.3
-        led[1] =~ led[1];
-#ifndef SIMPLE_USE
-#ifdef USB_DEBUG
-        xbee.printf("p1 : %03d, p2 : %03d\n", sluster_power, pump_power);
-        xbee.printf("Vbat : %2.1f [V], Im : %2.1f [A]\n", battery_voltage, motor_current);
-#else
-        xbee.printf("%03d, %03d, %2.1f, %2.1f\n", sluster_power, pump_power, battery_voltage, motor_current);
-#endif
-#else
-        xbee.printf("PM : %02d\n", kModuleID);
-#endif
-        wait(0.5);
+/* コマンド実行 */
+void cmd_exe(char cmd){
+    switch(cmd){
+    case IWCMD_PM1_SUU: //PM1スラスターのDuty比上昇率を正
+        iwcmd_pm_suu(1);
+        break;
+    case IWCMD_PM1_SPU: //PM1スラスターのDuty比を上昇
+        iwcmd_pm_spu(1);
+        break;
+    case IWCMD_PM1_SPD: //PM1スラスターのDuty比を減少
+        iwcmd_pm_spd(1);
+        break;
+    case IWCMD_PM1_PPU: //PM1ポンプのDuty比を上昇
+        iwcmd_pm_ppu(1);
+        break;
+    case IWCMD_PM1_PPD: //PM1ポンプのDuty比を減少
+        iwcmd_pm_ppd(1);
+        break;
+    case IWCMD_PM1_SPS: //PM1のスラスターとポンプを停止
+        iwcmd_pm_sps(1);
+        break;
+    case IWCMD_PM1_CCO: //PM1の接続確認
+        iwcmd_pm_cco(1);
+        break;
+    case IWCMD_PM2_SUU: //PM2スラスターのDuty比上昇率を正
+        iwcmd_pm_suu(2);
+        break;
+    case IWCMD_PM2_SPU: //PM2スラスターのDuty比を上昇
+        iwcmd_pm_spu(2);
+        break;
+    case IWCMD_PM2_SPD: //PM2スラスターのDuty比を減少
+        iwcmd_pm_spd(2);
+        break;
+    case IWCMD_PM2_PPU: //PM2ポンプのDuty比を上昇
+        iwcmd_pm_ppu(2);
+        break;
+    case IWCMD_PM2_PPD: //PM2ポンプのDuty比を減少
+        iwcmd_pm_ppd(2);
+        break;
+    case IWCMD_PM2_SPS: //PM2のスラスターとポンプを停止
+        iwcmd_pm_sps(2);
+        break;
+    case IWCMD_PM2_CCO: //PM2の接続確認
+        iwcmd_pm_cco(2);
+        break;
+    default:
+        iwcmd_pm_sps(1);  //PM1のスラスターとポンプを停止
+        iwcmd_pm_sps(2);  //PM2のスラスターとポンプを停止
+        break;
     }
-}
+    motors_limitter();
+    led[2] =~ led[2];
+}
\ No newline at end of file