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