Diff: main.cpp

Revision:

5:a846e1d2e1d8

Parent:

4:ba166f7b2b8f

Child:

6:2e992306910a

--- a/main.cpp	Sun Nov 22 15:10:21 2020 +0000
+++ b/main.cpp	Sat Nov 28 09:57:53 2020 +0000
@@ -1,8 +1,11 @@
 #include "mbed.h"
 #include "IWCMD.h"
+#warning CM 's program
 
 /* 設定 */
 const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
+const float k_vbat_offset = -0.1f;
+#define BATTERY_VOLTAGE_LOWER_LIMIT 5.0f   // バッテリー電圧下限，下回ったらxbeeオフ
 // Servo pulsewidth             MIN     MID     MAX
 double Servo_1_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
 double Servo_2_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
@@ -32,6 +35,7 @@
     DigitalOut(PF_0),
     DigitalOut(PF_1),
 };
+AnalogIn raw_battery_voltage(PA_4);
 
 PwmOut Servo_1(PA_8);
 // PwmOut Servo_2(PA_0);  // us_tickerにてTIM2を利用中のため使用不可
@@ -66,6 +70,9 @@
 void cmd_exe_wait_for(float);
 void cmd_exe(char);
 void reset_all_motors();
+float read_battery_voltage();
+void xbee_enable();
+void xbee_disable();
 
 
 Timer opt_tim;
@@ -77,11 +84,28 @@
     xbee_init();
     
     wait(1);
-    led[0] = 1;
     
     // ループ
     while(1){
-        pc.printf("CM : %02d\n", kModuleID);
+        
+        // バッテリー監視
+        float bv = read_battery_voltage();
+        if(bv <= BATTERY_VOLTAGE_LOWER_LIMIT){
+            int i=1;
+            while(read_battery_voltage() <= BATTERY_VOLTAGE_LOWER_LIMIT){
+                i++;
+                if(i==100){
+                    xbee_disable();
+                    break;
+                }
+            }
+        }else{
+            xbee_enable();
+        }
+
+        // CM接続確認
+        pc.printf("CM : %02d, BV : %04.2f\n", kModuleID, bv);
+        
         // PM1接続確認
         PM_checkconnection(1);                               // PM1に接続確認
         transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY);   // xbeeからの受信待機
@@ -109,9 +133,20 @@
     reset_all_motors();
 }
 
-/* xbee初期化 */
+/* xbee */
+// 初期化
 void xbee_init(){
+    xbee_enable();
+}
+// xbee動作許可
+void xbee_enable(){
     xbee_reset = 1;
+    led[0] = 1;
+}
+// xbee動作禁止
+void xbee_disable(){
+    xbee_reset = 0;
+    led[0] = 0;
 }
 
 
@@ -151,6 +186,13 @@
 }
 
 
+/* センシング */
+// バッテリー電圧読み取り
+float read_battery_voltage(){
+    return k_vbat_offset + raw_battery_voltage.read() * 33.0f;
+}
+
+
 /*各コマンド実行内容*/
 // 全てのモータを停止,初期位置に．
 void reset_all_motors(){
@@ -277,5 +319,5 @@
         reset_all_motors();
         break;
     }
-    led[2] =~ led[2];
+    led[2] = !led[2];
 }
\ No newline at end of file