2019年9月30日AM02:00現在のもの

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