ver6_2の修正版 変更点 phase1のバグ kaisyu関数 tyokudo関数

Dependencies:   mbed QEI PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }