ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数
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 /* Sensor: encorder*4 */ 00006 /* ------------------------------------------------------------------- */ 00007 /* ファンとサーボの動作を追加した */ 00008 /* ------------------------------------------------------------------- */ 00009 #include "mbed.h" 00010 #include "math.h" 00011 #include "QEI.h" 00012 #include "PID.h" 00013 00014 //直進補正の為の前後・左右の回転差の許容値 00015 #define wheel_difference 100 00016 00017 #define RED 0 00018 #define BLUE 1 00019 00020 //PID Gain of wheels(Kp, Ti, Td, control cycle) 00021 //前進 00022 PID front_migimae(4500000.0, 0.0, 0.0, 0.001); 00023 PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00024 PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00025 PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00026 00027 //後進 00028 PID back_migimae(4500000.0, 0.0, 0.0, 0.001); 00029 PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001); 00030 PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001); 00031 PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001); 00032 00033 //右進 00034 PID right_migimae(6000000.0, 0.0, 0.0, 0.001); 00035 PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00036 PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00037 PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00038 00039 //左進 00040 PID left_migimae(6000000.0, 0.0, 0.0, 0.001); 00041 PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001); 00042 PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001); 00043 PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001); 00044 00045 //右旋回 00046 PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001); 00047 PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001); 00048 PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001); 00049 PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001); 00050 00051 //左旋回 00052 PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001); 00053 PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001); 00054 PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001); 00055 PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001); 00056 00057 //MDとの通信ポート 00058 I2C i2c(PB_9, PB_8); //SDA, SCL 00059 00060 //PCとの通信ポート 00061 Serial pc(USBTX, USBRX); //TX, RX 00062 00063 //特小モジュールとの通信ポート 00064 Serial pic(A0, A1); 00065 00066 //リミットスイッチ基板との通信ポート 00067 Serial limit_serial(PC_12, PD_2); 00068 00069 //12V停止信号ピン 00070 DigitalOut emergency(D11); 00071 00072 DigitalOut USR_LED1(PB_7); 00073 DigitalOut USR_LED2(PC_13); 00074 DigitalOut USR_LED3(PC_2); 00075 DigitalOut USR_LED4(PC_3); 00076 00077 //遠隔非常停止ユニットLED 00078 AnalogOut myled(A2); 00079 00080 DigitalIn start_switch(PB_12); 00081 00082 QEI wheel_x1(PA_8, PA_6, NC, 624); 00083 QEI wheel_x2(PB_14, PB_13, NC, 624); 00084 QEI wheel_y1(PB_1, PB_15, NC, 624); 00085 QEI wheel_y2(PA_12, PA_11, NC, 624); 00086 QEI arm_enc(PB_5, PB_4, NC, 624); 00087 00088 //移動後n秒停止タイマー 00089 Timer counter; 00090 00091 //エンコーダ値格納変数 00092 int x_pulse1, x_pulse2, y_pulse1, y_pulse2; 00093 00094 //操作の段階変数 00095 unsigned int phase = 0; 00096 int kaisyu_phase = 0; 00097 int tyokudo_phase = 0; 00098 unsigned int start_zone = 1; 00099 bool zone = RED; 00100 00101 //i2c送信データ変数 00102 char init_send_data[1]; 00103 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1]; 00104 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1]; 00105 char arm_moter[1], drop_moter[1]; 00106 char fan_data[1]; 00107 char servo_data[1]; 00108 char right_arm_data[1], left_arm_data[1]; 00109 00110 //非常停止関連変数 00111 char RDATA; 00112 char baff; 00113 int flug = 0; 00114 00115 //リミット基板からの受信データ 00116 int limit_data = 0; 00117 int upper_limit_data = 0; 00118 int lower_limit_data = 0; 00119 00120 //各辺のスイッチが押されたかのフラグ 00121 //前部が壁に当たっているか 00122 int front_limit = 0; 00123 //右部が壁にあたあっているか 00124 int right_limit = 0; 00125 //後部が壁に当たっているか 00126 int back_limit = 0; 00127 //回収機構の下限(引っ込めてるほう) 00128 bool kaisyu_limit = 0; 00129 //右腕の下限 00130 bool right_arm_lower_limit = 0; 00131 //右腕の上限 00132 bool right_arm_upper_limit = 0; 00133 //左腕の下限 00134 bool left_arm_lower_limit = 0; 00135 //左腕の上限 00136 bool left_arm_upper_limit = 0; 00137 //吐き出し機構の上限 00138 bool tyokudo_mae_limit = 0; 00139 //吐き出し機構の下限 00140 bool tyokudo_usiro_limit = 0; 00141 00142 00143 int masked_lower_front_limit_data = 0xFF; 00144 int masked_lower_back_limit_data = 0xFF; 00145 int masked_lower_right_limit_data = 0xFF; 00146 int masked_kaisyu_limit_data = 0xFF; 00147 int masked_right_arm_lower_limit_data = 0xFF; 00148 int masked_right_arm_upper_limit_data = 0xFF; 00149 int masked_left_arm_lower_limit_data = 0xFF; 00150 int masked_left_arm_upper_limit_data = 0xFF; 00151 int masked_tyokudo_mae_limit_data = 0xFF; 00152 int masked_tyokudo_usiro_limit_data = 0xFF; 00153 00154 //関数のプロトタイプ宣言 00155 void init(void); 00156 void init_send(void); 00157 void get(void); 00158 void get_pulses(void); 00159 void print_pulses(void); 00160 void get_emergency(void); 00161 void read_limit(void); 00162 void wheel_reset(void); 00163 void kaisyu(int pulse); 00164 void tyokudo(int pulse); 00165 void arm_up(void); 00166 void front(int target); 00167 void back(int target); 00168 void right(int target); 00169 void left(int target); 00170 void turn_right(int target); 00171 void turn_left(int target); 00172 void stop(void); 00173 void front_PID(int target); 00174 void back_PID(int target); 00175 void right_PID(int target); 00176 void left_PID(int target); 00177 void turn_right_PID(int target); 00178 void turn_left_PID(int target); 00179 void dondonkasoku(void); 00180 00181 int main(void) 00182 { 00183 00184 init(); 00185 init_send(); 00186 00187 //とりあえず(後で消してね) 00188 zone = BLUE; 00189 00190 while(1) { 00191 00192 get_pulses(); 00193 print_pulses(); 00194 get_emergency(); 00195 read_limit(); 00196 00197 //if(start_switch == 1) { 00198 //tyokudo(arm_enc.getPulses()); 00199 //} 00200 00201 //青ゾーン 00202 if(zone == BLUE) { 00203 00204 switch(phase) { 00205 00206 //スタート位置へセット 00207 case 0: 00208 //リミットが洗濯物台に触れているか 00209 if(right_limit == 3) { 00210 USR_LED1 = 1; 00211 //スタートスイッチが押されたか 00212 if(start_switch == 1) { 00213 wheel_reset(); 00214 phase = 1; 00215 } 00216 } else { 00217 USR_LED1 = 0; 00218 } 00219 break; 00220 00221 //回収 00222 case 1: 00223 /* 00224 //回収アームのリミットが押されているか 00225 00226 //一度伸ばし始めると止まらないバグあり(09/18) 00227 if(kaisyu_limit == 1) { 00228 kaisyu(arm_enc.getPulses()); 00229 USR_LED2 = 1; 00230 } else if(kaisyu_phase >= 1){ 00231 USR_LED2 = 0; 00232 } 00233 */ 00234 kaisyu(arm_enc.getPulses()); 00235 servo_data[0] = 0x03; 00236 i2c.write(0x30, servo_data, 1); 00237 break; 00238 00239 //1秒停止 00240 case 2: 00241 USR_LED2 = 0; 00242 stop(); 00243 servo_data[0] = 0x04; 00244 i2c.write(0x30, servo_data, 1); 00245 counter.start(); 00246 if(counter.read() > 1.0f) { 00247 phase = 3; 00248 wheel_reset(); 00249 } 00250 break; 00251 00252 //左移動 00253 case 3: 00254 counter.reset(); 00255 left(12000); 00256 if((x_pulse1 > 12000) && (x_pulse2 > 12000)) { 00257 phase = 4; 00258 } 00259 break; 00260 00261 //1秒停止 00262 case 4: 00263 stop(); 00264 counter.start(); 00265 if(counter.read() > 1.0f) { 00266 phase = 5; 00267 wheel_reset(); 00268 } 00269 break; 00270 00271 //右旋回(180°) 00272 case 5: 00273 counter.reset(); 00274 turn_right(518); 00275 if(x_pulse2 > 518) { 00276 phase = 6; 00277 } 00278 break; 00279 00280 //1秒停止 00281 case 6: 00282 stop(); 00283 counter.start(); 00284 if(counter.read() > 1.0f) { 00285 phase = 7; 00286 wheel_reset(); 00287 } 00288 break; 00289 00290 //ちょっくら右移動 00291 case 7: 00292 counter.reset(); 00293 right(-1000); 00294 00295 if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) { 00296 true_migimae_data[0] = 0x94; 00297 true_migiusiro_data[0] = 0x10; 00298 true_hidarimae_data[0] = 0x10; 00299 true_hidariusiro_data[0] = 0x94; 00300 } 00301 if(right_limit == 3) { 00302 phase = 8; 00303 } 00304 break; 00305 00306 //1秒停止 00307 case 8: 00308 stop(); 00309 counter.start(); 00310 if(counter.read() > 1.0f) { 00311 phase = 9; 00312 wheel_reset(); 00313 } 00314 break; 00315 00316 //排出 00317 case 9: 00318 counter.reset(); 00319 tyokudo(arm_enc.getPulses()); 00320 //ここに排出動作が来るが今回は飛ばす 00321 //phase = 10; 00322 break; 00323 00324 //排出機構引っ込める 00325 case 10: 00326 //ここに排出機構引っ込める動作が来るが今回は飛ばす 00327 phase = 11; 00328 break; 00329 00330 //1秒停止 00331 case 11: 00332 stop(); 00333 counter.start(); 00334 if(counter.read() > 1.0f) { 00335 phase = 12; 00336 wheel_reset(); 00337 } 00338 break; 00339 00340 //前進 00341 case 12: 00342 counter.reset(); 00343 front(3000); 00344 if((y_pulse1 > 3000) && (y_pulse2 > 3000)) { 00345 phase = 13; 00346 } 00347 break; 00348 00349 //1秒停止 00350 case 13: 00351 stop(); 00352 counter.start(); 00353 if(counter.read() > 1.0f) { 00354 phase = 14; 00355 wheel_reset(); 00356 } 00357 break; 00358 00359 //右移動 00360 case 14: 00361 counter.reset(); 00362 right(-4000); 00363 if((x_pulse1*-1 > 3000) && (x_pulse2*-1 > 3000)) { 00364 true_migimae_data[0] = 0x94; 00365 true_migiusiro_data[0] = 0x10; 00366 true_hidarimae_data[0] = 0x10; 00367 true_hidariusiro_data[0] = 0x94; 00368 } 00369 if(right_limit == 3) { 00370 phase = 15; 00371 } 00372 break; 00373 00374 //シーツ装填 00375 case 15: 00376 if(start_switch == 1) { 00377 phase = 16; 00378 } else { 00379 stop(); 00380 } 00381 break; 00382 00383 //竿のラインまで前進 00384 case 16: 00385 counter.reset(); 00386 front(21500); 00387 if((y_pulse1 > 21500) && (y_pulse2 > 21500)) { 00388 phase = 17; 00389 } 00390 break; 00391 00392 //1秒停止 00393 case 17: 00394 stop(); 00395 counter.start(); 00396 if(counter.read() > 1.0f) { 00397 phase = 18; 00398 wheel_reset(); 00399 } 00400 break; 00401 00402 //掛けるところまで左移動 00403 case 18: 00404 counter.reset(); 00405 left(10000); 00406 if((x_pulse1 > 10000) && (x_pulse2 > 10000)) { 00407 phase = 19; 00408 } 00409 break; 00410 00411 //1秒停止 00412 case 19: 00413 stop(); 00414 counter.start(); 00415 if(counter.read() > 1.0f) { 00416 phase = 20; 00417 wheel_reset(); 00418 } 00419 break; 00420 00421 //妨害防止の右旋回 00422 case 20: 00423 counter.reset(); 00424 turn_right(300); 00425 if(x_pulse2 > 300) { 00426 phase = 21; 00427 } 00428 break; 00429 00430 //1秒停止 00431 case 21: 00432 stop(); 00433 counter.start(); 00434 if(counter.read() > 1.0f) { 00435 phase = 22; 00436 wheel_reset(); 00437 } 00438 break; 00439 00440 //アームアップ 00441 case 22: 00442 stop(); 00443 counter.reset(); 00444 arm_up(); 00445 break; 00446 00447 //シーツを掛ける 00448 case 23: 00449 counter.start(); 00450 00451 //5秒間ファン送風 00452 if(counter.read() <= 4.0f) { 00453 fan_data[0] = 0xFF; 00454 i2c.write(0x26, fan_data, 1); 00455 servo_data[0] = 0x04; 00456 i2c.write(0x30, servo_data, 1); 00457 } 00458 //4~5秒の間でサーボを放す 00459 else if((counter.read() > 4.0f) && (counter.read() <= 5.0f)) { 00460 fan_data[0] = 0xFF; 00461 i2c.write(0x26, fan_data, 1); 00462 servo_data[0] = 0x03; 00463 i2c.write(0x30, servo_data, 1); 00464 } 00465 //5秒過ぎたらファン停止 00466 else if(counter.read() > 5.0f) { 00467 fan_data[0] = 0x80; 00468 i2c.write(0x26, fan_data, 1); 00469 servo_data[0] = 0x04; 00470 i2c.write(0x30, servo_data, 1); 00471 phase = 24; 00472 } 00473 break; 00474 00475 //終了っ!(守衛さん風) 00476 case 24: 00477 //駆動系統OFF 00478 emergency = 0; 00479 break; 00480 00481 default: 00482 //駆動系統OFF 00483 emergency = 0; 00484 break; 00485 } 00486 } 00487 } 00488 } 00489 00490 void init(void) 00491 { 00492 00493 //通信ボーレートの設定 00494 pc.baud(460800); 00495 00496 limit_serial.baud(115200); 00497 // emergency = 0; 00498 start_switch.mode(PullUp); 00499 00500 //非常停止関連 00501 pic.baud(19200); 00502 pic.format(8, Serial::None, 1); 00503 pic.attach(get, Serial::RxIrq); 00504 00505 x_pulse1 = 0; 00506 x_pulse2 = 0; 00507 y_pulse1 = 0; 00508 y_pulse2 = 0; 00509 migimae_data[0] = 0x80; 00510 migiusiro_data[0] = 0x80; 00511 hidarimae_data[0] = 0x80; 00512 hidariusiro_data[0] = 0x80; 00513 true_migimae_data[0] = 0x80; 00514 true_migiusiro_data[0] = 0x80; 00515 true_hidarimae_data[0] = 0x80; 00516 true_hidariusiro_data[0] = 0x80; 00517 fan_data[0] = 0x80; 00518 servo_data[0] = 0x80; 00519 right_arm_data[0] = 0x80; 00520 left_arm_data[0] = 0x80; 00521 } 00522 00523 void init_send(void) 00524 { 00525 00526 init_send_data[0] = 0x80; 00527 i2c.write(0x10, init_send_data, 1); 00528 i2c.write(0x12, init_send_data, 1); 00529 i2c.write(0x14, init_send_data, 1); 00530 i2c.write(0x16, init_send_data, 1); 00531 i2c.write(0x18, init_send_data, 1); 00532 i2c.write(0x20, init_send_data, 1); 00533 i2c.write(0x22, init_send_data, 1); 00534 i2c.write(0x24, init_send_data, 1); 00535 i2c.write(0x30, init_send_data, 1); 00536 wait(0.1); 00537 } 00538 00539 void get(void) 00540 { 00541 00542 baff = pic.getc(); 00543 00544 for(; flug; flug--) 00545 RDATA = baff; 00546 00547 if(baff == ':') 00548 flug = 1; 00549 } 00550 00551 void get_pulses(void) 00552 { 00553 00554 x_pulse1 = wheel_x1.getPulses(); 00555 x_pulse2 = wheel_x2.getPulses(); 00556 y_pulse1 = wheel_y1.getPulses(); 00557 y_pulse2 = wheel_y2.getPulses(); 00558 } 00559 00560 void print_pulses(void) 00561 { 00562 pc.printf("%d\n",phase); 00563 //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data); 00564 //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase); 00565 //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]); 00566 } 00567 00568 void get_emergency(void) 00569 { 00570 00571 if(RDATA == '1') { 00572 myled = 1; 00573 emergency = 1; 00574 } else if(RDATA == '9') { 00575 myled = 0.2; 00576 emergency = 0; 00577 } 00578 } 00579 00580 void read_limit(void) 00581 { 00582 00583 limit_data = limit_serial.getc(); 00584 00585 //上位1bitが1ならば下のリミットのデータだと判断 00586 if((limit_data & 0b10000000) == 0b10000000) { 00587 lower_limit_data = limit_data; 00588 00589 //上位1bitが0ならば上のリミットのデータだと判断 00590 } else { 00591 upper_limit_data = limit_data; 00592 } 00593 00594 //下リミット基板からのデータのマスク処理 00595 masked_lower_front_limit_data = lower_limit_data & 0b00000011; 00596 masked_lower_back_limit_data = lower_limit_data & 0b00001100; 00597 masked_lower_right_limit_data = lower_limit_data & 0b00110000; 00598 masked_kaisyu_limit_data = lower_limit_data & 0b01000000; 00599 00600 //上リミット基板からのデータのマスク処理 00601 masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001; 00602 masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010; 00603 masked_left_arm_lower_limit_data = upper_limit_data & 0b00000100; 00604 masked_left_arm_upper_limit_data = upper_limit_data & 0b00001000; 00605 masked_tyokudo_mae_limit_data = upper_limit_data & 0b00010000; 00606 masked_tyokudo_usiro_limit_data = upper_limit_data & 0b00100000; 00607 00608 //前部リミット 00609 switch(masked_lower_front_limit_data) { 00610 //両方押された 00611 case 0x00: 00612 front_limit = 3; 00613 break; 00614 //右が押された 00615 case 0b00000010: 00616 front_limit = 1; 00617 break; 00618 //左が押された 00619 case 0b00000001: 00620 front_limit = 2; 00621 break; 00622 default: 00623 front_limit = 0; 00624 break; 00625 } 00626 00627 //後部リミット 00628 switch(masked_lower_back_limit_data) { 00629 //両方押された 00630 case 0x00: 00631 back_limit = 3; 00632 break; 00633 //右が押された 00634 case 0b00001000: 00635 back_limit = 1; 00636 break; 00637 //左が押された 00638 case 0b00000100: 00639 back_limit = 2; 00640 break; 00641 default: 00642 back_limit = 0; 00643 break; 00644 } 00645 00646 //右部リミット 00647 switch(masked_lower_right_limit_data) { 00648 //両方押された 00649 case 0x00: 00650 right_limit = 3; 00651 break; 00652 //右が押された 00653 case 0b00100000: 00654 right_limit = 1; 00655 break; 00656 //左が押された 00657 case 0b00010000: 00658 right_limit = 2; 00659 break; 00660 default: 00661 right_limit = 0; 00662 break; 00663 } 00664 00665 //回収機構リミット 00666 switch(masked_kaisyu_limit_data) { 00667 //押された 00668 case 0b00000000: 00669 kaisyu_limit = 1; 00670 break; 00671 case 0b01000000: 00672 kaisyu_limit = 0; 00673 break; 00674 default: 00675 kaisyu_limit = 0; 00676 break; 00677 } 00678 00679 //右腕下部リミット 00680 switch(masked_right_arm_lower_limit_data) { 00681 //押された 00682 case 0b00000000: 00683 right_arm_lower_limit = 1; 00684 break; 00685 case 0b00000001: 00686 right_arm_lower_limit = 0; 00687 break; 00688 default: 00689 right_arm_lower_limit = 0; 00690 break; 00691 } 00692 00693 //右腕上部リミット 00694 switch(masked_right_arm_upper_limit_data) { 00695 //押された 00696 case 0b00000000: 00697 right_arm_upper_limit = 1; 00698 break; 00699 case 0b00000010: 00700 right_arm_upper_limit = 0; 00701 break; 00702 default: 00703 right_arm_upper_limit = 0; 00704 break; 00705 } 00706 00707 //左腕下部リミット 00708 switch(masked_left_arm_lower_limit_data) { 00709 //押された 00710 case 0b00000000: 00711 left_arm_lower_limit = 1; 00712 break; 00713 case 0b00000100: 00714 left_arm_lower_limit = 0; 00715 break; 00716 default: 00717 left_arm_lower_limit = 0; 00718 break; 00719 } 00720 00721 //左腕上部リミット 00722 switch(masked_left_arm_upper_limit_data) { 00723 //押された 00724 case 0b00000000: 00725 left_arm_upper_limit = 1; 00726 break; 00727 case 0b00001000: 00728 left_arm_upper_limit = 0; 00729 break; 00730 default: 00731 left_arm_upper_limit = 0; 00732 break; 00733 } 00734 00735 //直動の前 00736 switch(masked_tyokudo_mae_limit_data) { 00737 //押された 00738 case 0b00000000: 00739 tyokudo_mae_limit = 1; 00740 break; 00741 case 0b00010000: 00742 tyokudo_mae_limit = 0; 00743 break; 00744 default: 00745 tyokudo_mae_limit = 0; 00746 break; 00747 } 00748 00749 //直動の後 00750 switch(masked_tyokudo_usiro_limit_data) { 00751 //押された 00752 case 0b00000000: 00753 tyokudo_usiro_limit = 1; 00754 break; 00755 case 0b00100000: 00756 tyokudo_usiro_limit = 0; 00757 break; 00758 default: 00759 tyokudo_usiro_limit = 0; 00760 break; 00761 } 00762 } 00763 00764 void wheel_reset(void) 00765 { 00766 00767 wheel_x1.reset(); 00768 wheel_x2.reset(); 00769 wheel_y1.reset(); 00770 wheel_y2.reset(); 00771 } 00772 00773 void kaisyu(int pulse) 00774 { 00775 switch (kaisyu_phase) { 00776 00777 case 0: 00778 //前進->減速 00779 //3000pulseまで高速前進 00780 if(pulse < 3000) { 00781 arm_moter[0] = 0xFF; 00782 //kaisyu_phase = 1; 00783 } 00784 00785 //3000pulse超えたら低速前進 00786 else if(pulse >= 3000) { 00787 arm_moter[0] = 0xB3; 00788 kaisyu_phase = 1; 00789 } 00790 break; 00791 00792 case 1: 00793 USR_LED2 = 1; 00794 //前進->停止->後進 00795 //3600pulseまで低速前進 00796 if(pulse < 3600) { 00797 arm_moter[0] = 0xB3; 00798 //kaisyu_phase = 2; 00799 } 00800 00801 //3600pulse超えたら停止 00802 else if(pulse >= 3600) { 00803 arm_moter[0] = 0x80; 00804 00805 //1秒待ってから引っ込める 00806 counter.start(); 00807 if(counter.read() > 1.0f) { 00808 kaisyu_phase = 2; 00809 } 00810 } 00811 00812 break; 00813 00814 case 2: 00815 //後進->減速 00816 //500pulseまで高速後進 00817 counter.reset(); 00818 if(pulse > 500) { 00819 arm_moter[0] = 0x00; 00820 //kaisyu_phase = 3; 00821 00822 } 00823 //500pulse以下になったら低速後進 00824 else if(pulse <= 500) { 00825 arm_moter[0] = 0x4C; 00826 kaisyu_phase = 3; 00827 } 00828 break; 00829 00830 case 3: 00831 //後進->停止 00832 //リミット押されるまで低速後進 00833 if(pulse <= 500) { 00834 arm_moter[0] = 0x4C; 00835 //kaisyu_phase = 4; 00836 } 00837 00838 //リミット押されたら停止 00839 if(kaisyu_limit == 1) { 00840 arm_moter[0] = 0x80; 00841 kaisyu_phase = 4; 00842 phase = 2; 00843 } 00844 break; 00845 00846 default: 00847 arm_moter[0] = 0x80; 00848 break; 00849 } 00850 00851 //回収MDへ書き込み 00852 i2c.write(0x18, arm_moter, 1); 00853 } 00854 00855 void tyokudo(int pulse) 00856 { 00857 00858 switch(tyokudo_phase) { 00859 00860 case 0: 00861 //前進->減速 00862 00863 /* エンコーダー読まずにリミットだけ(修正必須) */ 00864 //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行 00865 if(tyokudo_mae_limit == 0) { 00866 //2000pulseまで高速前進 00867 if(pulse < 2000) { 00868 arm_moter[0] = 0xC0; 00869 drop_moter[0] = 0xE6; 00870 } 00871 //2000pulse以上で低速前進 00872 else if(pulse >= 2000) { 00873 arm_moter[0] = 0xC0; 00874 drop_moter[0] = 0xE6; 00875 } 00876 //パルスが3600を終えたらアームのみ強制停止 00877 else if(pulse > 3600) { 00878 arm_moter[0] = 0x80; 00879 drop_moter[0] = 0xE6; 00880 } 00881 } 00882 00883 //直動の前リミットが押されたら 00884 else if(tyokudo_mae_limit == 1) { 00885 //高速後進 00886 arm_moter[0] = 0x40; 00887 drop_moter[0] = 0x00; 00888 tyokudo_phase = 1; 00889 } 00890 break; 00891 00892 case 1: 00893 //後進->減速 00894 //リミットが押されたら強制停止 00895 if(tyokudo_usiro_limit == 1) { 00896 arm_moter[0] = 0x80; 00897 drop_moter[0] = 0x80; 00898 tyokudo_phase = 2; 00899 phase = 10; 00900 } 00901 break; 00902 default: 00903 arm_moter[0] = 0x80; 00904 drop_moter[0] = 0x80; 00905 break; 00906 } 00907 00908 i2c.write(0x18, arm_moter, 1); 00909 i2c.write(0x20, drop_moter, 1); 00910 } 00911 00912 void arm_up(void) 00913 { 00914 00915 //両腕、上限リミットが押されてなかったら上昇 00916 if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) { 00917 right_arm_data[0] = 0xFF; 00918 left_arm_data[0] = 0xFF; 00919 } 00920 //右腕のみリミットが押されたら左腕のみ上昇 00921 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) { 00922 right_arm_data[0] = 0x80; 00923 left_arm_data[0] = 0xFF; 00924 } 00925 //左腕のみリミットが押されたら右腕のみ上昇 00926 else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) { 00927 right_arm_data[0] = 0xFF; 00928 left_arm_data[0] = 0x80; 00929 } 00930 //両腕、上限リミットが押されたら停止 00931 else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) { 00932 right_arm_data[0] = 0x80; 00933 left_arm_data[0] = 0x80; 00934 phase = 23; 00935 } 00936 00937 i2c.write(0x22, right_arm_data, 1); 00938 i2c.write(0x24, left_arm_data, 1); 00939 } 00940 00941 void front(int target) 00942 { 00943 00944 front_PID(target); 00945 i2c.write(0x10, true_migimae_data, 1, false); 00946 i2c.write(0x12, true_migiusiro_data, 1, false); 00947 i2c.write(0x14, true_hidarimae_data, 1, false); 00948 i2c.write(0x16, true_hidariusiro_data, 1, false); 00949 wait_us(20); 00950 } 00951 00952 void back(int target) 00953 { 00954 00955 back_PID(target); 00956 i2c.write(0x10, true_migimae_data, 1, false); 00957 i2c.write(0x12, true_migiusiro_data, 1, false); 00958 i2c.write(0x14, true_hidarimae_data, 1, false); 00959 i2c.write(0x16, true_hidariusiro_data, 1, false); 00960 wait_us(20); 00961 } 00962 00963 void right(int target) 00964 { 00965 00966 right_PID(target); 00967 i2c.write(0x10, true_migimae_data, 1, false); 00968 i2c.write(0x12, true_migiusiro_data, 1, false); 00969 i2c.write(0x14, true_hidarimae_data, 1, false); 00970 i2c.write(0x16, true_hidariusiro_data, 1, false); 00971 wait_us(20); 00972 } 00973 00974 void left(int target) 00975 { 00976 00977 left_PID(target); 00978 i2c.write(0x10, true_migimae_data, 1, false); 00979 i2c.write(0x12, true_migiusiro_data, 1, false); 00980 i2c.write(0x14, true_hidarimae_data, 1, false); 00981 i2c.write(0x16, true_hidariusiro_data, 1, false); 00982 wait_us(20); 00983 } 00984 00985 void turn_right(int target) 00986 { 00987 00988 turn_right_PID(target); 00989 i2c.write(0x10, true_migimae_data, 1, false); 00990 i2c.write(0x12, true_migiusiro_data, 1, false); 00991 i2c.write(0x14, true_hidarimae_data, 1, false); 00992 i2c.write(0x16, true_hidariusiro_data, 1, false); 00993 wait_us(20); 00994 } 00995 00996 void turn_left(int target) 00997 { 00998 00999 turn_left_PID(target); 01000 i2c.write(0x10, true_migimae_data, 1, false); 01001 i2c.write(0x12, true_migiusiro_data, 1, false); 01002 i2c.write(0x14, true_hidarimae_data, 1, false); 01003 i2c.write(0x16, true_hidariusiro_data, 1, false); 01004 wait_us(20); 01005 } 01006 01007 void stop(void) 01008 { 01009 01010 true_migimae_data[0] = 0x80; 01011 true_migiusiro_data[0] = 0x80; 01012 true_hidarimae_data[0] = 0x80; 01013 true_hidariusiro_data[0] = 0x80; 01014 01015 i2c.write(0x10, true_migimae_data, 1, false); 01016 i2c.write(0x12, true_migiusiro_data, 1, false); 01017 i2c.write(0x14, true_hidarimae_data, 1, false); 01018 i2c.write(0x16, true_hidariusiro_data, 1, false); 01019 wait_us(20); 01020 } 01021 01022 void front_PID(int target) 01023 { 01024 01025 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01026 front_migimae.setInputLimits(-2147483648, 2147483647); 01027 front_migiusiro.setInputLimits(-2147483648, 2147483647); 01028 front_hidarimae.setInputLimits(-2147483648, 2147483647); 01029 front_hidariusiro.setInputLimits(-2147483648, 2147483647); 01030 01031 //制御量の最小、最大 01032 //正転(目標に達してない) 01033 if((y_pulse1 < target) && (y_pulse2 < target)) { 01034 front_migimae.setOutputLimits(0x84, 0xF7); 01035 front_migiusiro.setOutputLimits(0x84, 0xF7); 01036 //front_migimae.setOutputLimits(0x84, 0xFF); 01037 //front_migiusiro.setOutputLimits(0x84, 0xFF); 01038 front_hidarimae.setOutputLimits(0x84, 0xFF); 01039 front_hidariusiro.setOutputLimits(0x84, 0xFF); 01040 } 01041 //左側が前に出ちゃった♥(右側だけ回して左側は停止) 01042 else if((y_pulse1 + wheel_difference) < y_pulse2) { 01043 front_migimae.setOutputLimits(0x84, 0xFF); 01044 front_migiusiro.setOutputLimits(0x84, 0xFF); 01045 front_hidarimae.setOutputLimits(0x7C, 0x83); 01046 front_hidariusiro.setOutputLimits(0x7C, 0x83); 01047 } 01048 //右側が前に出ちゃった♥(左側だけ回して右側は停止) 01049 else if(y_pulse1 > (y_pulse2 + wheel_difference)) { 01050 front_migimae.setOutputLimits(0x7C, 0x83); 01051 front_migiusiro.setOutputLimits(0x7C, 0x83); 01052 front_hidarimae.setOutputLimits(0x84, 0xFF); 01053 front_hidariusiro.setOutputLimits(0x84, 0xFF); 01054 } 01055 //停止(目標より行き過ぎ) 01056 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01057 front_migimae.setOutputLimits(0x7C, 0x83); 01058 front_migiusiro.setOutputLimits(0x7C, 0x83); 01059 front_hidarimae.setOutputLimits(0x7C, 0x83); 01060 front_hidariusiro.setOutputLimits(0x7C, 0x83); 01061 } 01062 01063 //よくわからんやつ 01064 front_migimae.setMode(AUTO_MODE); 01065 front_migiusiro.setMode(AUTO_MODE); 01066 front_hidarimae.setMode(AUTO_MODE); 01067 front_hidariusiro.setMode(AUTO_MODE); 01068 01069 //目標値 01070 front_migimae.setSetPoint(target); 01071 front_migiusiro.setSetPoint(target); 01072 front_hidarimae.setSetPoint(target); 01073 front_hidariusiro.setSetPoint(target); 01074 01075 //センサ出力 01076 front_migimae.setProcessValue(y_pulse1); 01077 front_migiusiro.setProcessValue(y_pulse1); 01078 front_hidarimae.setProcessValue(y_pulse2); 01079 front_hidariusiro.setProcessValue(y_pulse2); 01080 01081 //制御量(計算結果) 01082 migimae_data[0] = front_migimae.compute(); 01083 migiusiro_data[0] = front_migiusiro.compute(); 01084 hidarimae_data[0] = front_hidarimae.compute(); 01085 hidariusiro_data[0] = front_hidariusiro.compute(); 01086 01087 //制御量をPWM値に変換 01088 //正転(目標に達してない) 01089 if((y_pulse1 < target) && (y_pulse2 < target)) { 01090 true_migimae_data[0] = migimae_data[0]; 01091 true_migiusiro_data[0] = migiusiro_data[0]; 01092 true_hidarimae_data[0] = hidarimae_data[0]; 01093 true_hidariusiro_data[0] = hidariusiro_data[0]; 01094 } 01095 //左側が前に出ちゃった♥(右側だけ回して左側は停止) 01096 else if((y_pulse1 + wheel_difference) < y_pulse2) { 01097 true_migimae_data[0] = migimae_data[0]; 01098 true_migiusiro_data[0] = migiusiro_data[0]; 01099 true_hidarimae_data[0] = 0x80; 01100 true_hidariusiro_data[0] = 0x80; 01101 } 01102 //右側が前に出ちゃった♥(左側だけ回して右側は停止) 01103 else if(y_pulse1 > (y_pulse2 + wheel_difference)) { 01104 true_migimae_data[0] = 0x80; 01105 true_migiusiro_data[0] = 0x80; 01106 true_hidarimae_data[0] = hidarimae_data[0]; 01107 true_hidariusiro_data[0] = hidariusiro_data[0]; 01108 } 01109 //停止(目標より行き過ぎ) 01110 else if((y_pulse1 > target) && (y_pulse2 > target)) { 01111 true_migimae_data[0] = 0x80; 01112 true_migiusiro_data[0] = 0x80; 01113 true_hidarimae_data[0] = 0x80; 01114 true_hidariusiro_data[0] = 0x80; 01115 } 01116 } 01117 01118 void back_PID(int target) 01119 { 01120 01121 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01122 back_migimae.setInputLimits(-2147483648, 2147483647); 01123 back_migiusiro.setInputLimits(-2147483648, 2147483647); 01124 back_hidarimae.setInputLimits(-2147483648, 2147483647); 01125 back_hidariusiro.setInputLimits(-2147483648, 2147483647); 01126 01127 //制御量の最小、最大 01128 //逆転(目標に達してない) 01129 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01130 back_migimae.setOutputLimits(0x00, 0x7B); 01131 back_migiusiro.setOutputLimits(0x00, 0x7B); 01132 //back_hidarimae.setOutputLimits(0x00, 0x73); 01133 //back_hidariusiro.setOutputLimits(0x00, 0x73); 01134 back_hidarimae.setOutputLimits(0x00, 0x7B); 01135 back_hidariusiro.setOutputLimits(0x00, 0x7B); 01136 } 01137 //左側が後に出ちゃった♥(右側だけ回して左側は停止) 01138 else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1) { 01139 back_migimae.setOutputLimits(0x00, 0x7B); 01140 back_migiusiro.setOutputLimits(0x00, 0x7B); 01141 back_hidarimae.setOutputLimits(0x7C, 0x83); 01142 back_hidariusiro.setOutputLimits(0x7C, 0x83); 01143 } 01144 //右側が後に出ちゃった♥(左側だけ回して右側は停止) 01145 else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)) { 01146 back_migimae.setOutputLimits(0x7C, 0x83); 01147 back_migiusiro.setOutputLimits(0x7C, 0x83); 01148 back_hidarimae.setOutputLimits(0x00, 0x7B); 01149 back_hidariusiro.setOutputLimits(0x00, 0x7B); 01150 } 01151 //停止(目標より行き過ぎ) 01152 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01153 back_migimae.setOutputLimits(0x7C, 0x83); 01154 back_migiusiro.setOutputLimits(0x7C, 0x83); 01155 back_hidarimae.setOutputLimits(0x7C, 0x83); 01156 back_hidariusiro.setOutputLimits(0x7C, 0x83); 01157 } 01158 01159 //よくわからんやつ 01160 back_migimae.setMode(AUTO_MODE); 01161 back_migiusiro.setMode(AUTO_MODE); 01162 back_hidarimae.setMode(AUTO_MODE); 01163 back_hidariusiro.setMode(AUTO_MODE); 01164 01165 //目標値 01166 back_migimae.setSetPoint(target*-1); 01167 back_migiusiro.setSetPoint(target*-1); 01168 back_hidarimae.setSetPoint(target*-1); 01169 back_hidariusiro.setSetPoint(target*-1); 01170 01171 //センサ出力 01172 back_migimae.setProcessValue(y_pulse1*-1); 01173 back_migiusiro.setProcessValue(y_pulse1*-1); 01174 back_hidarimae.setProcessValue(y_pulse2*-1); 01175 back_hidariusiro.setProcessValue(y_pulse2*-1); 01176 01177 //制御量(計算結果) 01178 migimae_data[0] = back_migimae.compute(); 01179 migiusiro_data[0] = back_migiusiro.compute(); 01180 hidarimae_data[0] = back_hidarimae.compute(); 01181 hidariusiro_data[0] = back_hidariusiro.compute(); 01182 01183 //制御量をPWM値に変換 01184 //逆転(目標に達してない) 01185 if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) { 01186 true_migimae_data[0] = 0x7B - migimae_data[0]; 01187 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01188 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01189 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01190 } 01191 //左側が後に出ちゃった♥(右側だけ回して左側は停止) 01192 else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1) { 01193 true_migimae_data[0] = 0x7B - migimae_data[0]; 01194 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01195 true_hidarimae_data[0] = 0x80; 01196 true_hidariusiro_data[0] = 0x80; 01197 } 01198 //右側が後に出ちゃった♥(左側だけ回して右側は停止) 01199 else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)) { 01200 true_migimae_data[0] = 0x80; 01201 true_migiusiro_data[0] = 0x80; 01202 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01203 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01204 } 01205 //停止(目標より行き過ぎ) 01206 else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) { 01207 true_migimae_data[0] = 0x80; 01208 true_migiusiro_data[0] = 0x80; 01209 true_hidarimae_data[0] = 0x80; 01210 true_hidariusiro_data[0] = 0x80; 01211 } 01212 } 01213 01214 void right_PID(int target) 01215 { 01216 01217 //センサ出力値の最小、最大 01218 right_migimae.setInputLimits(-2147483648, 2147483647); 01219 right_migiusiro.setInputLimits(-2147483648, 2147483647); 01220 right_hidarimae.setInputLimits(-2147483648, 2147483647); 01221 right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01222 01223 //制御量の最小、最大 01224 //右進(目標まで達していない) 01225 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01226 // right_migimae.setOutputLimits(0x00, 0x6C); 01227 right_migimae.setOutputLimits(0x00, 0x7B); 01228 right_migiusiro.setOutputLimits(0x84, 0xFF); 01229 //right_hidarimae.setOutputLimits(0x84, 0xF0); 01230 right_hidarimae.setOutputLimits(0x84, 0xFF); 01231 right_hidariusiro.setOutputLimits(0x00, 0x7B); 01232 01233 } 01234 //前側が右に出ちゃった♥(後側だけ回して前側は停止) 01235 else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)) { 01236 right_migimae.setOutputLimits(0x7C, 0x83); 01237 right_migiusiro.setOutputLimits(0x00, 0x7B); 01238 right_hidarimae.setOutputLimits(0x7C, 0x83); 01239 right_hidariusiro.setOutputLimits(0x84, 0xFF); 01240 } 01241 //後側が右に出ちゃった♥(前側だけ回して後側は停止) 01242 else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1) { 01243 right_migimae.setOutputLimits(0x84, 0xFF); 01244 right_migiusiro.setOutputLimits(0x7C, 0x83); 01245 right_hidarimae.setOutputLimits(0x00, 0x7B); 01246 right_hidariusiro.setOutputLimits(0x7C, 0x83); 01247 } 01248 //停止(目標より行き過ぎ) 01249 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01250 right_migimae.setOutputLimits(0x7C, 0x83); 01251 right_migiusiro.setOutputLimits(0x7C, 0x83); 01252 right_hidarimae.setOutputLimits(0x7C, 0x83); 01253 right_hidariusiro.setOutputLimits(0x7C, 0x83); 01254 } 01255 01256 //よくわからんやつ 01257 right_migimae.setMode(AUTO_MODE); 01258 right_migiusiro.setMode(AUTO_MODE); 01259 right_hidarimae.setMode(AUTO_MODE); 01260 right_hidariusiro.setMode(AUTO_MODE); 01261 01262 //目標値 01263 right_migimae.setSetPoint(target*-1); 01264 right_migiusiro.setSetPoint(target*-1); 01265 right_hidarimae.setSetPoint(target*-1); 01266 right_hidariusiro.setSetPoint(target*-1); 01267 01268 //センサ出力 01269 right_migimae.setProcessValue(x_pulse1*-1); 01270 right_migiusiro.setProcessValue(x_pulse2*-1); 01271 right_hidarimae.setProcessValue(x_pulse1*-1); 01272 right_hidariusiro.setProcessValue(x_pulse2*-1); 01273 01274 //制御量(計算結果) 01275 migimae_data[0] = right_migimae.compute(); 01276 migiusiro_data[0] = right_migiusiro.compute(); 01277 hidarimae_data[0] = right_hidarimae.compute(); 01278 hidariusiro_data[0] = right_hidariusiro.compute(); 01279 01280 //制御量をPWM値に変換 01281 //右進(目標まで達していない) 01282 if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) { 01283 true_migimae_data[0] = 0x7B - migimae_data[0]; 01284 true_migiusiro_data[0] = migiusiro_data[0]; 01285 true_hidarimae_data[0] = hidarimae_data[0]; 01286 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01287 } 01288 //前側が右に出ちゃった♥(後側だけ回して前側は停止) 01289 else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)) { 01290 true_migimae_data[0] = 0x80; 01291 true_migiusiro_data[0] = migiusiro_data[0]; 01292 true_hidarimae_data[0] = 0x80; 01293 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01294 } 01295 //後側が右に出ちゃった♥(前側だけ回して後側は停止) 01296 else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1) { 01297 true_migimae_data[0] = 0x7B - migimae_data[0]; 01298 true_migiusiro_data[0] = 0x80; 01299 true_hidarimae_data[0] = hidarimae_data[0]; 01300 true_hidariusiro_data[0] = 0x80; 01301 } 01302 //左進(目標より行き過ぎ) 01303 else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) { 01304 true_migimae_data[0] = 0x80; 01305 true_migiusiro_data[0] = 0x80; 01306 true_hidarimae_data[0] = 0x80; 01307 true_hidariusiro_data[0] = 0x80; 01308 } 01309 } 01310 01311 void left_PID(int target) 01312 { 01313 01314 //センサ出力値の最小、最大 01315 left_migimae.setInputLimits(-2147483648, 2147483647); 01316 left_migiusiro.setInputLimits(-2147483648, 2147483647); 01317 left_hidarimae.setInputLimits(-2147483648, 2147483647); 01318 left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01319 01320 //制御量の最小、最大 01321 //左進(目標まで達していない) 01322 if((x_pulse1 < target) && (x_pulse2 < target)) { 01323 left_migimae.setOutputLimits(0x84, 0xED); 01324 left_migiusiro.setOutputLimits(0x00, 0x7B); 01325 left_hidarimae.setOutputLimits(0x00, 0x7B); 01326 left_hidariusiro.setOutputLimits(0x84, 0xFF); 01327 } 01328 //前側が左に出ちゃった♥(後側だけ回して前側は停止) 01329 else if(x_pulse1 > (x_pulse2 + wheel_difference)) { 01330 left_migimae.setOutputLimits(0x7C, 0x83); 01331 left_migiusiro.setOutputLimits(0x7C, 0x83); 01332 left_hidarimae.setOutputLimits(0x10, 0x7B); 01333 left_hidariusiro.setOutputLimits(0x94, 0xFF); 01334 } 01335 //後側が左に出ちゃった♥(前側だけ回して後側は停止) 01336 else if((x_pulse1 + wheel_difference) < x_pulse2) { 01337 left_migimae.setOutputLimits(0x94, 0xFF); 01338 left_migiusiro.setOutputLimits(0x10, 0x7B); 01339 left_hidarimae.setOutputLimits(0x7C, 0x83); 01340 left_hidariusiro.setOutputLimits(0x7C, 0x83); 01341 } 01342 //停止(目標より行き過ぎ) 01343 else if((x_pulse1 > target) && (x_pulse2 > target)) { 01344 left_migimae.setOutputLimits(0x7C, 0x83); 01345 left_migiusiro.setOutputLimits(0x7C, 0x83); 01346 left_hidarimae.setOutputLimits(0x7C, 0x83); 01347 left_hidariusiro.setOutputLimits(0x7C, 0x83); 01348 } 01349 01350 //よくわからんやつ 01351 left_migimae.setMode(AUTO_MODE); 01352 left_migiusiro.setMode(AUTO_MODE); 01353 left_hidarimae.setMode(AUTO_MODE); 01354 left_hidariusiro.setMode(AUTO_MODE); 01355 01356 //目標値 01357 left_migimae.setSetPoint(target); 01358 left_migiusiro.setSetPoint(target); 01359 left_hidarimae.setSetPoint(target); 01360 left_hidariusiro.setSetPoint(target); 01361 01362 //センサ出力 01363 left_migimae.setProcessValue(x_pulse1); 01364 left_migiusiro.setProcessValue(x_pulse2); 01365 left_hidarimae.setProcessValue(x_pulse1); 01366 left_hidariusiro.setProcessValue(x_pulse2); 01367 01368 //制御量(計算結果) 01369 migimae_data[0] = left_migimae.compute(); 01370 migiusiro_data[0] = left_migiusiro.compute(); 01371 hidarimae_data[0] = left_hidarimae.compute(); 01372 hidariusiro_data[0] = left_hidariusiro.compute(); 01373 01374 //制御量をPWM値に変換 01375 //左進(目標まで達していない) 01376 if((x_pulse1 < target) && (x_pulse2 < target)) { 01377 true_migimae_data[0] = migimae_data[0]; 01378 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01379 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01380 true_hidariusiro_data[0] = hidariusiro_data[0]; 01381 } 01382 //前側が左に出ちゃった♥(後側だけ回して前側は停止) 01383 else if(x_pulse1 > (x_pulse2 + wheel_difference)) { 01384 true_migimae_data[0] = 0x80; 01385 true_migiusiro_data[0] = 0x80; 01386 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01387 true_hidariusiro_data[0] = hidariusiro_data[0]; 01388 } 01389 //後側が左に出ちゃった♥(前側だけ回して後側は停止) 01390 else if((x_pulse1 + wheel_difference) < x_pulse2) { 01391 true_migimae_data[0] = migimae_data[0]; 01392 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01393 true_hidarimae_data[0] = 0x80; 01394 true_hidariusiro_data[0] = 0x80; 01395 } 01396 //停止(目標より行き過ぎ) 01397 else if((x_pulse1 > target) && (x_pulse2 > target)) { 01398 true_migimae_data[0] = 0x80; 01399 true_migiusiro_data[0] = 0x80; 01400 true_hidarimae_data[0] = 0x80; 01401 true_hidariusiro_data[0] = 0x80; 01402 } 01403 } 01404 01405 void turn_right_PID(int target) 01406 { 01407 01408 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01409 turn_right_migimae.setInputLimits(-2147483648, 2147483647); 01410 turn_right_migiusiro.setInputLimits(-2147483648, 2147483647); 01411 turn_right_hidarimae.setInputLimits(-2147483648, 2147483647); 01412 turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647); 01413 01414 //制御量の最小、最大 01415 //右旋回(目標に達してない) 01416 if(x_pulse2 < target) { 01417 turn_right_migimae.setOutputLimits(0x10, 0x7B); 01418 turn_right_migiusiro.setOutputLimits(0x10, 0x7B); 01419 turn_right_hidarimae.setOutputLimits(0x94, 0xFF); 01420 turn_right_hidariusiro.setOutputLimits(0x94, 0xFF); 01421 } 01422 //停止(目標より行き過ぎ) 01423 else if(x_pulse2 > target) { 01424 turn_right_migimae.setOutputLimits(0x7C, 0x83); 01425 turn_right_migiusiro.setOutputLimits(0x7C, 0x83); 01426 turn_right_hidarimae.setOutputLimits(0x7C, 0x83); 01427 turn_right_hidariusiro.setOutputLimits(0x7C, 0x83); 01428 } 01429 01430 //よくわからんやつ 01431 turn_right_migimae.setMode(AUTO_MODE); 01432 turn_right_migiusiro.setMode(AUTO_MODE); 01433 turn_right_hidarimae.setMode(AUTO_MODE); 01434 turn_right_hidariusiro.setMode(AUTO_MODE); 01435 01436 //目標値 01437 turn_right_migimae.setSetPoint(target); 01438 turn_right_migiusiro.setSetPoint(target); 01439 turn_right_hidarimae.setSetPoint(target); 01440 turn_right_hidariusiro.setSetPoint(target); 01441 01442 //センサ出力 01443 turn_right_migimae.setProcessValue(x_pulse2); 01444 turn_right_migiusiro.setProcessValue(x_pulse2); 01445 turn_right_hidarimae.setProcessValue(x_pulse2); 01446 turn_right_hidariusiro.setProcessValue(x_pulse2); 01447 01448 //制御量(計算結果) 01449 migimae_data[0] = turn_right_migimae.compute(); 01450 migiusiro_data[0] = turn_right_migiusiro.compute(); 01451 hidarimae_data[0] = turn_right_hidarimae.compute(); 01452 hidariusiro_data[0] = turn_right_hidariusiro.compute(); 01453 01454 //制御量をPWM値に変換 01455 //右旋回(目標に達してない) 01456 if(x_pulse2 < target) { 01457 true_migimae_data[0] = 0x7B - migimae_data[0]; 01458 true_migiusiro_data[0] = 0x7B - migiusiro_data[0]; 01459 true_hidarimae_data[0] = hidarimae_data[0]; 01460 true_hidariusiro_data[0] = hidariusiro_data[0]; 01461 } 01462 //停止(目標より行き過ぎ) 01463 else if(x_pulse2 > target) { 01464 true_migimae_data[0] = 0x80; 01465 true_migiusiro_data[0] = 0x80; 01466 true_hidarimae_data[0] = 0x80; 01467 true_hidariusiro_data[0] = 0x80; 01468 } 01469 } 01470 01471 void turn_left_PID(int target) 01472 { 01473 01474 //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定) 01475 turn_left_migimae.setInputLimits(-2147483648, 2147483647); 01476 turn_left_migiusiro.setInputLimits(-2147483648, 2147483647); 01477 turn_left_hidarimae.setInputLimits(-2147483648, 2147483647); 01478 turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647); 01479 01480 //制御量の最小、最大 01481 //左旋回(目標に達してない) 01482 if(x_pulse1 < target) { 01483 turn_left_migimae.setOutputLimits(0x94, 0xFF); 01484 turn_left_migiusiro.setOutputLimits(0x94, 0xFF); 01485 turn_left_hidarimae.setOutputLimits(0x10, 0x7B); 01486 turn_left_hidariusiro.setOutputLimits(0x10, 0x7B); 01487 } 01488 //停止(目標より行き過ぎ) 01489 else if(x_pulse1 > target) { 01490 turn_left_migimae.setOutputLimits(0x7C, 0x83); 01491 turn_left_migiusiro.setOutputLimits(0x7C, 0x83); 01492 turn_left_hidarimae.setOutputLimits(0x7C, 0x83); 01493 turn_left_hidariusiro.setOutputLimits(0x7C, 0x83); 01494 } 01495 01496 //よくわからんやつ 01497 turn_left_migimae.setMode(AUTO_MODE); 01498 turn_left_migiusiro.setMode(AUTO_MODE); 01499 turn_left_hidarimae.setMode(AUTO_MODE); 01500 turn_left_hidariusiro.setMode(AUTO_MODE); 01501 01502 //目標値 01503 turn_left_migimae.setSetPoint(target); 01504 turn_left_migiusiro.setSetPoint(target); 01505 turn_left_hidarimae.setSetPoint(target); 01506 turn_left_hidariusiro.setSetPoint(target); 01507 01508 //センサ出力 01509 turn_left_migimae.setProcessValue(x_pulse1); 01510 turn_left_migiusiro.setProcessValue(x_pulse1); 01511 turn_left_hidarimae.setProcessValue(x_pulse1); 01512 turn_left_hidariusiro.setProcessValue(x_pulse1); 01513 01514 //制御量(計算結果) 01515 migimae_data[0] = turn_left_migimae.compute(); 01516 migiusiro_data[0] = turn_left_migiusiro.compute(); 01517 hidarimae_data[0] = turn_left_hidarimae.compute(); 01518 hidariusiro_data[0] = turn_left_hidariusiro.compute(); 01519 01520 //制御量をPWM値に変換 01521 //左旋回(目標に達してない) 01522 if(x_pulse1 < target) { 01523 true_migimae_data[0] = migimae_data[0]; 01524 true_migiusiro_data[0] = migiusiro_data[0]; 01525 true_hidarimae_data[0] = 0x7B - hidarimae_data[0]; 01526 true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0]; 01527 } 01528 //左旋回(目標より行き過ぎ) 01529 else if(x_pulse1 > target) { 01530 true_migimae_data[0] = 0x80; 01531 true_migiusiro_data[0] = 0x80; 01532 true_hidarimae_data[0] = 0x80; 01533 true_hidariusiro_data[0] = 0x80; 01534 } 01535 } 01536 01537 void dondonkasoku(void) 01538 { 01539 01540 //どんどん加速(正転) 01541 for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++) { 01542 i2c.write(0x10, init_send_data, 1); 01543 i2c.write(0x12, init_send_data, 1); 01544 i2c.write(0x14, init_send_data, 1); 01545 i2c.write(0x16, init_send_data, 1); 01546 wait(0.05); 01547 } 01548 //どんどん減速(正転) 01549 for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--) { 01550 i2c.write(0x10, init_send_data, 1); 01551 i2c.write(0x12, init_send_data, 1); 01552 i2c.write(0x14, init_send_data, 1); 01553 i2c.write(0x16, init_send_data, 1); 01554 wait(0.05); 01555 } 01556 //だんだん加速(逆転) 01557 for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--) { 01558 i2c.write(0x10, init_send_data, 1); 01559 i2c.write(0x12, init_send_data, 1); 01560 i2c.write(0x14, init_send_data, 1); 01561 i2c.write(0x16, init_send_data, 1); 01562 wait(0.05); 01563 } 01564 //だんだん減速(逆転) 01565 for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++) { 01566 i2c.write(0x10, init_send_data, 1); 01567 i2c.write(0x12, init_send_data, 1); 01568 i2c.write(0x14, init_send_data, 1); 01569 i2c.write(0x16, init_send_data, 1); 01570 wait(0.05); 01571 } 01572 }
Generated on Tue Aug 30 2022 01:09:47 by
1.7.2