Diff: main.cpp

Revision:

6:2e992306910a

Parent:

5:a846e1d2e1d8

Child:

7:68b7925d0cf4

--- a/main.cpp	Sat Nov 28 09:57:53 2020 +0000
+++ b/main.cpp	Mon Nov 30 10:03:46 2020 +0000
@@ -5,14 +5,16 @@
 /* 設定 */
 const int kModuleID = 1;    // コントロールモジュールのID(い今のところ1だけ)
 const float k_vbat_offset = -0.1f;
-#define BATTERY_VOLTAGE_LOWER_LIMIT 5.0f   // バッテリー電圧下限，下回ったらxbeeオフ
+#define BATTERY_VOLTAGE_LOWER_LIMIT 3.0f   // バッテリー電圧下限，下回ったらxbeeオフ
+double Servo_slow_period   = 0.01;        // スローモード時，サーボ角度を変更させる周期
+const double Servo_slow_velocity = 0.02e-3;    // スローモード時，周期ごとに変更させるサーボ角度  <= slowMode時の回転速度はここで調整！
 // 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};
-double Servo_3_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
-double Servo_4_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
-double Servo_5_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
-double Servo_6_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_1_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_2_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_3_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_4_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_5_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
+double Servo_6_pulsewidth_range[3] = {0.0005, 0.0015, 0.0025};
 
 
 /* 時間設定 */
@@ -51,10 +53,13 @@
 Timer tim;
 Ticker  s2_pwm_tick;
 Timeout s2_pwm_timo;
+Ticker Servo_tick;
 
 
 /* グローバル変数 */
 double s2_pulsewidth_curr = 0.0015;
+double Servo_pulsewidth_target[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};
+double Servo_pulsewidth_current[6] = {1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3,1.5e-3};
 
 
 /* プロトタイプ宣言 */
@@ -73,6 +78,7 @@
 float read_battery_voltage();
 void xbee_enable();
 void xbee_disable();
+void Servo_change_angle_interrupt();
 
 
 Timer opt_tim;
@@ -131,6 +137,7 @@
     Servo_5.period_ms(20);
     Servo_6.period_ms(20);
     reset_all_motors();
+    Servo_tick.attach(&Servo_change_angle_interrupt, Servo_slow_period);
 }
 
 /* xbee */
@@ -196,16 +203,52 @@
 /*各コマンド実行内容*/
 // 全てのモータを停止,初期位置に．
 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]);
+    Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
+    Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
+    Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
+    Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
+    Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
+    Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
     xbee.putc(IWCMD_PM1_SPS);
     xbee.putc(IWCMD_PM2_SPS);
 }
 
+/* Servoスローモード関数 */
+void Servo_change_angle_interrupt(){
+    static const double Servo_pulsewidth_range[6][3] = {
+        {Servo_1_pulsewidth_range[0], Servo_1_pulsewidth_range[1], Servo_1_pulsewidth_range[2]},
+        {Servo_2_pulsewidth_range[0], Servo_2_pulsewidth_range[1], Servo_2_pulsewidth_range[2]},
+        {Servo_3_pulsewidth_range[0], Servo_3_pulsewidth_range[1], Servo_3_pulsewidth_range[2]},
+        {Servo_4_pulsewidth_range[0], Servo_4_pulsewidth_range[1], Servo_4_pulsewidth_range[2]},
+        {Servo_5_pulsewidth_range[0], Servo_5_pulsewidth_range[1], Servo_5_pulsewidth_range[2]},
+        {Servo_6_pulsewidth_range[0], Servo_6_pulsewidth_range[1], Servo_6_pulsewidth_range[2]},
+    };
+    for(int i=0; i<6; i++){
+        double v;
+        if(abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i])<Servo_slow_velocity){
+            v = abs(Servo_pulsewidth_current[i]-Servo_pulsewidth_target[i]);
+        }else{
+            v = Servo_slow_velocity;
+        }
+        if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_target[i]){
+            Servo_pulsewidth_current[i] -= v;
+        }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_target[i]){
+            Servo_pulsewidth_current[i] += v;
+        }
+        if      (Servo_pulsewidth_current[i] > Servo_pulsewidth_range[i][2]){
+            Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][2];
+        }else if(Servo_pulsewidth_current[i] < Servo_pulsewidth_range[i][0]){
+            Servo_pulsewidth_current[i] = Servo_pulsewidth_range[i][0];
+        }
+    }
+    Servo_1.pulsewidth(Servo_pulsewidth_current[0]);
+    Servo_2_pulsewidth(Servo_pulsewidth_current[1]);
+    Servo_3.pulsewidth(Servo_pulsewidth_current[2]);
+    Servo_4.pulsewidth(Servo_pulsewidth_current[3]);
+    Servo_5.pulsewidth(Servo_pulsewidth_current[4]);
+    Servo_6.pulsewidth(Servo_pulsewidth_current[5]);
+}
+
 
 /* Servo2用手動PWM */
 void Servo_2_off(){
@@ -250,58 +293,58 @@
 void cmd_exe(char cmd){
     switch(cmd){
     case IWCMD_CM_SS1_PP: //スラスターサーボ1を正位置
-        Servo_1.pulsewidth(Servo_1_pulthwidth_range[2]);
+        Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
-        Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
+        Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
-        Servo_1.pulsewidth(Servo_1_pulthwidth_range[0]);
+        Servo_pulsewidth_target[1-1] = Servo_1_pulsewidth_range[0];
         break;
     case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
-        Servo_2_pulsewidth(Servo_2_pulthwidth_range[2]);
+        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
-        Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
+        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
-        Servo_2_pulsewidth(Servo_2_pulthwidth_range[0]);
+        Servo_pulsewidth_target[2-1] = Servo_2_pulsewidth_range[0];
         break;
     case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
-        Servo_3.pulsewidth(Servo_3_pulthwidth_range[2]);
+        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
-        Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
+        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
-        Servo_3.pulsewidth(Servo_3_pulthwidth_range[0]);
+        Servo_pulsewidth_target[3-1] = Servo_3_pulsewidth_range[0];
         break;
     case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
-        Servo_4.pulsewidth(Servo_4_pulthwidth_range[2]);
+        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
-        Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
+        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
-        Servo_4.pulsewidth(Servo_4_pulthwidth_range[0]);
+        Servo_pulsewidth_target[4-1] = Servo_4_pulsewidth_range[0];
         break;
     case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
-        Servo_5.pulsewidth(Servo_5_pulthwidth_range[2]);
+        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
-        Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
+        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
-        Servo_5.pulsewidth(Servo_5_pulthwidth_range[0]);
+        Servo_pulsewidth_target[5-1] = Servo_5_pulsewidth_range[0];
         break;
     case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
-        Servo_6.pulsewidth(Servo_6_pulthwidth_range[2]);
+        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[2];
         break;
     case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
-        Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
+        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[1];
         break;
     case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
-        Servo_6.pulsewidth(Servo_6_pulthwidth_range[0]);
+        Servo_pulsewidth_target[6-1] = Servo_6_pulsewidth_range[0];
         break;
     case IWCMD_CM_PM1_FR: //PM1を正転(IWCMD_PM1_SUU)
         xbee.putc(IWCMD_PM1_SUU);