Shohei Kamiguchi / Mbed 2 deprecated Iwatobi_Control_Modulen

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@5:a846e1d2e1d8, 2020-11-28 (annotated)

Committer:
DiGengengen
Date:
Sat Nov 28 09:57:53 2020 +0000
Revision:
5:a846e1d2e1d8
Parent:
4:ba166f7b2b8f
Child:
6:2e992306910a
hoge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kintoki231 0:dde37cabc667 1 #include "mbed.h"
DiGengengen 3:2c2276646a5c 2 #include "IWCMD.h"
DiGengengen 5:a846e1d2e1d8 3 #warning CM 's program
DiGengengen 3:2c2276646a5c 4
DiGengengen 3:2c2276646a5c 5 /* 設定 */
DiGengengen 3:2c2276646a5c 6 const int kModuleID = 1; // コントロールモジュールのID(い今のところ1だけ)
DiGengengen 5:a846e1d2e1d8 7 const float k_vbat_offset = -0.1f;
DiGengengen 5:a846e1d2e1d8 8 #define BATTERY_VOLTAGE_LOWER_LIMIT 5.0f // バッテリー電圧下限，下回ったらxbeeオフ
DiGengengen 3:2c2276646a5c 9 // Servo pulsewidth MIN MID MAX
DiGengengen 4:ba166f7b2b8f 10 double Servo_1_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 4:ba166f7b2b8f 11 double Servo_2_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 4:ba166f7b2b8f 12 double Servo_3_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 4:ba166f7b2b8f 13 double Servo_4_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 4:ba166f7b2b8f 14 double Servo_5_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 4:ba166f7b2b8f 15 double Servo_6_pulthwidth_range[3] = {0.0005, 0.0015, 0.0025};
DiGengengen 3:2c2276646a5c 16
DiGengengen 3:2c2276646a5c 17
DiGengengen 3:2c2276646a5c 18 /* 時間設定 */
DiGengengen 3:2c2276646a5c 19 /* WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD > WDT_PERIOD => 接続状態でもWDTが必ず発動．
DiGengengen 3:2c2276646a5c 20 * WDT_PERIOD/2 < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD => 1or2回接続確認に失敗するとWDTが発動．
DiGengengen 3:2c2276646a5c 21 * WDT_PERIOD/(N+1) < WAIT_TIME_PM_REPLY + WAIT_TIME_PC_CMD < WDT_PERIOD/N => N回以上接続確認に失敗するとWDTが発動するかも．
DiGengengen 3:2c2276646a5c 22 */
DiGengengen 3:2c2276646a5c 23 const float WAIT_TIME_PM_REPLY = 0.2f; // PMの接続確認を待機する時間．xbeeの通信時間を考えて十分に長くとるべし．しかしこの時間はPCからの操作を受け付けないのでできるだけ短いほうが望ましい．
DiGengengen 3:2c2276646a5c 24 const float WAIT_TIME_PC_CMD = 3.0f; // PCからの命令を待機する時間．長いほうがより長い時間PCからのコマンドを受け付けるが，WDTの周期に注意．
DiGengengen 3:2c2276646a5c 25
DiGengengen 3:2c2276646a5c 26
DiGengengen 3:2c2276646a5c 27
kintoki231 0:dde37cabc667 28
kintoki231 0:dde37cabc667 29 /* ピン設定 */
DiGengengen 3:2c2276646a5c 30 Serial pc(USBTX, USBRX, 9600);
DiGengengen 3:2c2276646a5c 31 Serial xbee(PA_9, PA_10, 9600);
DiGengengen 3:2c2276646a5c 32 DigitalOut xbee_reset(PA_1);
kintoki231 0:dde37cabc667 33 DigitalOut led[] = {
kintoki231 0:dde37cabc667 34 DigitalOut(PB_1),
kintoki231 0:dde37cabc667 35 DigitalOut(PF_0),
kintoki231 0:dde37cabc667 36 DigitalOut(PF_1),
DiGengengen 3:2c2276646a5c 37 };
DiGengengen 5:a846e1d2e1d8 38 AnalogIn raw_battery_voltage(PA_4);
kintoki231 0:dde37cabc667 39
DiGengengen 3:2c2276646a5c 40 PwmOut Servo_1(PA_8);
DiGengengen 3:2c2276646a5c 41 // PwmOut Servo_2(PA_0); // us_tickerにてTIM2を利用中のため使用不可
DiGengengen 3:2c2276646a5c 42 DigitalOut Servo_2(PA_0);
DiGengengen 3:2c2276646a5c 43 PwmOut Servo_3(PA_6_ALT0);
DiGengengen 3:2c2276646a5c 44 PwmOut Servo_4(PA_3);
DiGengengen 3:2c2276646a5c 45 PwmOut Servo_5(PB_4);
DiGengengen 3:2c2276646a5c 46 PwmOut Servo_6(PA_7_ALT1);
kintoki231 0:dde37cabc667 47
kintoki231 0:dde37cabc667 48
kintoki231 0:dde37cabc667 49
DiGengengen 3:2c2276646a5c 50 /* タイマー設定 */
DiGengengen 3:2c2276646a5c 51 Timer tim;
DiGengengen 3:2c2276646a5c 52 Ticker s2_pwm_tick;
DiGengengen 3:2c2276646a5c 53 Timeout s2_pwm_timo;
kintoki231 0:dde37cabc667 54
kintoki231 0:dde37cabc667 55
DiGengengen 3:2c2276646a5c 56 /* グローバル変数 */
DiGengengen 3:2c2276646a5c 57 double s2_pulsewidth_curr = 0.0015;
kintoki231 0:dde37cabc667 58
kintoki231 0:dde37cabc667 59
DiGengengen 3:2c2276646a5c 60 /* プロトタイプ宣言 */
DiGengengen 3:2c2276646a5c 61 void pwm_init();
DiGengengen 3:2c2276646a5c 62 void xbee_init();
DiGengengen 3:2c2276646a5c 63 void PM_checkconnection(int);
DiGengengen 3:2c2276646a5c 64 void transmission_xbee2pc_wait_for(float);
DiGengengen 3:2c2276646a5c 65 void Servo_2_off();
DiGengengen 3:2c2276646a5c 66 void Servo_2_on();
DiGengengen 3:2c2276646a5c 67 void Servo_2_pwm_interrupt();
DiGengengen 3:2c2276646a5c 68 void Servo_2_period_ms(int);
DiGengengen 3:2c2276646a5c 69 void Servo_2_pulsewidth(double);
DiGengengen 3:2c2276646a5c 70 void cmd_exe_wait_for(float);
DiGengengen 3:2c2276646a5c 71 void cmd_exe(char);
DiGengengen 3:2c2276646a5c 72 void reset_all_motors();
DiGengengen 5:a846e1d2e1d8 73 float read_battery_voltage();
DiGengengen 5:a846e1d2e1d8 74 void xbee_enable();
DiGengengen 5:a846e1d2e1d8 75 void xbee_disable();
DiGengengen 3:2c2276646a5c 76
DiGengengen 3:2c2276646a5c 77
DiGengengen 3:2c2276646a5c 78 Timer opt_tim;
kintoki231 0:dde37cabc667 79
DiGengengen 3:2c2276646a5c 80 /* メイン文 */
DiGengengen 3:2c2276646a5c 81 int main(){
DiGengengen 3:2c2276646a5c 82 // 初期化
DiGengengen 3:2c2276646a5c 83 pwm_init();
DiGengengen 3:2c2276646a5c 84 xbee_init();
DiGengengen 3:2c2276646a5c 85
DiGengengen 3:2c2276646a5c 86 wait(1);
DiGengengen 3:2c2276646a5c 87
DiGengengen 3:2c2276646a5c 88 // ループ
DiGengengen 3:2c2276646a5c 89 while(1){
DiGengengen 5:a846e1d2e1d8 90
DiGengengen 5:a846e1d2e1d8 91 // バッテリー監視
DiGengengen 5:a846e1d2e1d8 92 float bv = read_battery_voltage();
DiGengengen 5:a846e1d2e1d8 93 if(bv <= BATTERY_VOLTAGE_LOWER_LIMIT){
DiGengengen 5:a846e1d2e1d8 94 int i=1;
DiGengengen 5:a846e1d2e1d8 95 while(read_battery_voltage() <= BATTERY_VOLTAGE_LOWER_LIMIT){
DiGengengen 5:a846e1d2e1d8 96 i++;
DiGengengen 5:a846e1d2e1d8 97 if(i==100){
DiGengengen 5:a846e1d2e1d8 98 xbee_disable();
DiGengengen 5:a846e1d2e1d8 99 break;
DiGengengen 5:a846e1d2e1d8 100 }
DiGengengen 5:a846e1d2e1d8 101 }
DiGengengen 5:a846e1d2e1d8 102 }else{
DiGengengen 5:a846e1d2e1d8 103 xbee_enable();
DiGengengen 5:a846e1d2e1d8 104 }
DiGengengen 5:a846e1d2e1d8 105
DiGengengen 5:a846e1d2e1d8 106 // CM接続確認
DiGengengen 5:a846e1d2e1d8 107 pc.printf("CM : %02d, BV : %04.2f\n", kModuleID, bv);
DiGengengen 5:a846e1d2e1d8 108
DiGengengen 3:2c2276646a5c 109 // PM1接続確認
DiGengengen 3:2c2276646a5c 110 PM_checkconnection(1); // PM1に接続確認
DiGengengen 3:2c2276646a5c 111 transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY); // xbeeからの受信待機
DiGengengen 3:2c2276646a5c 112
DiGengengen 3:2c2276646a5c 113 // コマンド実行
DiGengengen 3:2c2276646a5c 114 cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f); // PCからのコマンド待機
DiGengengen 3:2c2276646a5c 115
DiGengengen 3:2c2276646a5c 116 // PM2接続確認
DiGengengen 3:2c2276646a5c 117 PM_checkconnection(2); // PM2に接続確認
DiGengengen 3:2c2276646a5c 118 transmission_xbee2pc_wait_for(WAIT_TIME_PM_REPLY); // xbeeからの受信待機
DiGengengen 3:2c2276646a5c 119
DiGengengen 3:2c2276646a5c 120 // コマンド実行
DiGengengen 3:2c2276646a5c 121 cmd_exe_wait_for(WAIT_TIME_PC_CMD/2.0f); // PCからのコマンド待機
kintoki231 0:dde37cabc667 122 }
kintoki231 0:dde37cabc667 123 }
kintoki231 0:dde37cabc667 124
DiGengengen 3:2c2276646a5c 125 /* PWM初期化 */
DiGengengen 3:2c2276646a5c 126 void pwm_init(){
kintoki231 0:dde37cabc667 127 Servo_1.period_ms(20);
DiGengengen 3:2c2276646a5c 128 Servo_2_period_ms(20);
kintoki231 0:dde37cabc667 129 Servo_3.period_ms(20);
kintoki231 0:dde37cabc667 130 Servo_4.period_ms(20);
kintoki231 0:dde37cabc667 131 Servo_5.period_ms(20);
kintoki231 0:dde37cabc667 132 Servo_6.period_ms(20);
DiGengengen 3:2c2276646a5c 133 reset_all_motors();
DiGengengen 3:2c2276646a5c 134 }
DiGengengen 3:2c2276646a5c 135
DiGengengen 5:a846e1d2e1d8 136 /* xbee */
DiGengengen 5:a846e1d2e1d8 137 // 初期化
DiGengengen 3:2c2276646a5c 138 void xbee_init(){
DiGengengen 5:a846e1d2e1d8 139 xbee_enable();
DiGengengen 5:a846e1d2e1d8 140 }
DiGengengen 5:a846e1d2e1d8 141 // xbee動作許可
DiGengengen 5:a846e1d2e1d8 142 void xbee_enable(){
DiGengengen 3:2c2276646a5c 143 xbee_reset = 1;
DiGengengen 5:a846e1d2e1d8 144 led[0] = 1;
DiGengengen 5:a846e1d2e1d8 145 }
DiGengengen 5:a846e1d2e1d8 146 // xbee動作禁止
DiGengengen 5:a846e1d2e1d8 147 void xbee_disable(){
DiGengengen 5:a846e1d2e1d8 148 xbee_reset = 0;
DiGengengen 5:a846e1d2e1d8 149 led[0] = 0;
DiGengengen 3:2c2276646a5c 150 }
DiGengengen 3:2c2276646a5c 151
DiGengengen 3:2c2276646a5c 152
DiGengengen 3:2c2276646a5c 153 /* PMとの接続確認，PMのWDT更新 */
DiGengengen 3:2c2276646a5c 154 void PM_checkconnection(int id){
DiGengengen 3:2c2276646a5c 155 if(id == 1){
DiGengengen 3:2c2276646a5c 156 xbee.putc(IWCMD_PM1_CCO);
DiGengengen 3:2c2276646a5c 157 }else if(id == 2){
DiGengengen 3:2c2276646a5c 158 xbee.putc(IWCMD_PM2_CCO);
DiGengengen 3:2c2276646a5c 159 }
DiGengengen 3:2c2276646a5c 160 }
DiGengengen 3:2c2276646a5c 161
kintoki231 0:dde37cabc667 162
DiGengengen 3:2c2276646a5c 163 /* wait_time秒間，xbeeからの通信を受信してPCに表示 */
DiGengengen 3:2c2276646a5c 164 void transmission_xbee2pc_wait_for(float wait_time){
DiGengengen 3:2c2276646a5c 165 tim.reset();
DiGengengen 3:2c2276646a5c 166 tim.start();
DiGengengen 3:2c2276646a5c 167 while(tim.read() < wait_time){
DiGengengen 3:2c2276646a5c 168 if(xbee.readable()){
DiGengengen 3:2c2276646a5c 169 pc.putc(xbee.getc());
DiGengengen 3:2c2276646a5c 170 }
DiGengengen 3:2c2276646a5c 171 }
DiGengengen 3:2c2276646a5c 172 tim.stop();
DiGengengen 3:2c2276646a5c 173 }
DiGengengen 3:2c2276646a5c 174
kintoki231 0:dde37cabc667 175
DiGengengen 3:2c2276646a5c 176 /* wait_time秒間，PCからの通信を受信してコマンド実行 */
DiGengengen 3:2c2276646a5c 177 void cmd_exe_wait_for(float wait_time){
DiGengengen 3:2c2276646a5c 178 tim.reset();
DiGengengen 3:2c2276646a5c 179 tim.start();
DiGengengen 3:2c2276646a5c 180 while(tim.read() < wait_time){
DiGengengen 3:2c2276646a5c 181 if(pc.readable()){
DiGengengen 3:2c2276646a5c 182 cmd_exe(pc.getc());
DiGengengen 3:2c2276646a5c 183 }
DiGengengen 3:2c2276646a5c 184 }
DiGengengen 3:2c2276646a5c 185 tim.stop();
DiGengengen 3:2c2276646a5c 186 }
DiGengengen 3:2c2276646a5c 187
kintoki231 0:dde37cabc667 188
DiGengengen 5:a846e1d2e1d8 189 /* センシング */
DiGengengen 5:a846e1d2e1d8 190 // バッテリー電圧読み取り
DiGengengen 5:a846e1d2e1d8 191 float read_battery_voltage(){
DiGengengen 5:a846e1d2e1d8 192 return k_vbat_offset + raw_battery_voltage.read() * 33.0f;
DiGengengen 5:a846e1d2e1d8 193 }
DiGengengen 5:a846e1d2e1d8 194
DiGengengen 5:a846e1d2e1d8 195
DiGengengen 3:2c2276646a5c 196 /*各コマンド実行内容*/
DiGengengen 3:2c2276646a5c 197 // 全てのモータを停止,初期位置に．
DiGengengen 3:2c2276646a5c 198 void reset_all_motors(){
DiGengengen 3:2c2276646a5c 199 Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 200 Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 201 Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 202 Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 203 Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 204 Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 205 xbee.putc(IWCMD_PM1_SPS);
DiGengengen 3:2c2276646a5c 206 xbee.putc(IWCMD_PM2_SPS);
DiGengengen 3:2c2276646a5c 207 }
DiGengengen 3:2c2276646a5c 208
kintoki231 0:dde37cabc667 209
DiGengengen 3:2c2276646a5c 210 /* Servo2用手動PWM */
DiGengengen 3:2c2276646a5c 211 void Servo_2_off(){
DiGengengen 3:2c2276646a5c 212 Servo_2 = 0;
DiGengengen 3:2c2276646a5c 213 }
DiGengengen 3:2c2276646a5c 214 void Servo_2_on(){
DiGengengen 3:2c2276646a5c 215 Servo_2 = 1;
DiGengengen 3:2c2276646a5c 216 s2_pwm_timo.attach(&Servo_2_off, s2_pulsewidth_curr);
DiGengengen 3:2c2276646a5c 217 }
DiGengengen 3:2c2276646a5c 218 void Servo_2_period_ms(int p_ms){
DiGengengen 3:2c2276646a5c 219 Servo_2_off();
DiGengengen 3:2c2276646a5c 220 s2_pwm_tick.attach(&Servo_2_on, (double)p_ms / 1.0e3);
DiGengengen 3:2c2276646a5c 221 }
DiGengengen 3:2c2276646a5c 222 void Servo_2_pulsewidth(double p){
DiGengengen 3:2c2276646a5c 223 s2_pulsewidth_curr = p;
DiGengengen 3:2c2276646a5c 224 }
DiGengengen 3:2c2276646a5c 225 //const int s2_division = 100; // PWM用分割数
DiGengengen 3:2c2276646a5c 226 //int s2_duty; // PWM用分割数のうちduty個分High出力
DiGengengen 3:2c2276646a5c 227 //int s2_period_us; // PWM用分割数1つあたりの周期
DiGengengen 3:2c2276646a5c 228 //
DiGengengen 3:2c2276646a5c 229 //void Servo_2_period_ms(int p_ms){
DiGengengen 3:2c2276646a5c 230 // s2_period_us = (p_ms*1000) / s2_division;
DiGengengen 3:2c2276646a5c 231 // s2_pwm_tick.attach_us(&Servo_2_pwm_interrupt, s2_period_us);
DiGengengen 3:2c2276646a5c 232 //}
DiGengengen 3:2c2276646a5c 233 //void Servo_2_pulsewidth(double pw){
DiGengengen 3:2c2276646a5c 234 // s2_duty = pw*1000000.0 / (double)s2_period_us;
DiGengengen 3:2c2276646a5c 235 //}
DiGengengen 3:2c2276646a5c 236 //void Servo_2_pwm_interrupt(){
DiGengengen 3:2c2276646a5c 237 // static int t = 0;
DiGengengen 3:2c2276646a5c 238 // if(t == s2_division){
DiGengengen 3:2c2276646a5c 239 // Servo_2 = 0;
DiGengengen 3:2c2276646a5c 240 // t = 0;
DiGengengen 3:2c2276646a5c 241 // return;
DiGengengen 3:2c2276646a5c 242 // }
DiGengengen 3:2c2276646a5c 243 // if(t == s2_division-s2_duty)
DiGengengen 3:2c2276646a5c 244 // Servo_2 = 1;
DiGengengen 3:2c2276646a5c 245 // t++;
DiGengengen 3:2c2276646a5c 246 //}
DiGengengen 3:2c2276646a5c 247
DiGengengen 3:2c2276646a5c 248
DiGengengen 3:2c2276646a5c 249 /* コマンド実行 */
DiGengengen 3:2c2276646a5c 250 void cmd_exe(char cmd){
DiGengengen 3:2c2276646a5c 251 switch(cmd){
DiGengengen 3:2c2276646a5c 252 case IWCMD_CM_SS1_PP: //スラスターサーボ1を正位置
DiGengengen 3:2c2276646a5c 253 Servo_1.pulsewidth(Servo_1_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 254 break;
DiGengengen 3:2c2276646a5c 255 case IWCMD_CM_SS1_SP: //スラスターサーボ1を零位置
DiGengengen 3:2c2276646a5c 256 Servo_1.pulsewidth(Servo_1_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 257 break;
DiGengengen 3:2c2276646a5c 258 case IWCMD_CM_SS1_NP: //スラスターサーボ1を負位置
DiGengengen 3:2c2276646a5c 259 Servo_1.pulsewidth(Servo_1_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 260 break;
DiGengengen 3:2c2276646a5c 261 case IWCMD_CM_SS2_PP: //スラスターサーボ2を正位置
DiGengengen 3:2c2276646a5c 262 Servo_2_pulsewidth(Servo_2_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 263 break;
DiGengengen 3:2c2276646a5c 264 case IWCMD_CM_SS2_SP: //スラスターサーボ2を零位置
DiGengengen 3:2c2276646a5c 265 Servo_2_pulsewidth(Servo_2_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 266 break;
DiGengengen 3:2c2276646a5c 267 case IWCMD_CM_SS2_NP: //スラスターサーボ2を負位置
DiGengengen 3:2c2276646a5c 268 Servo_2_pulsewidth(Servo_2_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 269 break;
DiGengengen 3:2c2276646a5c 270 case IWCMD_CM_SS3_PP: //スラスターサーボ3を正位置
DiGengengen 3:2c2276646a5c 271 Servo_3.pulsewidth(Servo_3_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 272 break;
DiGengengen 3:2c2276646a5c 273 case IWCMD_CM_SS3_SP: //スラスターサーボ3を零位置
DiGengengen 3:2c2276646a5c 274 Servo_3.pulsewidth(Servo_3_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 275 break;
DiGengengen 3:2c2276646a5c 276 case IWCMD_CM_SS3_NP: //スラスターサーボ3を負位置
DiGengengen 3:2c2276646a5c 277 Servo_3.pulsewidth(Servo_3_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 278 break;
DiGengengen 3:2c2276646a5c 279 case IWCMD_CM_SS4_PP: //スラスターサーボ4を正位置
DiGengengen 3:2c2276646a5c 280 Servo_4.pulsewidth(Servo_4_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 281 break;
DiGengengen 3:2c2276646a5c 282 case IWCMD_CM_SS4_SP: //スラスターサーボ4を零位置
DiGengengen 3:2c2276646a5c 283 Servo_4.pulsewidth(Servo_4_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 284 break;
DiGengengen 3:2c2276646a5c 285 case IWCMD_CM_SS4_NP: //スラスターサーボ4を負位置
DiGengengen 3:2c2276646a5c 286 Servo_4.pulsewidth(Servo_4_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 287 break;
DiGengengen 3:2c2276646a5c 288 case IWCMD_CM_SS5_PP: //スラスターサーボ5を正位置
DiGengengen 3:2c2276646a5c 289 Servo_5.pulsewidth(Servo_5_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 290 break;
DiGengengen 3:2c2276646a5c 291 case IWCMD_CM_SS5_SP: //スラスターサーボ5を零位置
DiGengengen 3:2c2276646a5c 292 Servo_5.pulsewidth(Servo_5_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 293 break;
DiGengengen 3:2c2276646a5c 294 case IWCMD_CM_SS5_NP: //スラスターサーボ5を負位置
DiGengengen 3:2c2276646a5c 295 Servo_5.pulsewidth(Servo_5_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 296 break;
DiGengengen 3:2c2276646a5c 297 case IWCMD_CM_SS6_PP: //スラスターサーボ6を正位置
DiGengengen 3:2c2276646a5c 298 Servo_6.pulsewidth(Servo_6_pulthwidth_range[2]);
DiGengengen 3:2c2276646a5c 299 break;
DiGengengen 3:2c2276646a5c 300 case IWCMD_CM_SS6_SP: //スラスターサーボ6を零位置
DiGengengen 3:2c2276646a5c 301 Servo_6.pulsewidth(Servo_6_pulthwidth_range[1]);
DiGengengen 3:2c2276646a5c 302 break;
DiGengengen 3:2c2276646a5c 303 case IWCMD_CM_SS6_NP: //スラスターサーボ6を負位置
DiGengengen 3:2c2276646a5c 304 Servo_6.pulsewidth(Servo_6_pulthwidth_range[0]);
DiGengengen 3:2c2276646a5c 305 break;
DiGengengen 3:2c2276646a5c 306 case IWCMD_CM_PM1_FR: //PM1を正転(IWCMD_PM1_SUU)
DiGengengen 3:2c2276646a5c 307 xbee.putc(IWCMD_PM1_SUU);
DiGengengen 3:2c2276646a5c 308 break;
DiGengengen 3:2c2276646a5c 309 case IWCMD_CM_PM1_ST: //PM1を停止(IWCMD_PM1_SPS)
DiGengengen 3:2c2276646a5c 310 xbee.putc(IWCMD_PM1_SPS);
DiGengengen 3:2c2276646a5c 311 break;
DiGengengen 3:2c2276646a5c 312 case IWCMD_CM_PM2_FR: //PM2を正転(IWCMD_PM2_SUU)
DiGengengen 3:2c2276646a5c 313 xbee.putc(IWCMD_PM2_SUU);
DiGengengen 3:2c2276646a5c 314 break;
DiGengengen 3:2c2276646a5c 315 case IWCMD_CM_PM2_ST: //PM2を停止(IWCMD_PM2_SPS)
DiGengengen 3:2c2276646a5c 316 xbee.putc(IWCMD_PM2_SPS);
DiGengengen 3:2c2276646a5c 317 break;
DiGengengen 3:2c2276646a5c 318 default:
DiGengengen 3:2c2276646a5c 319 reset_all_motors();
DiGengengen 3:2c2276646a5c 320 break;
kintoki231 0:dde37cabc667 321 }
DiGengengen 5:a846e1d2e1d8 322 led[2] = !led[2];
kintoki231 0:dde37cabc667 323 }