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