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