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