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