9月18日実験用

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