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