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