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