青ゾーン&赤ゾーンで掛ける位置まで移動できたやつ
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /* ------------------------------------------------------------------- */ 00002 /* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic */ 00003 /* Nucleo Type: F446RE */ 00004 /* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com */ 00005 /* Actuator: RS-555*4, RS-380*2, RZ-735*2, RS-385*2, PWM_Servo(KONDO)*2 */ 00006 /* Sensor: encorder*4, limit_switch*14 */ 00007 /* ------------------------------------------------------------------- */ 00008 /* added red zone(checked) */ 00009 /* ------------------------------------------------------------------- */ 00010 #include "mbed.h" 00011 #include "math.h" 00012 #include "QEI.h" 00013 #include "PID.h" 00014 00015 //直進補正の為の前後・左右の回転差の許容値 00016 #define wheel_difference 100 00017 00018 #define RED 0 00019 #define BLUE 1 00020 00021 //PID Gain of wheels(Kp, Ti, Td, control cycle) 00022 //前進 00023 PID front_migimae(4500000.0, 0.0, 0.0, 0.001); 00024 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00025 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00026 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00027 00028 //後進 00029 PID back_migimae(4500000.0, 0.0, 0.0, 0.001); 00030 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00031 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00032 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00033 00034 //右進 00035 PID right_migimae(6000000.0, 0.0, 0.0, 0.001); 00036 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00037 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00038 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00039 00040 //左進 00041 PID left_migimae(6000000.0, 0.0, 0.0, 0.001); 00042 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00043 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00044 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00045 00046 //右旋回 00047 PID turn_right_migimae(3000000.0, 0.0, 0.0, 0.001); 00048 PID turn_right_migiusiro(3000000.0, 0.0, 0.0, 0.001); 00049 PID turn_right_hidarimae(3000000.0, 0.0, 0.0, 0.001); 00050 PID turn_right_hidariusiro(3000000.0, 0.0, 0.0, 0.001); 00051 00052 //左旋回 00053 PID turn_left_migimae(3000000.0, 0.0, 0.0, 0.001); 00054 PID turn_left_migiusiro(3000000.0, 0.0, 0.0, 0.001); 00055 PID turn_left_hidarimae(3000000.0, 0.0, 0.0, 0.001); 00056 PID turn_left_hidariusiro(3000000.0, 0.0, 0.0, 0.001); 00057 00058 //MDとの通信ポート 00059 I2C i2c(PB_9, PB_8); //SDA, SCL 00060 00061 //PCとの通信ポート 00062 Serial pc(USBTX, USBRX); //TX, RX 00063 00064 //特小モジュールとの通信ポート 00065 Serial pic(A0, A1); 00066 00067 //リミットスイッチ基板との通信ポート 00068 Serial limit_serial(PC_12, PD_2); 00069 00070 //12V停止信号ピン 00071 DigitalOut emergency(D11); 00072 00073 DigitalOut USR_LED1(PB_7); 00074 //DigitalOut USR_LED2(PC_13); 00075 DigitalOut USR_LED3(PC_2); 00076 DigitalOut USR_LED4(PC_3); 00077 DigitalOut GREEN_LED(D8); 00078 DigitalOut RED_LED(D10); 00079 00080 //遠隔非常停止ユニットLED 00081 AnalogOut myled(A2); 00082 00083 DigitalIn start_switch(PB_12); 00084 DigitalIn USR_SWITCH(PC_13); 00085 DigitalIn zone_switch(PC_10); 00086 00087 QEI wheel_x1(PA_8 , PA_6 , NC, 624); 00088 QEI wheel_x2(PB_14, PB_13, NC, 624); 00089 QEI wheel_y1(PB_1 , PB_15, NC, 624); 00090 QEI wheel_y2(PA_12, PA_11, NC, 624); 00091 QEI arm_enc(PB_5, PB_4 , NC, 624); 00092 00093 //移動後n秒停止タイマー 00094 Timer counter; 00095 00096 //エンコーダ値格納変数 00097 int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse; 00098 00099 //操作の段階変数 00100 unsigned int phase = 0; 00101 int kaisyu_phase = 0; 00102 int tyokudo_phase = 0; 00103 unsigned int start_zone = 1; 00104 bool zone = RED; 00105 00106 //i2c送信データ変数 00107 char init_send_data[1]; 00108 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; 00109 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; 00110 char arm_motor[1], drop_motor[1]; 00111 char fan_data[1]; 00112 char servo_data[1]; 00113 char right_arm_data[1], left_arm_data[1]; 00114 00115 //非常停止関連変数 00116 char RDATA; 00117 char baff; 00118 int flug = 0; 00119 00120 //リミット基板からの受信データ 00121 int limit_data = 0; 00122 int upper_limit_data = 0; 00123 int lower_limit_data = 0; 00124 00125 //各辺のスイッチが押されたかのフラグ 00126 //前部が壁に当たっているか 00127 int front_limit = 0; 00128 //右部が壁にあたあっているか 00129 int right_limit = 0; 00130 //後部が壁に当たっているか 00131 int back_limit = 0; 00132 //回収機構の下限(引っ込めてるほう) 00133 bool kaisyu_mae_limit = 0; 00134 00135 bool kaisyu_usiro_limit = 0; 00136 00137 //右腕の下限 00138 bool right_arm_lower_limit = 0; 00139 //右腕の上限 00140 bool right_arm_upper_limit = 0; 00141 //左腕の下限 00142 bool left_arm_lower_limit = 0; 00143 //左腕の上限 00144 bool left_arm_upper_limit = 0; 00145 //吐き出し機構の上限 00146 bool tyokudo_mae_limit = 0; 00147 //吐き出し機構の下限 00148 bool tyokudo_usiro_limit = 0; 00149 00150 int masked_lower_front_limit_data = 0xFF; 00151 int masked_lower_back_limit_data = 0xFF; 00152 int masked_lower_right_limit_data = 0xFF; 00153 int masked_kaisyu_mae_limit_data = 0xFF; 00154 int masked_kaisyu_usiro_limit_data = 0xFF; 00155 int masked_right_arm_lower_limit_data = 0xFF; 00156 int masked_right_arm_upper_limit_data = 0xFF; 00157 int masked_left_arm_lower_limit_data = 0xFF; 00158 int masked_left_arm_upper_limit_data = 0xFF; 00159 int masked_tyokudo_mae_limit_data = 0xFF; 00160 int masked_tyokudo_usiro_limit_data = 0xFF; 00161 00162 //関数のプロトタイプ宣言 00163 void init(void); 00164 void init_send(void); 00165 void get(void); 00166 void get_pulses(void); 00167 void print_pulses(void); 00168 void get_emergency(void); 00169 void read_limit(void); 00170 void wheel_reset(void); 00171 void kaisyu(int pulse, int next_phase); 00172 void tyokudo(int pulse, int next_phase); 00173 void arm_up(int next_phase); 00174 void front(int target); 00175 void back(int target); 00176 void right(int target); 00177 void left(int target); 00178 void turn_right(int target); 00179 void turn_left(int target); 00180 void stop(void); 00181 void all_stop(void); 00182 void front_PID(int target); 00183 void back_PID(int target); 00184 void right_PID(int target); 00185 void left_PID(int target); 00186 void turn_right_PID(int target); 00187 void turn_left_PID(int target); 00188 00189 int main(void) { 00190 00191 init(); 00192 init_send(); 00193 00194 //とりあえず(後で消してね) 00195 //zone = BLUE; 00196 //phase = 16; 00197 //phase = 23; 00198 phase = 50; 00199 00200 //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止) 00201 while(1) { 00202 if(zone_switch == 0) { 00203 zone = BLUE; 00204 } else { 00205 zone = RED; 00206 } 00207 break; 00208 } 00209 00210 while(1) { 00211 00212 get_pulses(); 00213 print_pulses(); 00214 get_emergency(); 00215 read_limit(); 00216 00217 //move_servo_with_using_onboard-switch 00218 if(USR_SWITCH == 0) { 00219 servo_data[0] = 0x03; 00220 i2c.write(0x30, servo_data, 1); 00221 } else { 00222 servo_data[0] = 0x04; 00223 i2c.write(0x30, servo_data, 1); 00224 } 00225 00226 if(start_switch == 1) { 00227 //phase = 31; 00228 right_arm_data[0] = 0xFF; 00229 left_arm_data[0] = 0xFF; 00230 i2c.write(0x22, right_arm_data, 1); 00231 i2c.write(0x24, left_arm_data, 1); 00232 wait_us(20); 00233 } else { 00234 right_arm_data[0] = 0x80; 00235 left_arm_data[0] = 0x80; 00236 i2c.write(0x22, right_arm_data, 1); 00237 i2c.write(0x24, left_arm_data, 1); 00238 wait_us(20); 00239 } 00240 00241 /* 00242 //青ゾーン 00243 if(zone == BLUE) { 00244 GREEN_LED = 1; 00245 RED_LED = 0; 00246 00247 switch(phase) { 00248 00249 //スタート位置へセット 00250 case 0: 00251 //リミットが洗濯物台に触れているか 00252 if(right_limit == 3) { 00253 USR_LED1 = 1; 00254 //スタートスイッチが押されたか 00255 if(start_switch == 1) { 00256 wheel_reset(); 00257 phase = 1; 00258 } 00259 } else { 00260 USR_LED1 = 0; 00261 } 00262 break; 00263 00264 //回収 00265 case 1: 00266 kaisyu(arm_enc.getPulses(), 2); 00267 servo_data[0] = 0x03; 00268 i2c.write(0x30, servo_data, 1); 00269 break; 00270 00271 //1秒停止 00272 case 2: 00273 stop(); 00274 servo_data[0] = 0x04; 00275 i2c.write(0x30, servo_data, 1); 00276 counter.start(); 00277 if(counter.read() > 1.0f) { 00278 phase = 3; 00279 wheel_reset(); 00280 } 00281 break; 00282 00283 //左移動 00284 case 3: 00285 counter.reset(); 00286 left(11500); 00287 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) { 00288 phase = 4; 00289 } 00290 break; 00291 00292 //1秒停止 00293 case 4: 00294 stop(); 00295 counter.start(); 00296 if(counter.read() > 1.0f) { 00297 phase = 5; 00298 wheel_reset(); 00299 } 00300 break; 00301 00302 //右旋回(180°) 00303 case 5: 00304 counter.reset(); 00305 turn_right(975); 00306 if(sum_pulse > 975) { 00307 phase = 6; 00308 } 00309 break; 00310 00311 //1秒停止 00312 case 6: 00313 stop(); 00314 counter.start(); 00315 if(counter.read() > 1.0f) { 00316 phase = 7; 00317 wheel_reset(); 00318 } 00319 break; 00320 00321 //壁に当たるまで右移動 00322 case 7: 00323 counter.reset(); 00324 00325 if(right_limit == 3) { 00326 phase = 8; 00327 } 00328 else if(right_limit != 3) { 00329 true_migimae_data[0] = 0x40; 00330 true_migiusiro_data[0] = 0xBF; 00331 true_hidarimae_data[0] = 0xBF; 00332 true_hidariusiro_data[0] = 0x40; 00333 i2c.write(0x10, true_migimae_data, 1, false); 00334 i2c.write(0x12, true_migiusiro_data, 1, false); 00335 i2c.write(0x14, true_hidarimae_data, 1, false); 00336 i2c.write(0x16, true_hidariusiro_data, 1, false); 00337 wait_us(20); 00338 } 00339 break; 00340 00341 //1秒停止 00342 case 8: 00343 stop(); 00344 counter.start(); 00345 if(counter.read() > 1.0f) { 00346 phase = 9; 00347 wheel_reset(); 00348 } 00349 break; 00350 00351 //排出 00352 case 9: 00353 counter.reset(); 00354 tyokudo(arm_enc.getPulses(), 10); 00355 break; 00356 00357 //1秒停止 00358 case 10: 00359 stop(); 00360 counter.start(); 00361 if(counter.read() > 1.0f) { 00362 phase = 11; 00363 wheel_reset(); 00364 } 00365 break; 00366 00367 //前進 00368 case 11: 00369 counter.reset(); 00370 front(5000); 00371 if((y_pulse1 > 5000) || (y_pulse2 > 5000)) { 00372 phase = 12; 00373 } 00374 break; 00375 00376 //1秒停止 00377 case 12: 00378 stop(); 00379 counter.start(); 00380 if(counter.read() > 1.0f) { 00381 phase = 13; 00382 wheel_reset(); 00383 } 00384 break; 00385 00386 //壁に当たるまで右移動 00387 case 13: 00388 counter.reset(); 00389 00390 if(right_limit == 3) { 00391 phase = 14; 00392 } 00393 else if(right_limit != 3) { 00394 true_migimae_data[0] = 0x40; 00395 true_migiusiro_data[0] = 0xBF; 00396 true_hidarimae_data[0] = 0xBF; 00397 true_hidariusiro_data[0] = 0x40; 00398 i2c.write(0x10, true_migimae_data, 1, false); 00399 i2c.write(0x12, true_migiusiro_data, 1, false); 00400 i2c.write(0x14, true_hidarimae_data, 1, false); 00401 i2c.write(0x16, true_hidariusiro_data, 1, false); 00402 wait_us(20); 00403 } 00404 break; 00405 00406 //1秒停止 00407 case 14: 00408 stop(); 00409 counter.start(); 00410 if(counter.read() > 1.0f) { 00411 phase = 15; 00412 wheel_reset(); 00413 } 00414 break; 00415 00416 //壁に当たるまで後進 00417 case 15: 00418 counter.reset(); 00419 00420 if(back_limit == 3) { 00421 phase = 16; 00422 } 00423 else if(back_limit != 3){ 00424 true_migimae_data[0] = 0x50; 00425 true_migiusiro_data[0] = 0x50; 00426 true_hidarimae_data[0] = 0x50; 00427 true_hidariusiro_data[0] = 0x50; 00428 i2c.write(0x10, true_migimae_data, 1, false); 00429 i2c.write(0x12, true_migiusiro_data, 1, false); 00430 i2c.write(0x14, true_hidarimae_data, 1, false); 00431 i2c.write(0x16, true_hidariusiro_data, 1, false); 00432 wait_us(20); 00433 } 00434 break; 00435 00436 //シーツ装填 00437 case 16: 00438 if(start_switch == 1) { 00439 wheel_reset(); 00440 phase = 17; 00441 } else { 00442 stop(); 00443 } 00444 break; 00445 00446 //竿のラインまで前進 00447 case 17: 00448 counter.reset(); 00449 front(22000); 00450 if((y_pulse1 > 22000) || (y_pulse2 > 22000)) { 00451 phase = 18; 00452 } 00453 break; 00454 00455 //1秒停止 00456 case 18: 00457 stop(); 00458 counter.start(); 00459 if(counter.read() > 1.0f) { 00460 phase = 19; 00461 wheel_reset(); 00462 } 00463 break; 00464 00465 //ちょっと左移動 00466 case 19: 00467 counter.reset(); 00468 left(500); 00469 if((x_pulse1 > 800) || (x_pulse2 > 800)) { 00470 phase = 20; 00471 } 00472 break; 00473 00474 //1秒停止 00475 case 20: 00476 stop(); 00477 counter.start(); 00478 if(counter.read() > 1.0f) { 00479 phase = 21; 00480 wheel_reset(); 00481 } 00482 break; 00483 00484 //90°右旋回 00485 case 21: 00486 counter.reset(); 00487 turn_right(465); 00488 if(sum_pulse > 465) { 00489 phase = 22; 00490 } 00491 break; 00492 00493 //1秒停止 00494 case 22: 00495 stop(); 00496 counter.start(); 00497 if(counter.read() > 1.0f) { 00498 phase = 23; 00499 wheel_reset(); 00500 } 00501 break; 00502 00503 //カウンターリセット 00504 case 23: 00505 counter.reset(); 00506 counter.start(); 00507 phase = 24; 00508 break; 00509 00510 //壁に当たるまで前進 00511 case 24: 00512 if(front_limit == 3) { 00513 counter.reset(); 00514 phase = 25; 00515 } 00516 else if(front_limit != 3){ 00517 true_migimae_data[0] = 0xC0; 00518 true_migiusiro_data[0] = 0xC0; 00519 true_hidarimae_data[0] = 0xC0; 00520 true_hidariusiro_data[0] = 0xC0; 00521 i2c.write(0x10, true_migimae_data, 1, false); 00522 i2c.write(0x12, true_migiusiro_data, 1, false); 00523 i2c.write(0x14, true_hidarimae_data, 1, false); 00524 i2c.write(0x16, true_hidariusiro_data, 1, false); 00525 wait_us(20); 00526 } 00527 break; 00528 00529 //1秒停止 00530 case 25: 00531 stop(); 00532 counter.start(); 00533 if(counter.read() > 1.0f) { 00534 phase = 26; 00535 wheel_reset(); 00536 } 00537 break; 00538 00539 //掛けるところまで後進 00540 case 26: 00541 counter.reset(); 00542 back(-10000); 00543 if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) { 00544 phase = 27; 00545 counter.start(); 00546 } 00547 break; 00548 00549 //1秒停止 00550 case 27: 00551 stop(); 00552 counter.start(); 00553 if(counter.read() > 1.0f) { 00554 phase = 28; 00555 wheel_reset(); 00556 } 00557 break; 00558 00559 //妨害防止の右旋回 00560 case 28: 00561 counter.reset(); 00562 turn_right(30); 00563 if(sum_pulse > 30) { 00564 phase = 29; 00565 } 00566 break; 00567 00568 //1秒停止 00569 case 29: 00570 stop(); 00571 counter.start(); 00572 if(counter.read() > 1.0f) { 00573 phase = 30; 00574 wheel_reset(); 00575 } 00576 break; 00577 00578 //カウンターリセット 00579 case 30: 00580 counter.reset(); 00581 counter.start(); 00582 phase = 31; 00583 break; 00584 00585 //アームアップ 00586 case 31: 00587 stop(); 00588 //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止) 00589 if(counter.read() < 3.0f) { 00590 right_arm_data[0] = 0xFF; 00591 left_arm_data[0] = 0xFF; 00592 i2c.write(0x22, right_arm_data, 1); 00593 i2c.write(0x24, left_arm_data, 1); 00594 wait_us(20); 00595 } else { 00596 arm_up(32); 00597 } 00598 break; 00599 00600 //カウンターリセット 00601 case 32: 00602 counter.reset(); 00603 phase = 33; 00604 break; 00605 00606 //シーツを掛ける 00607 case 33: 00608 counter.start(); 00609 00610 //1秒間ファン送風 00611 if(counter.read() <= 1.0f) { 00612 fan_data[0] = 0xFF; 00613 i2c.write(0x26, fan_data, 1); 00614 i2c.write(0x28, fan_data, 1); 00615 servo_data[0] = 0x04; 00616 i2c.write(0x30, servo_data, 1); 00617 } 00618 //1~3秒の間でサーボを開放 00619 else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) { 00620 fan_data[0] = 0xFF; 00621 i2c.write(0x26, fan_data, 1); 00622 i2c.write(0x28, fan_data, 1); 00623 servo_data[0] = 0x03; 00624 i2c.write(0x30, servo_data, 1); 00625 } 00626 //3秒過ぎたら終わり 00627 else if(counter.read() > 3.0f) { 00628 fan_data[0] = 0x80; 00629 i2c.write(0x26, fan_data, 1); 00630 i2c.write(0x28, fan_data, 1); 00631 servo_data[0] = 0x04; 00632 i2c.write(0x30, servo_data, 1); 00633 phase = 34; 00634 } 00635 break; 00636 00637 //終了っ!(守衛さん風) 00638 case 34: 00639 default: 00640 //駆動系統OFF 00641 emergency = 0; 00642 all_stop(); 00643 break; 00644 } 00645 } 00646 00647 //REDゾーン 00648 else if(zone == RED) { 00649 GREEN_LED = 0; 00650 RED_LED = 1; 00651 00652 switch(phase) { 00653 00654 //スタート位置へセット 00655 case 0: 00656 //リミットが洗濯物台に触れているか 00657 if(right_limit == 3) { 00658 USR_LED1 = 1; 00659 //スタートスイッチが押されたか 00660 if(start_switch == 1) { 00661 wheel_reset(); 00662 phase = 1; 00663 } 00664 } else { 00665 USR_LED1 = 0; 00666 } 00667 break; 00668 00669 //回収 00670 case 1: 00671 kaisyu(arm_enc.getPulses(), 2); 00672 servo_data[0] = 0x03; 00673 i2c.write(0x30, servo_data, 1); 00674 break; 00675 00676 //1秒停止 00677 case 2: 00678 stop(); 00679 servo_data[0] = 0x04; 00680 i2c.write(0x30, servo_data, 1); 00681 counter.start(); 00682 if(counter.read() > 1.0f) { 00683 phase = 3; 00684 wheel_reset(); 00685 } 00686 break; 00687 00688 //左移動 00689 case 3: 00690 counter.reset(); 00691 left(11500); 00692 if((x_pulse1 > 11500) || (x_pulse2 > 11500)) { 00693 phase = 4; 00694 } 00695 break; 00696 00697 //1秒停止 00698 case 4: 00699 stop(); 00700 counter.start(); 00701 if(counter.read() > 1.0f) { 00702 phase = 5; 00703 wheel_reset(); 00704 } 00705 break; 00706 00707 //右旋回(180°) 00708 case 5: 00709 counter.reset(); 00710 turn_right(975); 00711 if(sum_pulse > 975) { 00712 phase = 6; 00713 } 00714 break; 00715 00716 //1秒停止 00717 case 6: 00718 stop(); 00719 counter.start(); 00720 if(counter.read() > 1.0f) { 00721 phase = 7; 00722 wheel_reset(); 00723 } 00724 break; 00725 00726 //壁に当たるまで右移動 00727 case 7: 00728 counter.reset(); 00729 00730 if(right_limit == 3) { 00731 phase = 8; 00732 } 00733 else if(right_limit != 3) { 00734 true_migimae_data[0] = 0x40; 00735 true_migiusiro_data[0] = 0xBF; 00736 true_hidarimae_data[0] = 0xBF; 00737 true_hidariusiro_data[0] = 0x40; 00738 i2c.write(0x10, true_migimae_data, 1, false); 00739 i2c.write(0x12, true_migiusiro_data, 1, false); 00740 i2c.write(0x14, true_hidarimae_data, 1, false); 00741 i2c.write(0x16, true_hidariusiro_data, 1, false); 00742 wait_us(20); 00743 } 00744 break; 00745 00746 //1秒停止 00747 case 8: 00748 stop(); 00749 counter.start(); 00750 if(counter.read() > 1.0f) { 00751 phase = 9; 00752 wheel_reset(); 00753 } 00754 break; 00755 00756 //排出 00757 case 9: 00758 counter.reset(); 00759 tyokudo(arm_enc.getPulses(), 10); 00760 break; 00761 00762 //1秒停止 00763 case 10: 00764 stop(); 00765 counter.start(); 00766 if(counter.read() > 1.0f) { 00767 phase = 11; 00768 wheel_reset(); 00769 } 00770 break; 00771 00772 //後進 00773 case 11: 00774 counter.reset(); 00775 back(-5000); 00776 if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) { 00777 phase = 12; 00778 } 00779 break; 00780 00781 //1秒停止 00782 case 12: 00783 stop(); 00784 counter.start(); 00785 if(counter.read() > 1.0f) { 00786 phase = 13; 00787 wheel_reset(); 00788 } 00789 break; 00790 00791 //壁に当たるまで右移動 00792 case 13: 00793 counter.reset(); 00794 00795 if(right_limit == 3) { 00796 phase = 14; 00797 } 00798 else if(right_limit != 3) { 00799 true_migimae_data[0] = 0x40; 00800 true_migiusiro_data[0] = 0xBF; 00801 true_hidarimae_data[0] = 0xBF; 00802 true_hidariusiro_data[0] = 0x40; 00803 i2c.write(0x10, true_migimae_data, 1, false); 00804 i2c.write(0x12, true_migiusiro_data, 1, false); 00805 i2c.write(0x14, true_hidarimae_data, 1, false); 00806 i2c.write(0x16, true_hidariusiro_data, 1, false); 00807 wait_us(20); 00808 } 00809 break; 00810 00811 //1秒停止 00812 case 14: 00813 stop(); 00814 counter.start(); 00815 if(counter.read() > 1.0f) { 00816 phase = 15; 00817 wheel_reset(); 00818 } 00819 break; 00820 00821 //壁に当たるまで前進 00822 case 15: 00823 counter.reset(); 00824 00825 if(front_limit == 3) { 00826 phase = 16; 00827 } 00828 else if(front_limit != 3){ 00829 true_migimae_data[0] = 0xC0; 00830 true_migiusiro_data[0] = 0xC0; 00831 true_hidarimae_data[0] = 0xC0; 00832 true_hidariusiro_data[0] = 0xC0; 00833 i2c.write(0x10, true_migimae_data, 1, false); 00834 i2c.write(0x12, true_migiusiro_data, 1, false); 00835 i2c.write(0x14, true_hidarimae_data, 1, false); 00836 i2c.write(0x16, true_hidariusiro_data, 1, false); 00837 wait_us(20); 00838 } 00839 break; 00840 00841 //シーツ装填 00842 case 16: 00843 if(start_switch == 1) { 00844 wheel_reset(); 00845 phase = 17; 00846 } else { 00847 stop(); 00848 } 00849 break; 00850 00851 //竿のラインまで後進 00852 case 17: 00853 counter.reset(); 00854 back(-22000); 00855 if((y_pulse1*-1 > 22000) || (y_pulse2*-1 > 22000)) { 00856 phase = 18; 00857 } 00858 break; 00859 00860 //1秒停止 00861 case 18: 00862 stop(); 00863 counter.start(); 00864 if(counter.read() > 1.0f) { 00865 phase = 19; 00866 wheel_reset(); 00867 } 00868 break; 00869 00870 //ちょっと左移動 00871 case 19: 00872 counter.reset(); 00873 left(500); 00874 if((x_pulse1 > 500) || (x_pulse2 > 500)) { 00875 phase = 20; 00876 } 00877 break; 00878 00879 //1秒停止 00880 case 20: 00881 stop(); 00882 counter.start(); 00883 if(counter.read() > 1.0f) { 00884 phase = 21; 00885 wheel_reset(); 00886 } 00887 break; 00888 00889 //90°左旋回 00890 case 21: 00891 counter.reset(); 00892 turn_left(465); 00893 if(sum_pulse > 465) { 00894 phase = 22; 00895 } 00896 break; 00897 00898 //1秒停止 00899 case 22: 00900 stop(); 00901 counter.start(); 00902 if(counter.read() > 1.0f) { 00903 phase = 23; 00904 wheel_reset(); 00905 } 00906 break; 00907 00908 //カウンターリセット 00909 case 23: 00910 counter.reset(); 00911 counter.start(); 00912 phase = 24; 00913 break; 00914 00915 //壁に当たるまで後進 00916 case 24: 00917 if(back_limit == 3) { 00918 counter.reset(); 00919 phase = 25; 00920 } 00921 else if(back_limit != 3){ 00922 true_migimae_data[0] = 0x50; 00923 true_migiusiro_data[0] = 0x50; 00924 true_hidarimae_data[0] = 0x50; 00925 true_hidariusiro_data[0] = 0x50; 00926 i2c.write(0x10, true_migimae_data, 1, false); 00927 i2c.write(0x12, true_migiusiro_data, 1, false); 00928 i2c.write(0x14, true_hidarimae_data, 1, false); 00929 i2c.write(0x16, true_hidariusiro_data, 1, false); 00930 wait_us(20); 00931 } 00932 break; 00933 00934 //1秒停止 00935 case 25: 00936 stop(); 00937 counter.start(); 00938 if(counter.read() > 1.0f) { 00939 phase = 26; 00940 wheel_reset(); 00941 } 00942 break; 00943 00944 //掛けるところまで前進 00945 case 26: 00946 counter.reset(); 00947 front(10000); 00948 if((y_pulse1 > 10000) || (y_pulse2 > 10000)) { 00949 phase = 27; 00950 counter.start(); 00951 } 00952 break; 00953 00954 //1秒停止 00955 case 27: 00956 stop(); 00957 counter.start(); 00958 if(counter.read() > 1.0f) { 00959 phase = 28; 00960 wheel_reset(); 00961 } 00962 break; 00963 00964 //妨害防止の左旋回 00965 case 28: 00966 counter.reset(); 00967 turn_left(30); 00968 if(sum_pulse > 30) { 00969 phase = 29; 00970 } 00971 break; 00972 00973 //1秒停止 00974 case 29: 00975 stop(); 00976 counter.start(); 00977 if(counter.read() > 1.0f) { 00978 phase = 30; 00979 wheel_reset(); 00980 } 00981 break; 00982 00983 //カウンターリセット 00984 case 30: 00985 counter.reset(); 00986 counter.start(); 00987 phase = 31; 00988 break; 00989 00990 //アームアップ 00991 case 31: 00992 stop(); 00993 //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止) 00994 if(counter.read() < 3.0f) { 00995 right_arm_data[0] = 0xFF; 00996 left_arm_data[0] = 0xFF; 00997 i2c.write(0x22, right_arm_data, 1); 00998 i2c.write(0x24, left_arm_data, 1); 00999 wait_us(20); 01000 } else { 01001 arm_up(32); 01002 } 01003 break; 01004 01005 //カウンターリセット 01006 case 32: 01007 counter.reset(); 01008 phase = 33; 01009 break; 01010 01011 //シーツを掛ける 01012 case 33: 01013 counter.start(); 01014 01015 //1秒間ファン送風 01016 if(counter.read() <= 1.0f) { 01017 fan_data[0] = 0xFF; 01018 i2c.write(0x26, fan_data, 1); 01019 i2c.write(0x28, fan_data, 1); 01020 servo_data[0] = 0x04; 01021 i2c.write(0x30, servo_data, 1); 01022 } 01023 //1~3秒の間でサーボを開放 01024 else if((counter.read() > 1.0f) && (counter.read() <= 3.0f)) { 01025 fan_data[0] = 0xFF; 01026 i2c.write(0x26, fan_data, 1); 01027 i2c.write(0x28, fan_data, 1); 01028 servo_data[0] = 0x03; 01029 i2c.write(0x30, servo_data, 1); 01030 } 01031 //3秒過ぎたら終わり 01032 else if(counter.read() > 3.0f) { 01033 fan_data[0] = 0x80; 01034 i2c.write(0x26, fan_data, 1); 01035 i2c.write(0x28, fan_data, 1); 01036 servo_data[0] = 0x04; 01037 i2c.write(0x30, servo_data, 1); 01038 phase = 34; 01039 } 01040 break; 01041 01042 //終了っ!(守衛さん風) 01043 case 34: 01044 default: 01045 //駆動系統OFF 01046 emergency = 0; 01047 all_stop(); 01048 break; 01049 } 01050 } 01051 */ 01052 } 01053 } 01054 01055 void init(void) { 01056 01057 //通信ボーレートの設定 01058 pc.baud(460800); 01059 01060 limit_serial.baud(115200); 01061 01062 start_switch.mode(PullUp); 01063 zone_switch.mode(PullDown); 01064 01065 //非常停止関連 01066 pic.baud(19200); 01067 pic.format(8, Serial::None, 1); 01068 pic.attach(get, Serial::RxIrq); 01069 01070 x_pulse1 = 0; x_pulse2 = 0; y_pulse1 = 0; y_pulse2 = 0; sum_pulse = 0; 01071 migimae_data[0] = 0x80; migiusiro_data[0] = 0x80; hidarimae_data[0] = 0x80; hidariusiro_data[0] = 0x80; 01072 true_migimae_data[0] = 0x80; true_migiusiro_data[0] = 0x80; true_hidarimae_data[0] = 0x80; true_hidariusiro_data[0] = 0x80; 01073 fan_data[0] = 0x80; 01074 servo_data[0] = 0x80; 01075 arm_motor[0] = 0x80; drop_motor[0] = 0x80; 01076 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; 01077 } 01078 01079 void init_send(void) { 01080 01081 init_send_data[0] = 0x80; 01082 i2c.write(0x10, init_send_data, 1); 01083 i2c.write(0x12, init_send_data, 1); 01084 i2c.write(0x14, init_send_data, 1); 01085 i2c.write(0x16, init_send_data, 1); 01086 i2c.write(0x18, init_send_data, 1); 01087 i2c.write(0x20, init_send_data, 1); 01088 i2c.write(0x22, init_send_data, 1); 01089 i2c.write(0x24, init_send_data, 1); 01090 i2c.write(0x30, init_send_data, 1); 01091 wait(0.1); 01092 } 01093 01094 void get(void) { 01095 01096 baff = pic.getc(); 01097 01098 for(; flug; flug--) 01099 RDATA = baff; 01100 01101 if(baff == ':') 01102 flug = 1; 01103 } 01104 01105 void get_pulses(void) { 01106 01107 x_pulse1 = wheel_x1.getPulses(); 01108 x_pulse2 = wheel_x2.getPulses(); 01109 y_pulse1 = wheel_y1.getPulses(); 01110 y_pulse2 = wheel_y2.getPulses(); 01111 sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4; 01112 } 01113 01114 void print_pulses(void) { 01115 01116 pc.printf("X1: %d, X2: %d, Y1: %d, Y2: %d, sum: %d\n\r", abs(x_pulse1), x_pulse2, abs(y_pulse1), y_pulse2, sum_pulse); 01117 //pc.printf("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase); 01118 //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]); 01119 //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data); 01120 //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); 01121 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0], phase); 01122 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x, phase: %d\n\r", migimae_data[0], migiusiro_data[0], hidarimae_data[0], hidariusiro_data[0], phase); 01123 } 01124 01125 void get_emergency(void) { 01126 01127 if(RDATA == '1') { 01128 myled = 1; 01129 emergency = 1; 01130 } 01131 else if(RDATA == '9'){ 01132 myled = 0.2; 01133 emergency = 0; 01134 } 01135 } 01136 01137 void read_limit(void) { 01138 01139 limit_data = limit_serial.getc(); 01140 01141 //上位1bitが1ならば下のリミットのデータだと判断 01142 if((limit_data & 0b10000000) == 0b10000000) { 01143 lower_limit_data = limit_data; 01144 01145 //上位1bitが0ならば上のリミットのデータだと判断 01146 } else { 01147 upper_limit_data = limit_data; 01148 } 01149 01150 //下リミット基板からのデータのマスク処理 01151 masked_lower_front_limit_data = lower_limit_data & 0b00000011; 01152 masked_lower_back_limit_data = lower_limit_data & 0b00001100; 01153 masked_lower_right_limit_data = lower_limit_data & 0b00110000; 01154 masked_kaisyu_mae_limit_data = lower_limit_data & 0b01000000; 01155 01156 //上リミット基板からのデータのマスク処理 01157 //masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001; 01158 masked_kaisyu_usiro_limit_data = upper_limit_data & 0b00000001; 01159 masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010; 01160 masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100; 01161 masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000; 01162 masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000; 01163 masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000; 01164 01165 //前部リミット 01166 switch(masked_lower_front_limit_data) { 01167 //両方押された 01168 case 0x00: 01169 front_limit = 3; 01170 break; 01171 //右が押された 01172 case 0b00000010: 01173 front_limit = 1; 01174 break; 01175 //左が押された 01176 case 0b00000001: 01177 front_limit = 2; 01178 break; 01179 default: 01180 front_limit = 0; 01181 break; 01182 } 01183 01184 //後部リミット 01185 switch(masked_lower_back_limit_data) { 01186 //両方押された 01187 case 0x00: 01188 back_limit = 3; 01189 break; 01190 //右が押された 01191 case 0b00001000: 01192 back_limit = 1; 01193 break; 01194 //左が押された 01195 case 0b00000100: 01196 back_limit = 2; 01197 break; 01198 default: 01199 back_limit = 0; 01200 break; 01201 } 01202 01203 //右部リミット 01204 switch(masked_lower_right_limit_data) { 01205 //両方押された 01206 case 0x00: 01207 right_limit = 3; 01208 break; 01209 //右が押された 01210 case 0b00100000: 01211 right_limit = 1; 01212 break; 01213 //左が押された 01214 case 0b00010000: 01215 right_limit = 2; 01216 break; 01217 default: 01218 right_limit = 0; 01219 break; 01220 } 01221 01222 //回収機構リミット 01223 switch(masked_kaisyu_mae_limit_data) { 01224 //押された 01225 case 0b00000000: 01226 kaisyu_mae_limit = 1; 01227 break; 01228 case 0b01000000: 01229 kaisyu_mae_limit = 0; 01230 break; 01231 default: 01232 kaisyu_mae_limit = 0; 01233 break; 01234 } 01235 01236 //右腕下部リミット 01237 /* 01238 switch(masked_right_arm_lower_limit_data) { 01239 //押された 01240 case 0b00000000: 01241 right_arm_lower_limit = 1; 01242 break; 01243 case 0b00000001: 01244 right_arm_lower_limit = 0; 01245 break; 01246 default: 01247 right_arm_lower_limit = 0; 01248 break; 01249 } 01250 */ 01251 01252 //回収後リミット 01253 switch(masked_kaisyu_usiro_limit_data) { 01254 case 0b00000000: 01255 kaisyu_usiro_limit = 1; 01256 break; 01257 case 0b00000001: 01258 kaisyu_usiro_limit = 0; 01259 break; 01260 default: 01261 kaisyu_usiro_limit = 0; 01262 break; 01263 } 01264 01265 //右腕上部リミット 01266 switch(masked_right_arm_upper_limit_data) { 01267 //押された 01268 case 0b00000000: 01269 right_arm_upper_limit = 1; 01270 break; 01271 case 0b00000010: 01272 right_arm_upper_limit = 0; 01273 break; 01274 default: 01275 right_arm_upper_limit = 0; 01276 break; 01277 } 01278 01279 //左腕下部リミット 01280 switch(masked_left_arm_lower_limit_data) { 01281 //押された 01282 case 0b00000000: 01283 left_arm_lower_limit = 1; 01284 break; 01285 case 0b00000100: 01286 left_arm_lower_limit = 0; 01287 break; 01288 default: 01289 left_arm_lower_limit = 0; 01290 break; 01291 } 01292 01293 //左腕上部リミット 01294 switch(masked_left_arm_upper_limit_data) { 01295 //押された 01296 case 0b00000000: 01297 left_arm_upper_limit = 1; 01298 break; 01299 case 0b00001000: 01300 left_arm_upper_limit = 0; 01301 break; 01302 default: 01303 left_arm_upper_limit = 0; 01304 break; 01305 } 01306 01307 //直動の前 01308 switch(masked_tyokudo_mae_limit_data) { 01309 //押された 01310 case 0b00000000: 01311 tyokudo_mae_limit = 1; 01312 break; 01313 case 0b00010000: 01314 tyokudo_mae_limit = 0; 01315 break; 01316 default: 01317 tyokudo_mae_limit = 0; 01318 break; 01319 } 01320 01321 //直動の後 01322 switch(masked_tyokudo_usiro_limit_data) { 01323 //押された 01324 case 0b00000000: 01325 tyokudo_usiro_limit = 1; 01326 break; 01327 case 0b00100000: 01328 tyokudo_usiro_limit = 0; 01329 break; 01330 default: 01331 tyokudo_usiro_limit = 0; 01332 break; 01333 } 01334 } 01335 01336 void wheel_reset(void) { 01337 01338 wheel_x1.reset(); 01339 wheel_x2.reset(); 01340 wheel_y1.reset(); 01341 wheel_y2.reset(); 01342 } 01343 01344 void kaisyu(int pulse, int next_phase) { 01345 01346 switch (kaisyu_phase) { 01347 01348 case 0: 01349 //前進->減速 01350 //3000pulseまで高速前進 01351 if(pulse < 3000) { 01352 arm_motor[0] = 0xFF; 01353 //kaisyu_phase = 1; 01354 } 01355 01356 //3000pulse超えたら低速前進 01357 else if(pulse >= 3000) { 01358 arm_motor[0] = 0xB3; 01359 kaisyu_phase = 1; 01360 } 01361 break; 01362 01363 case 1: 01364 USR_LED3 = 1; 01365 //前進->停止->後進 01366 //3600pulseまで低速前進 01367 if(pulse < 3600) { 01368 arm_motor[0] = 0xB3; 01369 //kaisyu_phase = 2; 01370 } 01371 01372 //3600pulse超えたら停止 01373 else if(pulse >= 3600) { 01374 arm_motor[0] = 0x80; 01375 01376 //1秒待ってから引っ込める 01377 counter.start(); 01378 if(counter.read() > 1.0f) { 01379 kaisyu_phase = 2; 01380 } 01381 } 01382 //後ろのリミットが押されたら強制停止 01383 if(kaisyu_usiro_limit == 1) { 01384 arm_motor[0] = 0x80; 01385 } 01386 break; 01387 01388 case 2: 01389 //後進->減速 01390 //500pulseまで高速後進 01391 counter.reset(); 01392 if(pulse > 500) { 01393 arm_motor[0] = 0x00; 01394 //kaisyu_phase = 3; 01395 01396 } 01397 //500pulse以下になったら低速後進 01398 else if(pulse <= 500) { 01399 arm_motor[0] = 0x4C; 01400 kaisyu_phase = 3; 01401 } 01402 break; 01403 01404 case 3: 01405 //後進->停止 01406 //リミット押されるまで低速後進 01407 if(pulse <= 500) { 01408 arm_motor[0] = 0x4C; 01409 //kaisyu_phase = 4; 01410 } 01411 01412 //リミット押されたら停止 01413 if(kaisyu_mae_limit == 1) { 01414 arm_motor[0] = 0x80; 01415 kaisyu_phase = 4; 01416 phase = next_phase; 01417 } 01418 break; 01419 01420 default: 01421 arm_motor[0] = 0x80; 01422 break; 01423 } 01424 01425 //回収MDへ書き込み 01426 i2c.write(0x18, arm_motor, 1); 01427 } 01428 01429 void tyokudo(int pulse, int next_phase) { 01430 01431 switch(tyokudo_phase) { 01432 01433 case 0: 01434 //前進->減速 01435 01436 /* エンコーダー読まずにリミットだけ(修正必須) */ 01437 //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行 01438 if(tyokudo_mae_limit == 0) { 01439 //2000pulseまで高速前進 01440 if(pulse < 2000) { 01441 arm_motor[0] = 0xC0; 01442 drop_motor[0] = 0xE6; 01443 } 01444 //2000pulse以上で低速前進 01445 else if(pulse >= 2000) { 01446 arm_motor[0] = 0xC0; 01447 drop_motor[0] = 0xE6; 01448 } 01449 //パルスが3600を終えたらアームのみ強制停止 01450 else if(pulse > 3600) { 01451 arm_motor[0] = 0x80; 01452 drop_motor[0] = 0xE6; 01453 } 01454 01455 //後ろのリミットが押されたら強制停止 01456 if(kaisyu_usiro_limit == 1) { 01457 arm_motor[0] = 0x80; 01458 } 01459 } 01460 01461 //直動の前リミットが押されたら 01462 else if(tyokudo_mae_limit == 1) { 01463 //高速後進 01464 arm_motor[0] = 0x40; 01465 drop_motor[0] = 0x00; 01466 tyokudo_phase = 1; 01467 } 01468 break; 01469 01470 case 1: 01471 //後進->減速 01472 //リミットが押されたら強制停止 01473 if(tyokudo_usiro_limit == 1) { 01474 arm_motor[0] = 0x80; 01475 drop_motor[0] = 0x80; 01476 tyokudo_phase = 2; 01477 phase = next_phase; 01478 } 01479 break; 01480 01481 default: 01482 arm_motor[0] = 0x80; 01483 drop_motor[0] = 0x80; 01484 break; 01485 } 01486 01487 i2c.write(0x18, arm_motor, 1); 01488 i2c.write(0x20, drop_motor, 1); 01489 } 01490 01491 void arm_up(int next_phase) { 01492 01493 //両腕、上限リミットが押されてなかったら上昇 01494 if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) { 01495 right_arm_data[0] = 0xFF; left_arm_data[0] = 0xFF; 01496 } 01497 //右腕のみリミットが押されたら左腕のみ上昇 01498 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) { 01499 right_arm_data[0] = 0x80; left_arm_data[0] = 0xFF; 01500 } 01501 //左腕のみリミットが押されたら右腕のみ上昇 01502 else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) { 01503 right_arm_data[0] = 0xFF; left_arm_data[0] = 0x80; 01504 } 01505 //両腕、上限リミットが押されたら停止 01506 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) { 01507 right_arm_data[0] = 0x80; left_arm_data[0] = 0x80; 01508 phase = next_phase; 01509 } 01510 01511 i2c.write(0x22, right_arm_data, 1); 01512 i2c.write(0x24, left_arm_data, 1); 01513 wait_us(20); 01514 } 01515 01516 void front(int target) { 01517 01518 front_PID(target); 01519 i2c.write(0x10, true_migimae_data, 1, false); 01520 i2c.write(0x12, true_migiusiro_data, 1, false); 01521 i2c.write(0x14, true_hidarimae_data, 1, false); 01522 i2c.write(0x16, true_hidariusiro_data, 1, false); 01523 wait_us(20); 01524 } 01525 01526 void back(int target) { 01527 01528 back_PID(target); 01529 i2c.write(0x10, true_migimae_data, 1, false); 01530 i2c.write(0x12, true_migiusiro_data, 1, false); 01531 i2c.write(0x14, true_hidarimae_data, 1, false); 01532 i2c.write(0x16, true_hidariusiro_data, 1, false); 01533 wait_us(20); 01534 } 01535 01536 void right(int target) { 01537 01538 right_PID(target); 01539 i2c.write(0x10, true_migimae_data, 1, false); 01540 i2c.write(0x12, true_migiusiro_data, 1, false); 01541 i2c.write(0x14, true_hidarimae_data, 1, false); 01542 i2c.write(0x16, true_hidariusiro_data, 1, false); 01543 wait_us(20); 01544 } 01545 01546 void left(int target) { 01547 01548 left_PID(target); 01549 i2c.write(0x10, true_migimae_data, 1, false); 01550 i2c.write(0x12, true_migiusiro_data, 1, false); 01551 i2c.write(0x14, true_hidarimae_data, 1, false); 01552 i2c.write(0x16, true_hidariusiro_data, 1, false); 01553 wait_us(20); 01554 } 01555 01556 void turn_right(int target) { 01557 01558 turn_right_PID(target); 01559 i2c.write(0x10, true_migimae_data, 1, false); 01560 i2c.write(0x12, true_migiusiro_data, 1, false); 01561 i2c.write(0x14, true_hidarimae_data, 1, false); 01562 i2c.write(0x16, true_hidariusiro_data, 1, false); 01563 wait_us(20); 01564 } 01565 01566 void turn_left(int target) { 01567 01568 turn_left_PID(target); 01569 i2c.write(0x10, true_migimae_data, 1, false); 01570 i2c.write(0x12, true_migiusiro_data, 1, false); 01571 i2c.write(0x14, true_hidarimae_data, 1, false); 01572 i2c.write(0x16, true_hidariusiro_data, 1, false); 01573 wait_us(20); 01574 } 01575 01576 void stop(void) { 01577 01578 true_migimae_data[0] = 0x80; 01579 true_migiusiro_data[0] = 0x80; 01580 true_hidarimae_data[0] = 0x80; 01581 true_hidariusiro_data[0] = 0x80; 01582 01583 i2c.write(0x10, true_migimae_data, 1, false); 01584 i2c.write(0x12, true_migiusiro_data, 1, false); 01585 i2c.write(0x14, true_hidarimae_data, 1, false); 01586 i2c.write(0x16, true_hidariusiro_data, 1, false); 01587 wait_us(20); 01588 } 01589 01590 void all_stop(void) { 01591 01592 true_migimae_data[0] = 0x80; 01593 true_migiusiro_data[0] = 0x80; 01594 true_hidarimae_data[0] = 0x80; 01595 true_hidariusiro_data[0] = 0x80; 01596 arm_motor[0] = 0x80; 01597 drop_motor[0] = 0x80; 01598 right_arm_data[0] = 0x80; 01599 left_arm_data[0] = 0x80; 01600 fan_data[0] = 0x80; 01601 servo_data[0] = 0x04; 01602 01603 i2c.write(0x10, true_migimae_data, 1, false); 01604 i2c.write(0x12, true_migiusiro_data, 1, false); 01605 i2c.write(0x14, true_hidarimae_data, 1, false); 01606 i2c.write(0x16, true_hidariusiro_data, 1, false); 01607 i2c.write(0x18, arm_motor, 1); 01608 i2c.write(0x20, drop_motor, 1); 01609 i2c.write(0x22, right_arm_data, 1); 01610 i2c.write(0x24, left_arm_data, 1); 01611 i2c.write(0x26, fan_data, 1); 01612 i2c.write(0x28, fan_data, 1); 01613 i2c.write(0x30, servo_data, 1); 01614 wait_us(20); 01615 } 01616 01617 void front_PID(int target) { 01618 01619 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01620 front_migimae.setInputLimits(-2147483648, 2147483647); 01621 front_migiusiro.setInputLimits(-2147483648, 2147483647); 01622 front_hidarimae.setInputLimits(-2147483648, 2147483647); 01623 front_hidariusiro.setInputLimits(-2147483648, 2147483647); 01624 01625 //制御量の最小、最大 01626 //正転(目標に達してない) 01627 if((y_pulse1 < target) && (y_pulse2 < target)) { 01628 front_migimae.setOutputLimits(0x84, 0xF5); 01629 front_migiusiro.setOutputLimits(0x84, 0xF5); 01630 front_hidarimae.setOutputLimits(0x84, 0xFF); 01631 front_hidariusiro.setOutputLimits(0x84, 0xFF); 01632 } 01633 //停止(目標より行き過ぎ) 01634 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01635 front_migimae.setOutputLimits(0x7C, 0x83); 01636 front_migiusiro.setOutputLimits(0x7C, 0x83); 01637 front_hidarimae.setOutputLimits(0x7C, 0x83); 01638 front_hidariusiro.setOutputLimits(0x7C, 0x83); 01639 } 01640 01641 //よくわからんやつ 01642 front_migimae.setMode(AUTO_MODE); 01643 front_migiusiro.setMode(AUTO_MODE); 01644 front_hidarimae.setMode(AUTO_MODE); 01645 front_hidariusiro.setMode(AUTO_MODE); 01646 01647 //目標値 01648 front_migimae.setSetPoint(target); 01649 front_migiusiro.setSetPoint(target); 01650 front_hidarimae.setSetPoint(target); 01651 front_hidariusiro.setSetPoint(target); 01652 01653 //センサ出力 01654 front_migimae.setProcessValue(y_pulse1); 01655 front_migiusiro.setProcessValue(y_pulse1); 01656 front_hidarimae.setProcessValue(y_pulse2); 01657 front_hidariusiro.setProcessValue(y_pulse2); 01658 01659 //制御量(計算結果) 01660 migimae_data[0] = front_migimae.compute(); 01661 migiusiro_data[0] = front_migiusiro.compute(); 01662 hidarimae_data[0] = front_hidarimae.compute(); 01663 hidariusiro_data[0] = front_hidariusiro.compute(); 01664 01665 //制御量をPWM値に変換 01666 //正転(目標に達してない) 01667 if((y_pulse1 < target) && (y_pulse2 < target)) { 01668 true_migimae_data[0] = migimae_data[0]; 01669 true_migiusiro_data[0] = migiusiro_data[0]; 01670 true_hidarimae_data[0] = hidarimae_data[0]; 01671 true_hidariusiro_data[0] = hidariusiro_data[0]; 01672 } 01673 //停止(目標より行き過ぎ) 01674 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01675 true_migimae_data[0] = 0x80; 01676 true_migiusiro_data[0] = 0x80; 01677 true_hidarimae_data[0] = 0x80; 01678 true_hidariusiro_data[0] = 0x80; 01679 } 01680 } 01681 01682 void back_PID(int target) { 01683 01684 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01685 back_migimae.setInputLimits(-2147483648, 2147483647); 01686 back_migiusiro.setInputLimits(-2147483648, 2147483647); 01687 back_hidarimae.setInputLimits(-2147483648, 2147483647); 01688 back_hidariusiro.setInputLimits(-2147483648, 2147483647); 01689 01690 //制御量の最小、最大 01691 //逆転(目標に達してない) 01692 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01693 back_migimae.setOutputLimits(0x00, 0x7B); 01694 back_migiusiro.setOutputLimits(0x00, 0x7B); 01695 back_hidarimae.setOutputLimits(0x00, 0x70); 01696 back_hidariusiro.setOutputLimits(0x00, 0x70); 01697 //back_hidarimae.setOutputLimits(0x00, 0x7B); 01698 //back_hidariusiro.setOutputLimits(0x00, 0x7B); 01699 } 01700 //停止(目標より行き過ぎ) 01701 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01702 back_migimae.setOutputLimits(0x7C, 0x83); 01703 back_migiusiro.setOutputLimits(0x7C, 0x83); 01704 back_hidarimae.setOutputLimits(0x7C, 0x83); 01705 back_hidariusiro.setOutputLimits(0x7C, 0x83); 01706 } 01707 01708 //よくわからんやつ 01709 back_migimae.setMode(AUTO_MODE); 01710 back_migiusiro.setMode(AUTO_MODE); 01711 back_hidarimae.setMode(AUTO_MODE); 01712 back_hidariusiro.setMode(AUTO_MODE); 01713 01714 //目標値 01715 back_migimae.setSetPoint(target*-1); 01716 back_migiusiro.setSetPoint(target*-1); 01717 back_hidarimae.setSetPoint(target*-1); 01718 back_hidariusiro.setSetPoint(target*-1); 01719 01720 //センサ出力 01721 back_migimae.setProcessValue(y_pulse1*-1); 01722 back_migiusiro.setProcessValue(y_pulse1*-1); 01723 back_hidarimae.setProcessValue(y_pulse2*-1); 01724 back_hidariusiro.setProcessValue(y_pulse2*-1); 01725 01726 //制御量(計算結果) 01727 migimae_data[0] = back_migimae.compute(); 01728 migiusiro_data[0] = back_migiusiro.compute(); 01729 hidarimae_data[0] = back_hidarimae.compute(); 01730 hidariusiro_data[0] = back_hidariusiro.compute(); 01731 01732 //制御量をPWM値に変換 01733 //逆転(目標に達してない) 01734 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01735 true_migimae_data[0] = 0x7B - migimae_data[0]; 01736 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01737 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01738 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01739 } 01740 //停止(目標より行き過ぎ) 01741 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01742 true_migimae_data[0] = 0x80; 01743 true_migiusiro_data[0] = 0x80; 01744 true_hidarimae_data[0] = 0x80; 01745 true_hidariusiro_data[0] = 0x80; 01746 } 01747 } 01748 01749 void right_PID(int target) { 01750 01751 //センサ出力値の最小、最大 01752 right_migimae.setInputLimits(-2147483648, 2147483647); 01753 right_migiusiro.setInputLimits(-2147483648, 2147483647); 01754 right_hidarimae.setInputLimits(-2147483648, 2147483647); 01755 right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01756 01757 //制御量の最小、最大 01758 //右進(目標まで達していない) 01759 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01760 right_migimae.setOutputLimits(0x6A, 0x6C); 01761 //right_migimae.setOutputLimits(0x7A, 0x7B); 01762 right_migiusiro.setOutputLimits(0xFE, 0xFF); 01763 right_hidarimae.setOutputLimits(0xEF, 0xF0); 01764 //right_hidarimae.setOutputLimits(0xFE, 0xFF); 01765 right_hidariusiro.setOutputLimits(0x7A, 0x7B); 01766 01767 } 01768 //停止(目標より行き過ぎ) 01769 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01770 right_migimae.setOutputLimits(0x7C, 0x83); 01771 right_migiusiro.setOutputLimits(0x7C, 0x83); 01772 right_hidarimae.setOutputLimits(0x7C, 0x83); 01773 right_hidariusiro.setOutputLimits(0x7C, 0x83); 01774 } 01775 01776 //よくわからんやつ 01777 right_migimae.setMode(AUTO_MODE); 01778 right_migiusiro.setMode(AUTO_MODE); 01779 right_hidarimae.setMode(AUTO_MODE); 01780 right_hidariusiro.setMode(AUTO_MODE); 01781 01782 //目標値 01783 right_migimae.setSetPoint(target*-1); 01784 right_migiusiro.setSetPoint(target*-1); 01785 right_hidarimae.setSetPoint(target*-1); 01786 right_hidariusiro.setSetPoint(target*-1); 01787 01788 //センサ出力 01789 right_migimae.setProcessValue(x_pulse1*-1); 01790 right_migiusiro.setProcessValue(x_pulse2*-1); 01791 right_hidarimae.setProcessValue(x_pulse1*-1); 01792 right_hidariusiro.setProcessValue(x_pulse2*-1); 01793 01794 //制御量(計算結果) 01795 migimae_data[0] = right_migimae.compute(); 01796 migiusiro_data[0] = right_migiusiro.compute(); 01797 hidarimae_data[0] = right_hidarimae.compute(); 01798 hidariusiro_data[0] = right_hidariusiro.compute(); 01799 01800 //制御量をPWM値に変換 01801 //右進(目標まで達していない) 01802 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01803 true_migimae_data[0] = 0x7B - migimae_data[0]; 01804 true_migiusiro_data[0] = migiusiro_data[0]; 01805 true_hidarimae_data[0] = hidarimae_data[0]; 01806 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01807 } 01808 //左進(目標より行き過ぎ) 01809 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01810 true_migimae_data[0] = 0x80; 01811 true_migiusiro_data[0] = 0x80; 01812 true_hidarimae_data[0] = 0x80; 01813 true_hidariusiro_data[0] = 0x80; 01814 } 01815 } 01816 01817 void left_PID(int target) { 01818 01819 //センサ出力値の最小、最大 01820 left_migimae.setInputLimits(-2147483648, 2147483647); 01821 left_migiusiro.setInputLimits(-2147483648, 2147483647); 01822 left_hidarimae.setInputLimits(-2147483648, 2147483647); 01823 left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01824 01825 //制御量の最小、最大 01826 //左進(目標まで達していない) 01827 if((x_pulse1 < target) && (x_pulse2 < target)) { 01828 left_migimae.setOutputLimits(0xEC, 0xED); 01829 //left_migiusiro.setOutputLimits(0x7A, 0x7B); 01830 left_migiusiro.setOutputLimits(0x77, 0x78); 01831 left_hidarimae.setOutputLimits(0x7A, 0x7B); 01832 //left_hidarimae.setOutputLimits(0x77, 0x78); 01833 left_hidariusiro.setOutputLimits(0xFE, 0xFF); 01834 } 01835 //停止(目標より行き過ぎ) 01836 else if((x_pulse1 > target) && (x_pulse2 > target)) { 01837 left_migimae.setOutputLimits(0x7C, 0x83); 01838 left_migiusiro.setOutputLimits(0x7C, 0x83); 01839 left_hidarimae.setOutputLimits(0x7C, 0x83); 01840 left_hidariusiro.setOutputLimits(0x7C, 0x83); 01841 } 01842 01843 //よくわからんやつ 01844 left_migimae.setMode(AUTO_MODE); 01845 left_migiusiro.setMode(AUTO_MODE); 01846 left_hidarimae.setMode(AUTO_MODE); 01847 left_hidariusiro.setMode(AUTO_MODE); 01848 01849 //目標値 01850 left_migimae.setSetPoint(target); 01851 left_migiusiro.setSetPoint(target); 01852 left_hidarimae.setSetPoint(target); 01853 left_hidariusiro.setSetPoint(target); 01854 01855 //センサ出力 01856 left_migimae.setProcessValue(x_pulse1); 01857 left_migiusiro.setProcessValue(x_pulse2); 01858 left_hidarimae.setProcessValue(x_pulse1); 01859 left_hidariusiro.setProcessValue(x_pulse2); 01860 01861 //制御量(計算結果) 01862 migimae_data[0] = left_migimae.compute(); 01863 migiusiro_data[0] = left_migiusiro.compute(); 01864 hidarimae_data[0] = left_hidarimae.compute(); 01865 hidariusiro_data[0] = left_hidariusiro.compute(); 01866 01867 //制御量をPWM値に変換 01868 //左進(目標まで達していない) 01869 if((x_pulse1 < target) && (x_pulse2 < target)) { 01870 true_migimae_data[0] = migimae_data[0]; 01871 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01872 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01873 true_hidariusiro_data[0] = hidariusiro_data[0]; 01874 } 01875 //停止(目標より行き過ぎ) 01876 else if((x_pulse1 > target) && (x_pulse2 > target)) { 01877 true_migimae_data[0] = 0x80; 01878 true_migiusiro_data[0] = 0x80; 01879 true_hidarimae_data[0] = 0x80; 01880 true_hidariusiro_data[0] = 0x80; 01881 } 01882 } 01883 01884 void turn_right_PID(int target) { 01885 01886 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01887 turn_right_migimae.setInputLimits(-2147483648, 2147483647); 01888 turn_right_migiusiro.setInputLimits(-2147483648, 2147483647); 01889 turn_right_hidarimae.setInputLimits(-2147483648, 2147483647); 01890 turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01891 01892 //制御量の最小、最大 01893 //右旋回(目標に達してない) 01894 if(sum_pulse < target) { 01895 turn_right_migimae.setOutputLimits(0x10, 0x7B); 01896 turn_right_migiusiro.setOutputLimits(0x10, 0x7B); 01897 turn_right_hidarimae.setOutputLimits(0x94, 0xFF); 01898 turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); 01899 } 01900 //停止(目標より行き過ぎ) 01901 else if(sum_pulse > target) { 01902 turn_right_migimae.setOutputLimits(0x7C, 0x83); 01903 turn_right_migiusiro.setOutputLimits(0x7C, 0x83); 01904 turn_right_hidarimae.setOutputLimits(0x7C, 0x83); 01905 turn_right_hidariusiro.setOutputLimits(0x7C, 0x83); 01906 } 01907 01908 //よくわからんやつ 01909 turn_right_migimae.setMode(AUTO_MODE); 01910 turn_right_migiusiro.setMode(AUTO_MODE); 01911 turn_right_hidarimae.setMode(AUTO_MODE); 01912 turn_right_hidariusiro.setMode(AUTO_MODE); 01913 01914 //目標値 01915 turn_right_migimae.setSetPoint(target); 01916 turn_right_migiusiro.setSetPoint(target); 01917 turn_right_hidarimae.setSetPoint(target); 01918 turn_right_hidariusiro.setSetPoint(target); 01919 01920 //センサ出力 01921 turn_right_migimae.setProcessValue(sum_pulse); 01922 turn_right_migiusiro.setProcessValue(sum_pulse); 01923 turn_right_hidarimae.setProcessValue(sum_pulse); 01924 turn_right_hidariusiro.setProcessValue(sum_pulse); 01925 01926 //制御量(計算結果) 01927 migimae_data[0] = turn_right_migimae.compute(); 01928 migiusiro_data[0] = turn_right_migiusiro.compute(); 01929 hidarimae_data[0] = turn_right_hidarimae.compute(); 01930 hidariusiro_data[0] = turn_right_hidariusiro.compute(); 01931 01932 //制御量をPWM値に変換 01933 //右旋回(目標に達してない) 01934 if(sum_pulse < target) { 01935 true_migimae_data[0] = 0x7B - migimae_data[0]; 01936 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01937 true_hidarimae_data[0] = hidarimae_data[0]; 01938 true_hidariusiro_data[0] = hidariusiro_data[0]; 01939 } 01940 //停止(目標より行き過ぎ) 01941 else if(sum_pulse > target) { 01942 true_migimae_data[0] = 0x80; 01943 true_migiusiro_data[0] = 0x80; 01944 true_hidarimae_data[0] = 0x80; 01945 true_hidariusiro_data[0] = 0x80; 01946 } 01947 } 01948 01949 void turn_left_PID(int target) { 01950 01951 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01952 turn_left_migimae.setInputLimits(-2147483648, 2147483647); 01953 turn_left_migiusiro.setInputLimits(-2147483648, 2147483647); 01954 turn_left_hidarimae.setInputLimits(-2147483648, 2147483647); 01955 turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01956 01957 //制御量の最小、最大 01958 //左旋回(目標に達してない) 01959 if(sum_pulse < target) { 01960 turn_left_migimae.setOutputLimits(0x94, 0xFF); 01961 turn_left_migiusiro.setOutputLimits(0x94, 0xFF); 01962 turn_left_hidarimae.setOutputLimits(0x10, 0x7B); 01963 turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); 01964 } 01965 //停止(目標より行き過ぎ) 01966 else if(sum_pulse > target) { 01967 turn_left_migimae.setOutputLimits(0x7C, 0x83); 01968 turn_left_migiusiro.setOutputLimits(0x7C, 0x83); 01969 turn_left_hidarimae.setOutputLimits(0x7C, 0x83); 01970 turn_left_hidariusiro.setOutputLimits(0x7C, 0x83); 01971 } 01972 01973 //よくわからんやつ 01974 turn_left_migimae.setMode(AUTO_MODE); 01975 turn_left_migiusiro.setMode(AUTO_MODE); 01976 turn_left_hidarimae.setMode(AUTO_MODE); 01977 turn_left_hidariusiro.setMode(AUTO_MODE); 01978 01979 //目標値 01980 turn_left_migimae.setSetPoint(target); 01981 turn_left_migiusiro.setSetPoint(target); 01982 turn_left_hidarimae.setSetPoint(target); 01983 turn_left_hidariusiro.setSetPoint(target); 01984 01985 //センサ出力 01986 turn_left_migimae.setProcessValue(sum_pulse); 01987 turn_left_migiusiro.setProcessValue(sum_pulse); 01988 turn_left_hidarimae.setProcessValue(sum_pulse); 01989 turn_left_hidariusiro.setProcessValue(sum_pulse); 01990 01991 //制御量(計算結果) 01992 migimae_data[0] = turn_left_migimae.compute(); 01993 migiusiro_data[0] = turn_left_migiusiro.compute(); 01994 hidarimae_data[0] = turn_left_hidarimae.compute(); 01995 hidariusiro_data[0] = turn_left_hidariusiro.compute(); 01996 01997 //制御量をPWM値に変換 01998 //左旋回(目標に達してない) 01999 if(sum_pulse < target) { 02000 true_migimae_data[0] = migimae_data[0]; 02001 true_migiusiro_data[0] = migiusiro_data[0]; 02002 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 02003 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 02004 } 02005 //左旋回(目標より行き過ぎ) 02006 else if(sum_pulse > target) { 02007 true_migimae_data[0] = 0x80; 02008 true_migiusiro_data[0] = 0x80; 02009 true_hidarimae_data[0] = 0x80; 02010 true_hidariusiro_data[0] = 0x80; 02011 } 02012 }
Generated on Tue Aug 16 2022 04:33:42 by
1.7.2