2019NHK_teamA
/
2019_A_ver6-2_fixed_phase1
ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
Diff: main.cpp
- Revision:
- 20:ac4954be1fe0
- Parent:
- 19:f17d2e585973
- Child:
- 21:123520676121
diff -r f17d2e585973 -r ac4954be1fe0 main.cpp --- a/main.cpp Tue Sep 17 01:33:16 2019 +0000 +++ b/main.cpp Wed Sep 18 03:36:25 2019 +0000 @@ -105,6 +105,7 @@ char arm_moter[1], drop_moter[1]; char fan_data[1]; char servo_data[1]; +char right_arm_data[1], left_arm_data[1]; //非常停止関連変数 char RDATA; @@ -123,26 +124,32 @@ int right_limit = 0; //後部が壁に当たっているか int back_limit = 0; +//回収機構の下限(引っ込めてるほう) +bool kaisyu_limit = 0; +//右腕の下限 +bool right_arm_lower_limit = 0; //右腕の上限 bool right_arm_upper_limit = 0; -//右腕の下限 -bool right_arm_lower_limit = 0; -//左腕の上限 -bool left_arm_upper_limit = 0; //左腕の下限 bool left_arm_lower_limit = 0; -//吐き出し機構の下限 -bool tyokudo_usiro = 0; +//左腕の上限 +bool left_arm_upper_limit = 0; //吐き出し機構の上限 -bool tyokudo_mae = 0; -//回収機構の上限 -bool kaisyu_mae = 0; -//回収機構の下限 -bool kaisyu_usiro = 0; +bool tyokudo_mae_limit = 0; +//吐き出し機構の下限 +bool tyokudo_usiro_limit = 0; + -int masked_lower_front_limit_data = 0; -int masked_lower_back_limit_data = 0; -int masked_lower_right_limit_data = 0; +int masked_lower_front_limit_data = 0xFF; +int masked_lower_back_limit_data = 0xFF; +int masked_lower_right_limit_data = 0xFF; +int masked_kaisyu_limit_data = 0xFF; +int masked_right_arm_lower_limit_data = 0xFF; +int masked_right_arm_upper_limit_data = 0xFF; +int masked_left_arm_lower_limit_data = 0xFF; +int masked_left_arm_upper_limit_data = 0xFF; +int masked_tyokudo_mae_limit_data = 0xFF; +int masked_tyokudo_usiro_limit_data = 0xFF; //関数のプロトタイプ宣言 void init(void); @@ -155,6 +162,7 @@ void wheel_reset(void); void kaisyu(int pulse); void tyokudo(int pulse); +void arm_up(void); void front(int target); void back(int target); void right(int target); @@ -171,30 +179,31 @@ void dondonkasoku(void); int main(void) { - + init(); init_send(); - + //とりあえず(後で消してね) zone = BLUE; - + while(1) { - + get_pulses(); print_pulses(); get_emergency(); read_limit(); + + //if(start_switch == 1) { + //tyokudo(arm_enc.getPulses()); + //} //青ゾーン if(zone == BLUE) { - + switch(phase) { - + //スタート位置へセット case 0: - servo_data[0] = 0x03; - i2c.write(0x30, servo_data, 1); - //リミットが洗濯物台に触れているか if(right_limit == 3) { USR_LED1 = 1; @@ -207,23 +216,32 @@ USR_LED1 = 0; } break; - + //回収 case 1: - //ここに回収動作が来るが今回は飛ばす - phase = 2; + //回収アームのリミットが押されているか + if(kaisyu_limit == 1) { + kaisyu(arm_enc.getPulses()); + USR_LED2 = 1; + } else { + USR_LED2 = 0; + } + servo_data[0] = 0x03; + i2c.write(0x30, servo_data, 1); break; - + //1秒停止 case 2: stop(); + servo_data[0] = 0x04; + i2c.write(0x30, servo_data, 1); counter.start(); if(counter.read() > 1.0f) { phase = 3; wheel_reset(); } break; - + //左移動 case 3: counter.reset(); @@ -232,7 +250,7 @@ phase = 4; } break; - + //1秒停止 case 4: stop(); @@ -242,16 +260,16 @@ wheel_reset(); } break; - + //右旋回(180°) case 5: counter.reset(); - turn_right(520); - if(x_pulse2 > 520) { + turn_right(518); + if(x_pulse2 > 518) { phase = 6; } break; - + //1秒停止 case 6: stop(); @@ -261,12 +279,12 @@ wheel_reset(); } break; - + //ちょっくら右移動 case 7: counter.reset(); right(-1000); - + if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) { true_migimae_data[0] = 0x94; true_migiusiro_data[0] = 0x10; @@ -277,7 +295,7 @@ phase = 8; } break; - + //1秒停止 case 8: stop(); @@ -287,20 +305,21 @@ wheel_reset(); } break; - + //排出 case 9: counter.reset(); + tyokudo(arm_enc.getPulses()); //ここに排出動作が来るが今回は飛ばす - phase = 10; + //phase = 10; break; - + //排出機構引っ込める case 10: //ここに排出機構引っ込める動作が来るが今回は飛ばす phase = 11; break; - + //1秒停止 case 11: stop(); @@ -310,7 +329,7 @@ wheel_reset(); } break; - + //前進 case 12: counter.reset(); @@ -319,7 +338,7 @@ phase = 13; } break; - + //1秒停止 case 13: stop(); @@ -329,7 +348,7 @@ wheel_reset(); } break; - + //右移動 case 14: counter.reset(); @@ -344,7 +363,7 @@ phase = 15; } break; - + //シーツ装填 case 15: if(start_switch == 1) { @@ -353,7 +372,7 @@ stop(); } break; - + //竿のラインまで前進 case 16: counter.reset(); @@ -362,7 +381,7 @@ phase = 17; } break; - + //1秒停止 case 17: stop(); @@ -372,8 +391,8 @@ wheel_reset(); } break; - - //掛けるところまで左移動 + + //掛けるところまで左移動 case 18: counter.reset(); left(10000); @@ -381,7 +400,7 @@ phase = 19; } break; - + //1秒停止 case 19: stop(); @@ -390,17 +409,17 @@ phase = 20; wheel_reset(); } - break; - + break; + //妨害防止の右旋回 - case 20: + case 20: counter.reset(); turn_right(300); if(x_pulse2 > 300) { phase = 21; } break; - + //1秒停止 case 21: stop(); @@ -409,19 +428,19 @@ phase = 22; wheel_reset(); } - break; - - //カウンターリセット + break; + + //アームアップ case 22: stop(); counter.reset(); - phase = 23; + arm_up(); break; - + //シーツを掛ける case 23: counter.start(); - + //5秒間ファン送風 if(counter.read() <= 4.0f) { fan_data[0] = 0xFF; @@ -445,13 +464,13 @@ phase = 24; } break; - + //終了っ!(守衛さん風) case 24: //駆動系統OFF emergency = 0; break; - + default: //駆動系統OFF emergency = 0; @@ -465,46 +484,52 @@ //通信ボーレートの設定 pc.baud(460800); - + limit_serial.baud(115200); - + start_switch.mode(PullUp); - + //非常停止関連 pic.baud(19200); pic.format(8, Serial::None, 1); pic.attach(get, Serial::RxIrq); - + x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; fan_data[0] = 0x80; servo_data[0] = 0x80; + right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; } void init_send(void) { - + init_send_data[0] = 0x80; i2c.write(0x10, init_send_data, 1); i2c.write(0x12, init_send_data, 1); i2c.write(0x14, init_send_data, 1); i2c.write(0x16, init_send_data, 1); + i2c.write(0x18, init_send_data, 1); + i2c.write(0x20, init_send_data, 1); + i2c.write(0x22, init_send_data, 1); + i2c.write(0x24, init_send_data, 1); + i2c.write(0x30, init_send_data, 1); wait(0.1); } void get(void) { - - baff = pic.getc(); - + + baff = pic.getc(); + for(; flug; flug--) RDATA = baff; - + if(baff == ':') flug = 1; } void get_pulses(void) { - + x_pulse1 = wheel_x1.getPulses(); x_pulse2 = wheel_x2.getPulses(); y_pulse1 = wheel_y1.getPulses(); @@ -513,14 +538,15 @@ void print_pulses(void) { + pc.printf("%d\n\r", tyokudo_phase); + //pc.printf("%d\n\r", arm_enc.getPulses()); //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data); - pc.printf("upper: 0x%x, low: 0x%x, f: %d, b: %d, r: %d\n\r", upper_limit_data, lower_limit_data, front_limit, back_limit, right_limit); //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]); } void get_emergency(void) { - + if(RDATA == '1') { myled = 1; emergency = 1; @@ -532,24 +558,32 @@ } void read_limit(void) { - + limit_data = limit_serial.getc(); - + //上位1bitが1ならば下のリミットのデータだと判断 if((limit_data & 0b10000000) == 0b10000000) { lower_limit_data = limit_data; - //upper_limit_data = 0b01111111; - + //上位1bitが0ならば上のリミットのデータだと判断 } else { upper_limit_data = limit_data; - //lower_limit_data = 0b11111111; } - + + //下リミット基板からのデータのマスク処理 masked_lower_front_limit_data = lower_limit_data & 0b00000011; masked_lower_back_limit_data = lower_limit_data & 0b00001100; masked_lower_right_limit_data = lower_limit_data & 0b00110000; - + masked_kaisyu_limit_data = lower_limit_data & 0b01000000; + + //上リミット基板からのデータのマスク処理 + masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001; + masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010; + masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100; + masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000; + masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000; + masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000; + //前部リミット switch(masked_lower_front_limit_data) { //両方押された @@ -568,7 +602,7 @@ front_limit = 0; break; } - + //後部リミット switch(masked_lower_back_limit_data) { //両方押された @@ -587,7 +621,7 @@ back_limit = 0; break; } - + //右部リミット switch(masked_lower_right_limit_data) { //両方押された @@ -606,68 +640,108 @@ right_limit = 0; break; } - - /* - //回収機構の上限 - if((lower_limit_data & 0b01000000) == 0b01000000) { - withdrawal_upper_limit = 1; - } else { - withdrawal_upper_limit = 0; + + //回収機構リミット + switch(masked_kaisyu_limit_data) { + //押された + case 0b00000000: + kaisyu_limit = 1; + break; + case 0b01000000: + kaisyu_limit = 0; + break; + default: + kaisyu_limit = 0; + break; } - - //回収機構の下限 - if((lower_limit_data & 0b10000000) == 0b10000000) { - withdrawal_lower_limit = 1; - } else { - withdrawal_lower_limit = 0; + + //右腕下部リミット + switch(masked_right_arm_lower_limit_data) { + //押された + case 0b00000000: + right_arm_lower_limit = 1; + break; + case 0b00000001: + right_arm_lower_limit = 0; + break; + default: + right_arm_lower_limit = 0; + break; } - - //右腕の上限 - if((upper_limit_data & 0b00000001) == 0b00000001) { - right_arm_upper_limit = 1; - } else { - right_arm_upper_limit = 0; - } - - //右腕の下限 - if((upper_limit_data & 0b00000010) == 0b00000010) { - right_arm_lower_limit = 1; - } else { - right_arm_lower_limit = 0; + + //右腕上部リミット + switch(masked_right_arm_upper_limit_data) { + //押された + case 0b00000000: + right_arm_upper_limit = 1; + break; + case 0b00000010: + right_arm_upper_limit = 0; + break; + default: + right_arm_upper_limit = 0; + break; } - - //左腕の上限 - if((upper_limit_data & 0b00000100) == 0b00000100) { - left_arm_upper_limit = 1; - } else { - left_arm_upper_limit = 0; + + //左腕下部リミット + switch(masked_left_arm_lower_limit_data) { + //押された + case 0b00000000: + left_arm_lower_limit = 1; + break; + case 0b00000100: + left_arm_lower_limit = 0; + break; + default: + left_arm_lower_limit = 0; + break; } - - //左腕の下限 - if((upper_limit_data & 0b00001000) == 0b00001000) { - left_arm_lower_limit = 1; - } else { - left_arm_lower_limit = 0; + + //左腕上部リミット + switch(masked_left_arm_upper_limit_data) { + //押された + case 0b00000000: + left_arm_upper_limit = 1; + break; + case 0b00001000: + left_arm_upper_limit = 0; + break; + default: + left_arm_upper_limit = 0; + break; } - - //吐き出し機構の上限 - if((upper_limit_data & 0b00010000) == 0b00010000) { - force_out_upper_limit = 1; - } else { - force_out_upper_limit = 0; + + //直動の前 + switch(masked_tyokudo_mae_limit_data) { + //押された + case 0b00000000: + tyokudo_mae_limit = 1; + break; + case 0b00010000: + tyokudo_mae_limit = 0; + break; + default: + tyokudo_mae_limit = 0; + break; } - - //吐き出し機構の下限 - if((upper_limit_data & 0b00100000) == 0b00100000) { - force_out_lower_limit = 1; - } else { - force_out_lower_limit = 0; + + //直動の後 + switch(masked_tyokudo_usiro_limit_data) { + //押された + case 0b00000000: + tyokudo_usiro_limit = 1; + break; + case 0b00100000: + tyokudo_usiro_limit = 0; + break; + default: + tyokudo_usiro_limit = 0; + break; } - */ } void wheel_reset(void) { - + wheel_x1.reset(); wheel_x2.reset(); wheel_y1.reset(); @@ -677,104 +751,186 @@ void kaisyu(int pulse) { switch (kaisyu_phase) { + case 0: - arm_moter[0] = 0x80; - kaisyu_phase = 0; - break; - case 1: //前進->減速 - if(pulse < 2000) { + //3000pulseまで高速前進 + if(pulse < 3000) { arm_moter[0] = 0xFF; + //kaisyu_phase = 1; + } + + //3000pulse超えたら低速前進 + else if(pulse >= 3000) { + arm_moter[0] = 0xB3; kaisyu_phase = 1; - } else { - arm_moter[0] = 0xB3; - kaisyu_phase = 2; } break; - case 2: + + case 1: //前進->停止->後進 - if(kaisyu_usiro == 1) { + //3600pulseまで低速前進 + if(pulse < 3600) { arm_moter[0] = 0xB3; - kaisyu_phase = 2; - } else { - arm_moter[0] = 0x80; + //kaisyu_phase = 2; + } + + //3600pulse超えたら停止 + else if(pulse >= 3600) { + arm_moter[0] = 0x80; + + //1秒待ってから引っ込める + counter.start(); + if(counter.read() > 1.0f) { + kaisyu_phase = 2; + } + } + + break; + + case 2: + //後進->減速 + //500pulseまで高速後進 + counter.reset(); + if(pulse > 500) { + arm_moter[0] = 0x00; + //kaisyu_phase = 3; + + } + //500pulse以下になったら低速後進 + else if(pulse <= 500) { + arm_moter[0] = 0x4C; kaisyu_phase = 3; - i2c.write(0x18, arm_moter,1); - wait(1); } break; + case 3: - //後進->減速 - if(pulse > 1600) { - arm_moter[0] = 0x00; - kaisyu_phase = 3; - } else { + //後進->停止 + //リミット押されるまで低速後進 + if(pulse <= 500) { arm_moter[0] = 0x4C; + //kaisyu_phase = 4; + } + + //リミット押されたら停止 + if(kaisyu_limit == 1) { + arm_moter[0] = 0x80; kaisyu_phase = 4; + phase = 2; } break; - case 4: - //後進->停止 - if(kaisyu_mae == 1) { - arm_moter[0] = 0x4C; - kaisyu_phase = 4; - } else { - arm_moter[0] = 0x80; - kaisyu_phase = 0; - phase = 1; - } - break; + default: + arm_moter[0] = 0x80; break; } + //回収MDへ書き込み i2c.write(0x18, arm_moter, 1); } void tyokudo(int pulse) { - - switch (tyokudo_phase) { + + switch(tyokudo_phase) { + case 0: - arm_moter[0] = 0x80; - drop_moter[0] = 0x80; - break; - case 1: //前進->減速 - arm_moter[0] = (pulse < 2000)? 0xCD:0xC0; - drop_moter[0] = (pulse < 2000)? 0xE6:0xCD; - tyokudo_phase = (pulse < 2000)? 1:2; - break; - case 2: - //前進(遅い)->停止->後進(早い) - if(tyokudo_mae == 1) { - arm_moter[0] = 0xC0; - drop_moter[0] = 0xCD; - tyokudo_phase = 2; - } else { - arm_moter[0] = 0x80; + + //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行 + if(tyokudo_mae_limit == 0) { + //2000pulseまで高速前進 + if(pulse < 2000) { + arm_moter[0] = 0xCD; + drop_moter[0] = 0xE6; + } + //2000pulse以上で低速前進 + else if(pulse >= 2000) { + arm_moter[0] = 0xC0; + drop_moter[0] = 0xCD; + } + //パルスが3600を終えたらアームのみ強制停止 + else if(pulse > 3600) { + arm_moter[0] = 0x80; + drop_moter[0] = 0xCD; + } + } + + //直動の前リミットが押されたら + else if(tyokudo_mae_limit == 1) { + //直動のみ強制停止 drop_moter[0] = 0x80; - tyokudo_phase = 3; - i2c.write(0x18, arm_moter,1); - i2c.write(0x20, drop_moter,1); - wait(1); + + //2000pulseまで高速前進 + if(pulse < 2000) { + arm_moter[0] = 0xCD; + } + //2000pulse以上で低速前進 + else if(pulse >= 2000) { + arm_moter[0] = 0xC0; + } + //パルスが3600を終えたらアームも強制停止 + else if(pulse > 3600) { + arm_moter[0] = 0x80; + tyokudo_phase = 1; + } } break; - case 3: + + case 1: //後進->減速 - arm_moter[0] = (pulse > 1600)? 0x33:0x33; - drop_moter[0] = (pulse > 1600)? 0x19:0x19; - tyokudo_phase = (pulse > 1600)? 3:4; + + //1600pulseより大きいとき高速後進 + if(pulse > 1600) { + + if(kaisyu_limit == 0) { + arm_moter[0] = 0x00; + } + //リミットが押されたら強制停止 + else if(kaisyu_limit == 1) { + arm_moter[0] = 0x80; + } + + if(tyokudo_usiro_limit == 0) { + drop_moter[0] = 0x19; + } + //リミットが押されたら強制停止 + else if(tyokudo_usiro_limit == 1) { + drop_moter[0] = 0x80; + } + } + //500pulse以下でphase2へ移行 + else if(pulse <= 500) { + tyokudo_phase = 2; + } break; - case 4: + + case 2: //後進->停止 - arm_moter[0] = (tyokudo_usiro == 1)? 0x33:0x80; - drop_moter[0] = (tyokudo_usiro == 1)? 0x19:0x80; - tyokudo_phase = (tyokudo_usiro == 1)? 4:0; + + //直動後リミットが押されるまで後進 + if(tyokudo_usiro_limit == 0) { + drop_moter[0] = 0x19; + } + //直動後リミットが押されたら停止 + else if(tyokudo_usiro_limit == 1) { + drop_moter[0] = 0x80; + } + + //リミット押されるまで低速後進 + if(kaisyu_limit == 0) { + arm_moter[0] = 0x4C; + } + else if(kaisyu_limit == 1) { + arm_moter[1] = 0x80; + tyokudo_phase = 3; + phase = 10; + } + break; + default: arm_moter[0] = 0x80; drop_moter[0] = 0x80; - tyokudo_phase = 0; break; } @@ -782,8 +938,32 @@ i2c.write(0x20, drop_moter, 1); } +void arm_up(void) { + + //両腕、上限リミットが押されてなかったら上昇 + if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) { + right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF; + } + //右腕のみリミットが押されたら左腕のみ上昇 + else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) { + right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF; + } + //左腕のみリミットが押されたら右腕のみ上昇 + else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) { + right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80; + } + //両腕、上限リミットが押されたら停止 + else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) { + right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; + phase = 23; + } + + i2c.write(0x22, right_arm_data, 1); + i2c.write(0x24, left_arm_data, 1); +} + void front(int target) { - + front_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -793,7 +973,7 @@ } void back(int target) { - + back_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -803,7 +983,7 @@ } void right(int target) { - + right_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -813,7 +993,7 @@ } void left(int target) { - + left_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -823,7 +1003,7 @@ } void turn_right(int target) { - + turn_right_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -833,7 +1013,7 @@ } void turn_left(int target) { - + turn_left_PID(target); i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); @@ -843,12 +1023,12 @@ } void stop(void) { - + true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; - true_hidariusiro_data[0] = 0x80; - + true_hidariusiro_data[0] = 0x80; + i2c.write(0x10, true_migimae_data, 1, false); i2c.write(0x12, true_migiusiro_data, 1, false); i2c.write(0x14, true_hidarimae_data, 1, false); @@ -952,7 +1132,7 @@ } void back_PID(int target) { - + //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) back_migimae.setInputLimits(-2147483648, 2147483647); back_migiusiro.setInputLimits(-2147483648, 2147483647); @@ -1143,7 +1323,7 @@ } void left_PID(int target) { - + //センサ出力値の最小、最大 left_migimae.setInputLimits(-2147483648, 2147483647); left_migiusiro.setInputLimits(-2147483648, 2147483647); @@ -1179,7 +1359,7 @@ left_hidarimae.setOutputLimits(0x7C, 0x83); left_hidariusiro.setOutputLimits(0x7C, 0x83); } - + //よくわからんやつ left_migimae.setMode(AUTO_MODE); left_migiusiro.setMode(AUTO_MODE); @@ -1301,7 +1481,7 @@ } void turn_left_PID(int target) { - + //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) turn_left_migimae.setInputLimits(-2147483648, 2147483647); turn_left_migiusiro.setInputLimits(-2147483648, 2147483647);