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@6:2e992306910a, 2020-11-30 (annotated)

Committer:
DiGengengen
Date:
Mon Nov 30 10:03:46 2020 +0000
Revision:
6:2e992306910a
Parent:
5:a846e1d2e1d8
Child:
7:68b7925d0cf4
added slow servo mode

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