2019年9月28日AM10:25現在のもの

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 /* added red zone(checked)                                              */
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 
00083 //遠隔非常停止ユニットLED
00084 AnalogOut myled(A2);
00085 
00086 DigitalIn start_switch(PB_12);
00087 DigitalIn USR_SWITCH(PC_13);
00088 DigitalIn zone_switch(PC_10);
00089 
00090 QEI wheel_x1(PA_8 , PA_6 , NC, 624);
00091 QEI wheel_x2(PB_14, PB_13, NC, 624);
00092 QEI wheel_y1(PB_1 , PB_15, NC, 624);
00093 QEI wheel_y2(PA_12, PA_11, NC, 624);
00094 QEI arm_enc(PB_5,   PB_4 , NC, 624);
00095 
00096 //移動後n秒停止タイマー
00097 Timer counter;
00098 
00099 //エンコーダ値格納変数
00100 int x_pulse1, x_pulse2, y_pulse1, y_pulse2, sum_pulse, arm_pulse;
00101 
00102 //操作の段階変数
00103 unsigned int phase = 0;
00104 int kaisyu_phase = 0;
00105 int tyokudo_phase = 0;
00106 unsigned int start_zone = 1;
00107 bool zone = RED;
00108 
00109 //i2c送信データ変数
00110 char init_send_data[1];
00111 char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
00112 char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
00113 char arm_motor[1], drop_motor[1];
00114 char fan_data[1];
00115 char servo_data[1];
00116 char right_arm_data[1], left_arm_data[1];
00117 
00118 //非常停止関連変数
00119 char RDATA;
00120 char baff;
00121 int flug = 0;
00122 
00123 //リミット基板からの受信データ
00124 int limit_data = 0;
00125 int upper_limit_data = 0;
00126 int lower_limit_data = 0;
00127 
00128 //各辺のスイッチが押されたかのフラグ
00129 //前部が壁に当たっているか
00130 int front_limit = 0;
00131 //右部が壁にあたあっているか
00132 int right_limit = 0;
00133 //後部が壁に当たっているか
00134 int back_limit = 0;
00135 //回収機構の下限(引っ込めてるほう)
00136 bool kaisyu_mae_limit = 0;
00137 
00138 bool kaisyu_usiro_limit = 0;
00139 
00140 //右腕の下限
00141 bool right_arm_lower_limit = 0;
00142 //右腕の上限
00143 bool right_arm_upper_limit = 0;
00144 //左腕の下限
00145 bool left_arm_lower_limit = 0;
00146 //左腕の上限
00147 bool left_arm_upper_limit = 0;
00148 //吐き出し機構の上限
00149 bool tyokudo_mae_limit = 0;
00150 //吐き出し機構の下限
00151 bool tyokudo_usiro_limit = 0;
00152 
00153 int masked_lower_front_limit_data     = 0xFF;
00154 int masked_lower_back_limit_data      = 0xFF;
00155 int masked_lower_right_limit_data     = 0xFF;
00156 int masked_kaisyu_mae_limit_data      = 0xFF;
00157 int masked_kaisyu_usiro_limit_data    = 0xFF;
00158 int masked_right_arm_lower_limit_data = 0xFF;
00159 int masked_right_arm_upper_limit_data = 0xFF;
00160 int masked_left_arm_lower_limit_data  = 0xFF;
00161 int masked_left_arm_upper_limit_data  = 0xFF;
00162 int masked_tyokudo_mae_limit_data     = 0xFF;
00163 int masked_tyokudo_usiro_limit_data   = 0xFF;
00164 
00165 //関数のプロトタイプ宣言
00166 void init(void);
00167 void init_send(void);
00168 void get(void);
00169 void get_pulses(void);
00170 void print_pulses(void);
00171 void get_emergency(void);
00172 void read_limit(void);
00173 void wheel_reset(void);
00174 void kaisyu(int pulse, int next_phase);
00175 void kaisyu_nobasu(int pulse, int next_phase);
00176 void kaisyu_hiku(int pulse, int next_phase);
00177 void tyokudo(int pulse, int next_phase);
00178 void arm_up(int next_phase);
00179 void front(int target);
00180 void back(int target);
00181 void right(int target);
00182 void left(int target);
00183 void turn_right(int target);
00184 void turn_left(int target);
00185 void stop(void);
00186 void all_stop(void);
00187 void front_PID(int target);
00188 void back_PID(int target);
00189 void right_PID(int target);
00190 void left_PID(int target);
00191 void turn_right_PID(int target);
00192 void turn_left_PID(int target);
00193 
00194 int main(void) {
00195 
00196     init();
00197     init_send();
00198 
00199     //とりあえず(後で消してね)
00200     //phase = 22;   //スタートゾーン2から
00201     //phase = 36;   //アームアップ
00202     //phase = 50;   //default
00203     
00204     //起動時にゾーンを読んでからループに入る(試合中誤ってスイッチ押すのを防止)
00205     while(1) {
00206         if(zone_switch == 0) {
00207             zone = BLUE;
00208         } else {
00209             zone = RED;
00210         }
00211         break;
00212     }
00213         
00214     while(1) {
00215 
00216         get_pulses();
00217         //print_pulses();
00218         get_emergency();
00219         read_limit();
00220                 
00221         //move_servo_with_using_onboard-switch
00222         if(USR_SWITCH == 0) {
00223             servo_data[0] = 0x03;
00224             i2c.write(0x30, servo_data, 1);
00225         } else {
00226             servo_data[0] = 0x04;
00227             i2c.write(0x30, servo_data, 1);
00228         }
00229         
00230         /*
00231         if(start_switch == 1) {
00232             phase = 39;
00233         }
00234         */
00235         
00236         //青ゾーン
00237         if(zone == BLUE) {
00238             GREEN_LED = 1;
00239             RED_LED = 0;
00240             
00241             switch(phase) {
00242 
00243                 //スタート位置へセット
00244                 case 0:
00245                     //リミットが洗濯物台に触れているか
00246                     if(right_limit == 3) {
00247                         USR_LED1 = 1;
00248                         //スタートスイッチが押されたか
00249                         if(start_switch == 1) {
00250                             wheel_reset();
00251                             phase = 1;
00252                         }
00253                     } else {
00254                         USR_LED1 = 0;
00255                     }
00256                     break;
00257 
00258                 //回収アームを伸ばす
00259                 case 1:
00260                     counter.reset();
00261                     //kaisyu(arm_enc.getPulses(), 2);
00262                     kaisyu_nobasu(arm_pulse, 2);
00263                     //サーボを開いておく
00264                     servo_data[0] = 0x03;
00265                     i2c.write(0x30, servo_data, 1);
00266                     break;
00267 
00268                 //1秒停止
00269                 case 2:
00270                     stop();
00271                     servo_data[0] = 0x04;
00272                     i2c.write(0x30, servo_data, 1);
00273                     counter.start();
00274                     if(counter.read() > 1.0f) {
00275                         phase = 3;
00276                         wheel_reset();
00277                     }
00278                     break;
00279 
00280                 //ちょっと後進
00281                 case 3:
00282                     counter.reset();
00283                     back(-800);
00284                     if((y_pulse1*-1 > 800) || (y_pulse2*-1 > 800)) {
00285                         phase = 4;
00286                     }
00287                     break;
00288 
00289                 //1秒停止
00290                 case 4:
00291                     stop();
00292                     counter.start();
00293                     if(counter.read() > 1.0f) {
00294                         phase = 5;
00295                         wheel_reset();
00296                     }
00297                     break;
00298 
00299                 //回収アーム引っ込める
00300                 case 5:
00301                     counter.reset();
00302                     kaisyu_hiku(arm_pulse, 6);
00303                     break;
00304 
00305                 //1秒停止
00306                 case 6:
00307                     stop();
00308                     counter.start();
00309                     if(counter.read() > 1.0f) {
00310                         phase = 7;
00311                         wheel_reset();
00312                     }
00313                     break;
00314 
00315                 //左移動
00316                 case 7:
00317                     counter.reset();
00318                     left(11500);
00319                     if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
00320                         phase = 8;
00321                     }
00322                     break;
00323 
00324                 //1秒停止
00325                 case 8:
00326                     stop();
00327                     counter.start();
00328                     if(counter.read() > 1.0f) {
00329                         phase = 9;
00330                         wheel_reset();
00331                     }
00332                     break;
00333 
00334                 //右旋回(180°)
00335                 case 9:
00336                     counter.reset();
00337                     turn_right(975);
00338                     if(sum_pulse > 975) {
00339                         phase = 10;
00340                     }
00341                     break;
00342 
00343                 //1秒停止
00344                 case 10:
00345                     stop();
00346                     counter.start();
00347                     if(counter.read() > 1.0f) {
00348                         phase = 11;
00349                         wheel_reset();
00350                     }
00351                     break;
00352 
00353                 //壁に当たるまで後進
00354                 case 11:
00355                     counter.reset();
00356 
00357                     if(back_limit == 3) {
00358                         phase = 12;
00359                     }
00360                     else if(back_limit != 3){
00361                         true_migimae_data[0]     = 0x50;
00362                         true_migiusiro_data[0]   = 0x50;
00363                         true_hidarimae_data[0]   = 0x50;
00364                         true_hidariusiro_data[0] = 0x50;
00365                         i2c.write(0x10, true_migimae_data,     1, false);
00366                         i2c.write(0x12, true_migiusiro_data,   1, false);
00367                         i2c.write(0x14, true_hidarimae_data,   1, false);
00368                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00369                         wait_us(20);
00370                     }
00371                     break;
00372 
00373                 //1秒停止
00374                 case 12:
00375                     stop();
00376                     counter.start();
00377                     if(counter.read() > 1.0f) {
00378                         phase = 13;
00379                         wheel_reset();
00380                     }
00381                     break;
00382 
00383                 //壁に当たるまで右移動
00384                 case 13:
00385                     counter.reset();
00386 
00387                     if(right_limit == 3) {
00388                         phase = 14;
00389                     }
00390                     else if(right_limit != 3) {
00391                         true_migimae_data[0]     = 0x40;
00392                         true_migiusiro_data[0]   = 0xBF;
00393                         true_hidarimae_data[0]   = 0xBF;
00394                         true_hidariusiro_data[0] = 0x40;
00395                         i2c.write(0x10, true_migimae_data,     1, false);
00396                         i2c.write(0x12, true_migiusiro_data,   1, false);
00397                         i2c.write(0x14, true_hidarimae_data,   1, false);
00398                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00399                         wait_us(20);
00400                     }
00401                     break;
00402 
00403                 //1秒停止
00404                 case 14:
00405                     stop();
00406                     counter.start();
00407                     if(counter.read() > 1.0f) {
00408                         phase = 15;
00409                         wheel_reset();
00410                     }
00411                     break;
00412 
00413                 //排出
00414                 case 15:
00415                     counter.reset();
00416                     tyokudo(arm_enc.getPulses(), 16);
00417                     break;
00418 
00419                 //1秒停止
00420                 case 16:
00421                     stop();
00422                     counter.start();
00423                     if(counter.read() > 1.0f) {
00424                         phase = 17;
00425                         wheel_reset();
00426                     }
00427                     break;
00428 
00429                 //前進
00430                 case 17:
00431                     counter.reset();
00432                     front(5000);
00433                     if((y_pulse1 > 5000) || (y_pulse2 > 5000)) {
00434                         phase = 18;
00435                     }
00436                     break;
00437 
00438                 //1秒停止
00439                 case 18:
00440                     stop();
00441                     counter.start();
00442                     if(counter.read() > 1.0f) {
00443                         phase = 19;
00444                         wheel_reset();
00445                     }
00446                     break;
00447 
00448                 //壁に当たるまで右移動
00449                 case 19:
00450                     counter.reset();
00451 
00452                     if(right_limit == 3) {
00453                         phase = 20;
00454                     }
00455                     else if(right_limit != 3) {
00456                         true_migimae_data[0]     = 0x40;
00457                         true_migiusiro_data[0]   = 0xBF;
00458                         true_hidarimae_data[0]   = 0xBF;
00459                         true_hidariusiro_data[0] = 0x40;
00460                         i2c.write(0x10, true_migimae_data,     1, false);
00461                         i2c.write(0x12, true_migiusiro_data,   1, false);
00462                         i2c.write(0x14, true_hidarimae_data,   1, false);
00463                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00464                         wait_us(20);
00465                     }
00466                     break;
00467 
00468                 //1秒停止
00469                 case 20:
00470                     stop();
00471                     counter.start();
00472                     if(counter.read() > 1.0f) {
00473                         phase = 21;
00474                         wheel_reset();
00475                     }
00476                     break;
00477 
00478                 //壁に当たるまで後進
00479                 case 21:
00480                     counter.reset();
00481 
00482                     if(back_limit == 3) {
00483                         phase = 22;
00484                     }
00485                     else if(back_limit != 3){
00486                         true_migimae_data[0]     = 0x50;
00487                         true_migiusiro_data[0]   = 0x50;
00488                         true_hidarimae_data[0]   = 0x50;
00489                         true_hidariusiro_data[0] = 0x50;
00490                         i2c.write(0x10, true_migimae_data,     1, false);
00491                         i2c.write(0x12, true_migiusiro_data,   1, false);
00492                         i2c.write(0x14, true_hidarimae_data,   1, false);
00493                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00494                         wait_us(20);
00495                     }
00496                     break;
00497 
00498                 //シーツ装填
00499                 case 22:
00500                     if(start_switch == 1) {
00501                         wheel_reset();
00502                         phase = 23;
00503                     } else {
00504                         stop();
00505                     }
00506                     break;
00507 
00508                 //竿のラインまで前進
00509                 case 23:
00510                     counter.reset();
00511                     front(20500);
00512                     if((y_pulse1 > 20500) || (y_pulse2 > 20500)) {
00513                         phase = 24;
00514                     }
00515                     break;
00516 
00517                 //1秒停止
00518                 case 24:
00519                     stop();
00520                     counter.start();
00521                     if(counter.read() > 1.0f) {
00522                         phase = 25;
00523                         wheel_reset();
00524                     }
00525                     break;
00526 
00527                 //ちょっと左移動
00528                 case 25:
00529                     counter.reset();
00530                     left(500);
00531                     if((x_pulse1 > 500) || (x_pulse2 > 500)) {
00532                         phase = 26;
00533                     }
00534                     break;
00535 
00536                 //1秒停止
00537                 case 26:
00538                     stop();
00539                     counter.start();
00540                     if(counter.read() > 1.0f) {
00541                         phase = 27;
00542                         wheel_reset();
00543                     }
00544                     break;
00545 
00546                 //90°右旋回
00547                 case 27:
00548                     counter.reset();
00549                     turn_right(465);
00550                     if(sum_pulse > 465) {
00551                         phase = 28;
00552                     }
00553                     break;
00554 
00555                 //1秒停止
00556                 case 28:
00557                     stop();
00558                     counter.start();
00559                     if(counter.read() > 1.0f) {
00560                         phase = 29;
00561                         wheel_reset();
00562                     }
00563                     break;
00564 
00565                 //壁に当たるまで前進
00566                 case 29:
00567                     counter.reset();
00568                     if(front_limit == 3) {
00569                         phase = 30;
00570                     }
00571                     else if(front_limit != 3){
00572                         true_migimae_data[0]     = 0xC0;
00573                         true_migiusiro_data[0]   = 0xC0;
00574                         true_hidarimae_data[0]   = 0xC0;
00575                         true_hidariusiro_data[0] = 0xC0;
00576                         i2c.write(0x10, true_migimae_data,     1, false);
00577                         i2c.write(0x12, true_migiusiro_data,   1, false);
00578                         i2c.write(0x14, true_hidarimae_data,   1, false);
00579                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00580                         wait_us(20);
00581                     }
00582                     break;
00583 
00584                 //1秒停止
00585                 case 30:
00586                     stop();
00587                     counter.start();
00588                     if(counter.read() > 1.0f) {
00589                         phase = 31;
00590                         wheel_reset();
00591                     }
00592                     break;
00593 
00594                 //掛けるところまで後進
00595                 case 31:
00596                     counter.reset();
00597                     back(-10000);
00598                     if((y_pulse1*-1 > 10000) || (y_pulse2*-1 > 10000)) {
00599                         phase = 32;
00600                         counter.start();
00601                     }
00602                     break;
00603 
00604                 //1秒停止
00605                 case 32:
00606                     stop();
00607                     counter.start();
00608                     if(counter.read() > 1.0f) {
00609                         phase = 33;
00610                         wheel_reset();
00611                     }
00612                     break;
00613 
00614                 //妨害防止の右旋回
00615                 case 33:
00616                     counter.reset();
00617                     turn_right(30);
00618                     if(sum_pulse > 30) {
00619                         phase = 34;
00620                     }
00621                     break;
00622 
00623                 //1秒停止
00624                 case 34:
00625                     stop();
00626                     counter.start();
00627                     if(counter.read() > 1.0f) {
00628                         phase = 35;
00629                         wheel_reset();
00630                     }
00631                     break;
00632 
00633                 //カウンターリセット
00634                 case 35:
00635                     counter.reset();
00636                     counter.start();
00637                     phase = 36;
00638                     break;
00639 
00640                 //アームアップ
00641                 case 36:
00642                     stop();
00643                     //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
00644                     if(counter.read() < 1.0f) {
00645                         right_arm_data[0] = 0xFF;
00646                         left_arm_data[0]  = 0xFF;
00647                         i2c.write(0x22, right_arm_data, 1);
00648                         i2c.write(0x24, left_arm_data, 1);
00649                         wait_us(20);
00650                     } else {
00651                         arm_up(37);
00652                     }
00653                     break;
00654 
00655                 //カウンターリセット
00656                 case 37:
00657                     counter.reset();
00658                     phase = 38;
00659                     break;
00660 
00661                 //シーツを掛ける
00662                 case 38:
00663                     counter.start();
00664 
00665                     //1秒間ファン送風
00666                     if(counter.read() <= 2.0f) {
00667                         fan_data[0] = 0xFF;
00668                         i2c.write(0x26, fan_data, 1);
00669                         i2c.write(0x28, fan_data, 1);
00670                         servo_data[0] = 0x04;
00671                         i2c.write(0x30, servo_data, 1);
00672                     }
00673                     //1~3秒の間でサーボを開放
00674                     else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
00675                         fan_data[0] = 0xFF;
00676                         i2c.write(0x26, fan_data, 1);
00677                         i2c.write(0x28, fan_data, 1);
00678                         servo_data[0] = 0x03;
00679                         i2c.write(0x30, servo_data, 1);
00680                     }
00681                     //3秒過ぎたら終わり
00682                     else if(counter.read() > 4.0f) {
00683                         fan_data[0] = 0x80;
00684                         i2c.write(0x26, fan_data, 1);
00685                         i2c.write(0x28, fan_data, 1);
00686                         servo_data[0] = 0x04;
00687                         i2c.write(0x30, servo_data, 1);
00688                         phase = 39;
00689                     }
00690                     break;
00691 
00692                 //終了っ!(守衛さん風)
00693                 case 39:
00694                 default:
00695                     //駆動系統OFF
00696                     all_stop();
00697                     break;
00698             }
00699         }
00700         
00701         //REDゾーン
00702         else if(zone == RED) {
00703             GREEN_LED = 0;
00704             RED_LED = 1;
00705             
00706             switch(phase) {
00707 
00708                 //スタート位置へセット
00709                 case 0:
00710                     //リミットが洗濯物台に触れているか
00711                     if(right_limit == 3) {
00712                         USR_LED1 = 1;
00713                         //スタートスイッチが押されたか
00714                         if(start_switch == 1) {
00715                             wheel_reset();
00716                             phase = 1;
00717                         }
00718                     } else {
00719                         USR_LED1 = 0;
00720                     }
00721                     break;
00722 
00723                 //回収アームを伸ばす
00724                 case 1:
00725                     counter.reset();
00726                     //kaisyu(arm_enc.getPulses(), 2);
00727                     kaisyu_nobasu(arm_pulse, 2);
00728                     //サーボを開いておく
00729                     servo_data[0] = 0x03;
00730                     i2c.write(0x30, servo_data, 1);
00731                     break;
00732 
00733                 //1秒停止
00734                 case 2:
00735                     stop();
00736                     servo_data[0] = 0x04;
00737                     i2c.write(0x30, servo_data, 1);
00738                     counter.start();
00739                     if(counter.read() > 1.0f) {
00740                         phase = 3;
00741                         wheel_reset();
00742                     }
00743                     break;
00744 
00745                 //ちょっと前進
00746                 case 3:
00747                     counter.reset();
00748                     front(800);
00749                     if((y_pulse1 > 800) || (y_pulse2 > 800)) {
00750                         phase = 4;
00751                     }
00752                     break;
00753                     
00754                 //1秒停止
00755                 case 4:
00756                     stop();
00757                     counter.start();
00758                     if(counter.read() > 1.0f) {
00759                         phase = 5;
00760                         wheel_reset();
00761                     }
00762                     break;
00763                     
00764                 //回収アーム引っ込める
00765                 case 5:
00766                     counter.reset();
00767                     kaisyu_hiku(arm_pulse, 6);
00768                     break;
00769                     
00770                 //1秒停止
00771                 case 6:
00772                     stop();
00773                     counter.start();
00774                     if(counter.read() > 1.0f) {
00775                         phase = 7;
00776                         wheel_reset();
00777                     }
00778                     break;        
00779                         
00780                 //左移動
00781                 case 7:
00782                     counter.reset();
00783                     left(11500);
00784                     if((x_pulse1 > 11500) || (x_pulse2 > 11500)) {
00785                         phase = 8;
00786                     }
00787                     break;
00788 
00789                 //1秒停止
00790                 case 8:
00791                     stop();
00792                     counter.start();
00793                     if(counter.read() > 1.0f) {
00794                         phase = 9;
00795                         wheel_reset();
00796                     }
00797                     break;
00798 
00799                 //右旋回(180°)
00800                 case 9:
00801                     counter.reset();
00802                     turn_right(975);
00803                     if(sum_pulse > 975) {
00804                         phase = 10;
00805                     }
00806                     break;
00807 
00808                 //1秒停止
00809                 case 10:
00810                     stop();
00811                     counter.start();
00812                     if(counter.read() > 1.0f) {
00813                         phase = 11;
00814                         wheel_reset();
00815                     }
00816                     break;            
00817                     
00818                 //壁に当たるまで前進
00819                 case 11:
00820                     counter.reset();
00821                     
00822                     if(front_limit == 3) {
00823                         phase = 12;
00824                     }
00825                     else if(front_limit != 3){
00826                         true_migimae_data[0]     = 0xC0;
00827                         true_migiusiro_data[0]   = 0xC0;
00828                         true_hidarimae_data[0]   = 0xC0;
00829                         true_hidariusiro_data[0] = 0xC0;
00830                         i2c.write(0x10, true_migimae_data,     1, false);
00831                         i2c.write(0x12, true_migiusiro_data,   1, false);
00832                         i2c.write(0x14, true_hidarimae_data,   1, false);
00833                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00834                         wait_us(20);
00835                     }
00836                     break;
00837                     
00838                 //1秒停止
00839                 case 12:
00840                     stop();
00841                     counter.start();
00842                     if(counter.read() > 1.0f) {
00843                         phase = 13;
00844                         wheel_reset();
00845                     }
00846                     break;
00847 
00848                 //壁に当たるまで右移動
00849                 case 13:
00850                     counter.reset();        
00851                                 
00852                     if(right_limit == 3) {
00853                         phase = 14;
00854                     }
00855                     else if(right_limit != 3) {
00856                         true_migimae_data[0]     = 0x40;
00857                         true_migiusiro_data[0]   = 0xBF;
00858                         true_hidarimae_data[0]   = 0xBF;
00859                         true_hidariusiro_data[0] = 0x40;
00860                         i2c.write(0x10, true_migimae_data,     1, false);
00861                         i2c.write(0x12, true_migiusiro_data,   1, false);
00862                         i2c.write(0x14, true_hidarimae_data,   1, false);
00863                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00864                         wait_us(20);   
00865                     }
00866                     break;
00867 
00868                 //1秒停止
00869                 case 14:
00870                     stop();
00871                     counter.start();
00872                     if(counter.read() > 1.0f) {
00873                         phase = 15;
00874                         wheel_reset();
00875                     }
00876                     break;
00877 
00878                 //排出
00879                 case 15:
00880                     counter.reset();
00881                     tyokudo(arm_enc.getPulses(), 16);
00882                     break;
00883 
00884                 //1秒停止
00885                 case 16:
00886                     stop();
00887                     counter.start();
00888                     if(counter.read() > 1.0f) {
00889                         phase = 17;
00890                         wheel_reset();
00891                     }
00892                     break;
00893 
00894                 //後進
00895                 case 17:
00896                     counter.reset();
00897                     back(-5000);
00898                     if((y_pulse1*-1 > 5000) || (y_pulse2*-1 > 5000)) {
00899                         phase = 18;
00900                     }
00901                     break;
00902 
00903                 //1秒停止
00904                 case 18:
00905                     stop();
00906                     counter.start();
00907                     if(counter.read() > 1.0f) {
00908                         phase = 19;
00909                         wheel_reset();
00910                     }
00911                     break;
00912 
00913                 //壁に当たるまで右移動
00914                 case 19:
00915                     counter.reset();
00916                     
00917                     if(right_limit == 3) {
00918                         phase = 20;
00919                     }
00920                     else if(right_limit != 3) {
00921                         true_migimae_data[0]     = 0x40;
00922                         true_migiusiro_data[0]   = 0xBF;
00923                         true_hidarimae_data[0]   = 0xBF;
00924                         true_hidariusiro_data[0] = 0x40;
00925                         i2c.write(0x10, true_migimae_data,     1, false);
00926                         i2c.write(0x12, true_migiusiro_data,   1, false);
00927                         i2c.write(0x14, true_hidarimae_data,   1, false);
00928                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00929                         wait_us(20);   
00930                     }
00931                     break;
00932                     
00933                 //1秒停止
00934                 case 20:
00935                     stop();
00936                     counter.start();
00937                     if(counter.read() > 1.0f) {
00938                         phase = 21;
00939                         wheel_reset();
00940                     }
00941                     break;
00942                     
00943                 //壁に当たるまで前進
00944                 case 21:
00945                     counter.reset();
00946                     
00947                     if(front_limit == 3) {
00948                         phase = 22;
00949                     }
00950                     else if(front_limit != 3){
00951                         true_migimae_data[0]     = 0xC0;
00952                         true_migiusiro_data[0]   = 0xC0;
00953                         true_hidarimae_data[0]   = 0xC0;
00954                         true_hidariusiro_data[0] = 0xC0;
00955                         i2c.write(0x10, true_migimae_data,     1, false);
00956                         i2c.write(0x12, true_migiusiro_data,   1, false);
00957                         i2c.write(0x14, true_hidarimae_data,   1, false);
00958                         i2c.write(0x16, true_hidariusiro_data, 1, false);
00959                         wait_us(20);
00960                     }
00961                     break;
00962 
00963                 //シーツ装填
00964                 case 22:
00965                     if(start_switch == 1) {
00966                         wheel_reset();
00967                         phase = 23;
00968                     } else {
00969                         stop();
00970                     }
00971                     break;
00972 
00973                 //竿のラインまで後進
00974                 case 23:
00975                     counter.reset();
00976                     back(-20500);
00977                     if((y_pulse1*-1 > 20500) || (y_pulse2*-1 > 20500)) {
00978                         phase = 24;
00979                     }
00980                     break;
00981 
00982                 //1秒停止
00983                 case 24:
00984                     stop();
00985                     counter.start();
00986                     if(counter.read() > 1.0f) {
00987                         phase = 25;
00988                         wheel_reset();
00989                     }
00990                     break;
00991                     
00992                 //ちょっと左移動
00993                 case 25:
00994                     counter.reset();
00995                     left(500);
00996                     if((x_pulse1 > 500) || (x_pulse2 > 500)) {
00997                         phase = 26;
00998                     }
00999                     break;             
01000 
01001                 //1秒停止
01002                 case 26:
01003                     stop();
01004                     counter.start();
01005                     if(counter.read() > 1.0f) {
01006                         phase = 27;
01007                         wheel_reset();
01008                     }
01009                     break;
01010                     
01011                 //90°左旋回
01012                 case 27:
01013                     counter.reset();
01014                     //turn_left(465);
01015                     turn_left(485);
01016                     if(sum_pulse > 485) {
01017                         phase = 28;
01018                     }
01019                     break;
01020                     
01021                 //1秒停止
01022                 case 28:
01023                     stop();
01024                     counter.start();
01025                     if(counter.read() > 1.0f) {
01026                         phase = 29;
01027                         wheel_reset();
01028                     }
01029                     break;
01030                     
01031                 //壁に当たるまで後進
01032                 case 29:
01033                     counter.reset();
01034                     
01035                     if(back_limit == 3) {
01036                         phase = 30;
01037                     } 
01038                     else if(back_limit != 3){
01039                         true_migimae_data[0]     = 0x50;
01040                         true_migiusiro_data[0]   = 0x50;
01041                         true_hidarimae_data[0]   = 0x50;
01042                         true_hidariusiro_data[0] = 0x50;
01043                         i2c.write(0x10, true_migimae_data,     1, false);
01044                         i2c.write(0x12, true_migiusiro_data,   1, false);
01045                         i2c.write(0x14, true_hidarimae_data,   1, false);
01046                         i2c.write(0x16, true_hidariusiro_data, 1, false);
01047                         wait_us(20);    
01048                     }
01049                     break;
01050                     
01051                 //1秒停止
01052                 case 30:
01053                     stop();
01054                     counter.start();
01055                     if(counter.read() > 1.0f) {
01056                         phase = 31;
01057                         wheel_reset();
01058                     }
01059                     break;
01060                     
01061                 //掛けるところまで前進
01062                 case 31:
01063                     counter.reset();
01064                     front(10000);
01065                     if((y_pulse1 > 10000) || (y_pulse2 > 10000)) {
01066                         phase = 32;
01067                         counter.start();
01068                     }
01069                     break;
01070                     
01071                 //1秒停止
01072                 case 32:
01073                     stop();
01074                     counter.start();
01075                     if(counter.read() > 1.0f) {
01076                         phase = 33;
01077                         wheel_reset();
01078                     }
01079                     break;
01080 
01081                 //妨害防止の左旋回
01082                 case 33:
01083                     counter.reset();
01084                     turn_left(30);
01085                     if(sum_pulse > 30) {
01086                         phase = 34;
01087                     }
01088                     break;
01089 
01090                 //1秒停止
01091                 case 34:
01092                     stop();
01093                     counter.start();
01094                     if(counter.read() > 1.0f) {
01095                         phase = 35;
01096                         wheel_reset();
01097                     }
01098                     break;
01099 
01100                 //カウンターリセット
01101                 case 35:
01102                     counter.reset();
01103                     counter.start();
01104                     phase = 36;
01105                     break;
01106                     
01107                 //アームアップ
01108                 case 36:
01109                     stop();
01110                     //3秒間リミットを読まずに無条件で上昇(チャタリングによる誤作動防止)
01111                     if(counter.read() < 1.0f) {
01112                         right_arm_data[0] = 0xFF;
01113                         left_arm_data[0]  = 0xFF;
01114                         i2c.write(0x22, right_arm_data, 1);
01115                         i2c.write(0x24, left_arm_data, 1);
01116                         wait_us(20);
01117                     } else {
01118                         arm_up(37);
01119                     }
01120                     break;
01121                     
01122                 //カウンターリセット
01123                 case 37:
01124                     counter.reset();
01125                     phase = 38;
01126                     break;
01127                 
01128                 //シーツを掛ける
01129                 case 38:
01130                     counter.start();
01131                     
01132                     //1秒間ファン送風
01133                     if(counter.read() <= 2.0f) {
01134                         fan_data[0] = 0xFF;
01135                         i2c.write(0x26, fan_data, 1);
01136                         i2c.write(0x28, fan_data, 1);
01137                         servo_data[0] = 0x04;
01138                         i2c.write(0x30, servo_data, 1);
01139                     }
01140                     //1~3秒の間でサーボを開放
01141                     else if((counter.read() > 2.0f) && (counter.read() <= 4.0f)) {
01142                         fan_data[0] = 0xFF;
01143                         i2c.write(0x26, fan_data, 1);
01144                         i2c.write(0x28, fan_data, 1);
01145                         servo_data[0] = 0x03;
01146                         i2c.write(0x30, servo_data, 1);
01147                     }
01148                     //3秒過ぎたら終わり
01149                     else if(counter.read() > 4.0f) {
01150                         fan_data[0] = 0x80;
01151                         i2c.write(0x26, fan_data, 1);
01152                         i2c.write(0x28, fan_data, 1);
01153                         servo_data[0] = 0x04;
01154                         i2c.write(0x30, servo_data, 1);
01155                         phase = 39;
01156                     }
01157                     break;
01158 
01159                 //終了っ!(守衛さん風)
01160                 case 39:
01161                 default:
01162                     //駆動系統OFF
01163                     all_stop();
01164                     break;
01165             }
01166         }
01167     }
01168 }
01169 
01170 void init(void) {
01171 
01172     //通信ボーレートの設定
01173     pc.baud(460800);
01174 
01175     limit_serial.baud(115200);
01176 
01177     start_switch.mode(PullUp);
01178     zone_switch.mode(PullDown);
01179 
01180     //非常停止関連
01181     pic.baud(19200);
01182     pic.format(8, Serial::None, 1);
01183     pic.attach(get, Serial::RxIrq);
01184 
01185     x_pulse1 = 0;   x_pulse2 = 0;   y_pulse1 = 0;   y_pulse2 = 0;   sum_pulse = 0;
01186     migimae_data[0] = 0x80; migiusiro_data[0] = 0x80;   hidarimae_data[0] = 0x80;   hidariusiro_data[0] = 0x80;
01187     true_migimae_data[0] = 0x80;    true_migiusiro_data[0] = 0x80;  true_hidarimae_data[0] = 0x80;  true_hidariusiro_data[0] = 0x80;
01188     fan_data[0] = 0x80;
01189     servo_data[0] = 0x80;
01190     arm_motor[0] = 0x80;    drop_motor[0] = 0x80;
01191     right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
01192 }
01193 
01194 void init_send(void) {
01195 
01196     init_send_data[0] = 0x80;
01197     i2c.write(0x10, init_send_data, 1);
01198     i2c.write(0x12, init_send_data, 1);
01199     i2c.write(0x14, init_send_data, 1);
01200     i2c.write(0x16, init_send_data, 1);
01201     i2c.write(0x18, init_send_data, 1);
01202     i2c.write(0x20, init_send_data, 1);
01203     i2c.write(0x22, init_send_data, 1);
01204     i2c.write(0x24, init_send_data, 1);
01205     i2c.write(0x30, init_send_data, 1);
01206     wait(0.1);
01207 }
01208 
01209 void get(void) {
01210 
01211     baff = pic.getc();
01212 
01213     for(; flug; flug--)
01214         RDATA = baff;
01215 
01216     if(baff == ':')
01217         flug = 1;
01218 }
01219 
01220 void get_pulses(void) {
01221 
01222         x_pulse1 = wheel_x1.getPulses();
01223         x_pulse2 = wheel_x2.getPulses();
01224         y_pulse1 = wheel_y1.getPulses();
01225         y_pulse2 = wheel_y2.getPulses();
01226         sum_pulse = (abs(x_pulse1) + abs(x_pulse2) + abs(y_pulse1) + abs(y_pulse2)) / 4;
01227         arm_pulse = arm_enc.getPulses();
01228 }
01229 
01230 void print_pulses(void) {
01231         //pc.printf("p: %d, k_p: %d, pulse: %d\n\r", phase, kaisyu_phase, arm_pulse);
01232         //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);
01233         //pc.printf("f: %d, b: %d, r: %d, phase: %d\n\r", front_limit, back_limit, right_limit, phase);
01234         //pc.printf("%r: %x, l: %x\n\r", right_arm_data[0], left_arm_data[0]);
01235         //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
01236         //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
01237         //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);
01238 }
01239 
01240 void get_emergency(void) {
01241 
01242     if(RDATA == '1') {
01243         myled = 1;
01244         emergency = 1;
01245     }
01246     else if(RDATA == '9'){
01247         myled = 0.2;
01248         emergency = 0;
01249         
01250         //終了phaseで駆動系統OFF
01251         if(phase >= 39) {
01252             emergency = 1;
01253         }
01254         else if(phase < 39) {
01255             emergency = 0;
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                 phase = next_phase;
01588                 kaisyu_phase = 2;
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] = 0x40;
01675                 drop_motor[0] = 0x00;
01676                 tyokudo_phase = 1;
01677             }
01678             break;
01679 
01680         case 1:
01681             //後進->減速
01682             //リミットが押されたら強制停止
01683             if(tyokudo_usiro_limit == 1) {
01684                 arm_motor[0] = 0x80;
01685                 drop_motor[0] = 0x80;
01686                 tyokudo_phase = 2;
01687                 phase = next_phase;
01688             }
01689             break;
01690             
01691         default:
01692             arm_motor[0] = 0x80;
01693             drop_motor[0] = 0x80;
01694             break;
01695     }
01696     
01697     i2c.write(0x18, arm_motor,  1);
01698     i2c.write(0x20, drop_motor, 1);
01699 }
01700 
01701 void arm_up(int next_phase) {
01702     
01703     //両腕、上限リミットが押されてなかったら上昇
01704     if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
01705         right_arm_data[0] = 0xFF;   left_arm_data[0] = 0xFF;
01706     }
01707     //右腕のみリミットが押されたら左腕のみ上昇
01708     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
01709         right_arm_data[0] = 0x80;   left_arm_data[0] = 0xFF;
01710     }
01711     //左腕のみリミットが押されたら右腕のみ上昇
01712     else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
01713         right_arm_data[0] = 0xFF;   left_arm_data[0] = 0x80;
01714     }
01715     //両腕、上限リミットが押されたら停止
01716     else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
01717         right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
01718         phase = next_phase;
01719     }
01720     
01721     i2c.write(0x22, right_arm_data, 1);
01722     i2c.write(0x24, left_arm_data, 1);
01723     wait_us(20);
01724 }
01725 
01726 void front(int target) {
01727 
01728         front_PID(target);
01729         i2c.write(0x10, true_migimae_data,     1, false);
01730         i2c.write(0x12, true_migiusiro_data,   1, false);
01731         i2c.write(0x14, true_hidarimae_data,   1, false);
01732         i2c.write(0x16, true_hidariusiro_data, 1, false);
01733         wait_us(20);
01734 }
01735 
01736 void back(int target) {
01737 
01738         back_PID(target);
01739         i2c.write(0x10, true_migimae_data,     1, false);
01740         i2c.write(0x12, true_migiusiro_data,   1, false);
01741         i2c.write(0x14, true_hidarimae_data,   1, false);
01742         i2c.write(0x16, true_hidariusiro_data, 1, false);
01743         wait_us(20);
01744 }
01745 
01746 void right(int target) {
01747 
01748         right_PID(target);
01749         i2c.write(0x10, true_migimae_data,     1, false);
01750         i2c.write(0x12, true_migiusiro_data,   1, false);
01751         i2c.write(0x14, true_hidarimae_data,   1, false);
01752         i2c.write(0x16, true_hidariusiro_data, 1, false);
01753         wait_us(20);
01754 }
01755 
01756 void left(int target) {
01757 
01758         left_PID(target);
01759         i2c.write(0x10, true_migimae_data,     1, false);
01760         i2c.write(0x12, true_migiusiro_data,   1, false);
01761         i2c.write(0x14, true_hidarimae_data,   1, false);
01762         i2c.write(0x16, true_hidariusiro_data, 1, false);
01763         wait_us(20);
01764 }
01765 
01766 void turn_right(int target) {
01767 
01768         turn_right_PID(target);
01769         i2c.write(0x10, true_migimae_data,     1, false);
01770         i2c.write(0x12, true_migiusiro_data,   1, false);
01771         i2c.write(0x14, true_hidarimae_data,   1, false);
01772         i2c.write(0x16, true_hidariusiro_data, 1, false);
01773         wait_us(20);
01774 }
01775 
01776 void turn_left(int target) {
01777 
01778         turn_left_PID(target);
01779         i2c.write(0x10, true_migimae_data,     1, false);
01780         i2c.write(0x12, true_migiusiro_data,   1, false);
01781         i2c.write(0x14, true_hidarimae_data,   1, false);
01782         i2c.write(0x16, true_hidariusiro_data, 1, false);
01783         wait_us(20);
01784 }
01785 
01786 void stop(void) {
01787 
01788         true_migimae_data[0]     = 0x80;
01789         true_migiusiro_data[0]   = 0x80;
01790         true_hidarimae_data[0]   = 0x80;
01791         true_hidariusiro_data[0] = 0x80;
01792 
01793         i2c.write(0x10, true_migimae_data,     1, false);
01794         i2c.write(0x12, true_migiusiro_data,   1, false);
01795         i2c.write(0x14, true_hidarimae_data,   1, false);
01796         i2c.write(0x16, true_hidariusiro_data, 1, false);
01797         wait_us(20);
01798 }
01799 
01800 void all_stop(void) {
01801     
01802         true_migimae_data[0]     = 0x80;
01803         true_migiusiro_data[0]   = 0x80;
01804         true_hidarimae_data[0]   = 0x80;
01805         true_hidariusiro_data[0] = 0x80;
01806         arm_motor[0]  = 0x80;
01807         drop_motor[0] = 0x80;
01808         right_arm_data[0] = 0x80;
01809         left_arm_data[0]  = 0x80;
01810         fan_data[0]   = 0x80;
01811         servo_data[0] = 0x04;
01812                         
01813         i2c.write(0x10, true_migimae_data,     1, false);
01814         i2c.write(0x12, true_migiusiro_data,   1, false);
01815         i2c.write(0x14, true_hidarimae_data,   1, false);
01816         i2c.write(0x16, true_hidariusiro_data, 1, false);
01817         i2c.write(0x18, arm_motor,  1);
01818         i2c.write(0x20, drop_motor, 1);
01819         i2c.write(0x22, right_arm_data, 1);
01820         i2c.write(0x24, left_arm_data, 1);
01821         i2c.write(0x26, fan_data, 1);
01822         i2c.write(0x28, fan_data, 1);
01823         i2c.write(0x30, servo_data, 1);
01824         wait_us(20);
01825 }
01826 
01827 void front_PID(int target) {
01828 
01829         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01830         front_migimae.setInputLimits(-2147483648,     2147483647);
01831         front_migiusiro.setInputLimits(-2147483648,   2147483647);
01832         front_hidarimae.setInputLimits(-2147483648,   2147483647);
01833         front_hidariusiro.setInputLimits(-2147483648, 2147483647);
01834 
01835         //制御量の最小、最大
01836         //正転(目標に達してない)
01837         if((y_pulse1 < target) && (y_pulse2 < target)) {
01838             front_migimae.setOutputLimits(0x84,     0xF5);
01839             front_migiusiro.setOutputLimits(0x84,   0xF5);
01840             front_hidarimae.setOutputLimits(0x84,   0xFF);
01841             front_hidariusiro.setOutputLimits(0x84, 0xFF);
01842         }
01843         //停止(目標より行き過ぎ)
01844         else if((y_pulse1 > target) && (y_pulse2 > target)) {
01845             front_migimae.setOutputLimits(0x7C,     0x83);
01846             front_migiusiro.setOutputLimits(0x7C,   0x83);
01847             front_hidarimae.setOutputLimits(0x7C,   0x83);
01848             front_hidariusiro.setOutputLimits(0x7C, 0x83);
01849         }
01850 
01851         //よくわからんやつ
01852         front_migimae.setMode(AUTO_MODE);
01853         front_migiusiro.setMode(AUTO_MODE);
01854         front_hidarimae.setMode(AUTO_MODE);
01855         front_hidariusiro.setMode(AUTO_MODE);
01856 
01857         //目標値
01858         front_migimae.setSetPoint(target);
01859         front_migiusiro.setSetPoint(target);
01860         front_hidarimae.setSetPoint(target);
01861         front_hidariusiro.setSetPoint(target);
01862 
01863         //センサ出力
01864         front_migimae.setProcessValue(y_pulse1);
01865         front_migiusiro.setProcessValue(y_pulse1);
01866         front_hidarimae.setProcessValue(y_pulse2);
01867         front_hidariusiro.setProcessValue(y_pulse2);
01868 
01869         //制御量(計算結果)
01870         migimae_data[0]      = front_migimae.compute();
01871         migiusiro_data[0]    = front_migiusiro.compute();
01872         hidarimae_data[0]    = front_hidarimae.compute();
01873         hidariusiro_data[0]  = front_hidariusiro.compute();
01874 
01875         //制御量をPWM値に変換
01876         //正転(目標に達してない)
01877         if((y_pulse1 < target) && (y_pulse2 < target)) {
01878             true_migimae_data[0]     = migimae_data[0];
01879             true_migiusiro_data[0]   = migiusiro_data[0];
01880             true_hidarimae_data[0]   = hidarimae_data[0];
01881             true_hidariusiro_data[0] = hidariusiro_data[0];
01882         }
01883         //停止(目標より行き過ぎ)
01884         else if((y_pulse1 > target) && (y_pulse2 > target)) {
01885             true_migimae_data[0]     = 0x80;
01886             true_migiusiro_data[0]   = 0x80;
01887             true_hidarimae_data[0]   = 0x80;
01888             true_hidariusiro_data[0] = 0x80;
01889         }
01890 }
01891 
01892 void back_PID(int target) {
01893 
01894         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
01895         back_migimae.setInputLimits(-2147483648,     2147483647);
01896         back_migiusiro.setInputLimits(-2147483648,   2147483647);
01897         back_hidarimae.setInputLimits(-2147483648,   2147483647);
01898         back_hidariusiro.setInputLimits(-2147483648, 2147483647);
01899 
01900         //制御量の最小、最大
01901         //逆転(目標に達してない)
01902         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
01903             back_migimae.setOutputLimits(0x00,     0x7B);
01904             back_migiusiro.setOutputLimits(0x00,   0x7B);
01905             back_hidarimae.setOutputLimits(0x00,   0x70);
01906             back_hidariusiro.setOutputLimits(0x00, 0x70);
01907             //back_hidarimae.setOutputLimits(0x00,   0x7B);
01908             //back_hidariusiro.setOutputLimits(0x00, 0x7B);
01909         }
01910         //停止(目標より行き過ぎ)
01911         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
01912             back_migimae.setOutputLimits(0x7C,     0x83);
01913             back_migiusiro.setOutputLimits(0x7C,   0x83);
01914             back_hidarimae.setOutputLimits(0x7C,   0x83);
01915             back_hidariusiro.setOutputLimits(0x7C, 0x83);
01916         }
01917 
01918         //よくわからんやつ
01919         back_migimae.setMode(AUTO_MODE);
01920         back_migiusiro.setMode(AUTO_MODE);
01921         back_hidarimae.setMode(AUTO_MODE);
01922         back_hidariusiro.setMode(AUTO_MODE);
01923 
01924         //目標値
01925         back_migimae.setSetPoint(target*-1);
01926         back_migiusiro.setSetPoint(target*-1);
01927         back_hidarimae.setSetPoint(target*-1);
01928         back_hidariusiro.setSetPoint(target*-1);
01929 
01930         //センサ出力
01931         back_migimae.setProcessValue(y_pulse1*-1);
01932         back_migiusiro.setProcessValue(y_pulse1*-1);
01933         back_hidarimae.setProcessValue(y_pulse2*-1);
01934         back_hidariusiro.setProcessValue(y_pulse2*-1);
01935 
01936         //制御量(計算結果)
01937         migimae_data[0]      = back_migimae.compute();
01938         migiusiro_data[0]    = back_migiusiro.compute();
01939         hidarimae_data[0]    = back_hidarimae.compute();
01940         hidariusiro_data[0]  = back_hidariusiro.compute();
01941 
01942         //制御量をPWM値に変換
01943         //逆転(目標に達してない)
01944         if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
01945             true_migimae_data[0]     = 0x7B - migimae_data[0];
01946             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
01947             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
01948             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
01949         }
01950         //停止(目標より行き過ぎ)
01951         else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
01952             true_migimae_data[0]     = 0x80;
01953             true_migiusiro_data[0]   = 0x80;
01954             true_hidarimae_data[0]   = 0x80;
01955             true_hidariusiro_data[0] = 0x80;
01956         }
01957 }
01958 
01959 void right_PID(int target) {
01960 
01961         //センサ出力値の最小、最大
01962         right_migimae.setInputLimits(-2147483648,     2147483647);
01963         right_migiusiro.setInputLimits(-2147483648,   2147483647);
01964         right_hidarimae.setInputLimits(-2147483648,   2147483647);
01965         right_hidariusiro.setInputLimits(-2147483648, 2147483647);
01966 
01967         //制御量の最小、最大
01968         //右進(目標まで達していない)
01969         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
01970             right_migimae.setOutputLimits(0x6A,     0x6C);
01971             //right_migimae.setOutputLimits(0x7A,     0x7B);
01972             right_migiusiro.setOutputLimits(0xFE,   0xFF);
01973             right_hidarimae.setOutputLimits(0xEF,   0xF0);
01974             //right_hidarimae.setOutputLimits(0xFE,   0xFF);
01975             right_hidariusiro.setOutputLimits(0x7A, 0x7B);
01976 
01977         }
01978         //停止(目標より行き過ぎ)
01979         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
01980             right_migimae.setOutputLimits(0x7C,     0x83);
01981             right_migiusiro.setOutputLimits(0x7C,   0x83);
01982             right_hidarimae.setOutputLimits(0x7C,   0x83);
01983             right_hidariusiro.setOutputLimits(0x7C, 0x83);
01984         }
01985 
01986         //よくわからんやつ
01987         right_migimae.setMode(AUTO_MODE);
01988         right_migiusiro.setMode(AUTO_MODE);
01989         right_hidarimae.setMode(AUTO_MODE);
01990         right_hidariusiro.setMode(AUTO_MODE);
01991 
01992         //目標値
01993         right_migimae.setSetPoint(target*-1);
01994         right_migiusiro.setSetPoint(target*-1);
01995         right_hidarimae.setSetPoint(target*-1);
01996         right_hidariusiro.setSetPoint(target*-1);
01997 
01998         //センサ出力
01999         right_migimae.setProcessValue(x_pulse1*-1);
02000         right_migiusiro.setProcessValue(x_pulse2*-1);
02001         right_hidarimae.setProcessValue(x_pulse1*-1);
02002         right_hidariusiro.setProcessValue(x_pulse2*-1);
02003 
02004         //制御量(計算結果)
02005         migimae_data[0]      = right_migimae.compute();
02006         migiusiro_data[0]    = right_migiusiro.compute();
02007         hidarimae_data[0]    = right_hidarimae.compute();
02008         hidariusiro_data[0]  = right_hidariusiro.compute();
02009 
02010         //制御量をPWM値に変換
02011         //右進(目標まで達していない)
02012         if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
02013             true_migimae_data[0]     = 0x7B - migimae_data[0];
02014             true_migiusiro_data[0]   = migiusiro_data[0];
02015             true_hidarimae_data[0]   = hidarimae_data[0];
02016             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
02017         }
02018         //左進(目標より行き過ぎ)
02019         else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
02020             true_migimae_data[0]     = 0x80;
02021             true_migiusiro_data[0]   = 0x80;
02022             true_hidarimae_data[0]   = 0x80;
02023             true_hidariusiro_data[0] = 0x80;
02024         }
02025 }
02026 
02027 void left_PID(int target) {
02028 
02029         //センサ出力値の最小、最大
02030         left_migimae.setInputLimits(-2147483648,     2147483647);
02031         left_migiusiro.setInputLimits(-2147483648,   2147483647);
02032         left_hidarimae.setInputLimits(-2147483648,   2147483647);
02033         left_hidariusiro.setInputLimits(-2147483648, 2147483647);
02034 
02035         //制御量の最小、最大
02036         //左進(目標まで達していない)
02037         if((x_pulse1 < target) && (x_pulse2 < target)) {
02038             left_migimae.setOutputLimits(0xEC,     0xED);   //0xFFに近いほうが速い
02039             left_migiusiro.setOutputLimits(0x7A,   0x7B);   //0x7Bに近いほうが速い
02040             //left_hidarimae.setOutputLimits(0x6B, 0x6C);     //0x7Bに近いほうが速い
02041             left_hidarimae.setOutputLimits(0x6E, 0x6F);
02042             left_hidariusiro.setOutputLimits(0xFE, 0xFF);   //0xFFに近いほうが速い
02043         }
02044         //停止(目標より行き過ぎ)
02045         else if((x_pulse1 > target) && (x_pulse2 > target)) {
02046             left_migimae.setOutputLimits(0x7C,     0x83);
02047             left_migiusiro.setOutputLimits(0x7C,   0x83);
02048             left_hidarimae.setOutputLimits(0x7C,   0x83);
02049             left_hidariusiro.setOutputLimits(0x7C, 0x83);
02050         }
02051 
02052         //よくわからんやつ
02053         left_migimae.setMode(AUTO_MODE);
02054         left_migiusiro.setMode(AUTO_MODE);
02055         left_hidarimae.setMode(AUTO_MODE);
02056         left_hidariusiro.setMode(AUTO_MODE);
02057 
02058         //目標値
02059         left_migimae.setSetPoint(target);
02060         left_migiusiro.setSetPoint(target);
02061         left_hidarimae.setSetPoint(target);
02062         left_hidariusiro.setSetPoint(target);
02063 
02064         //センサ出力
02065         left_migimae.setProcessValue(x_pulse1);
02066         left_migiusiro.setProcessValue(x_pulse2);
02067         left_hidarimae.setProcessValue(x_pulse1);
02068         left_hidariusiro.setProcessValue(x_pulse2);
02069 
02070         //制御量(計算結果)
02071         migimae_data[0]      = left_migimae.compute();
02072         migiusiro_data[0]    = left_migiusiro.compute();
02073         hidarimae_data[0]    = left_hidarimae.compute();
02074         hidariusiro_data[0]  = left_hidariusiro.compute();
02075 
02076         //制御量をPWM値に変換
02077         //左進(目標まで達していない)
02078         if((x_pulse1 < target) && (x_pulse2 < target)) {
02079             true_migimae_data[0]     = migimae_data[0];
02080             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
02081             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
02082             true_hidariusiro_data[0] = hidariusiro_data[0];
02083         }
02084         //停止(目標より行き過ぎ)
02085         else if((x_pulse1 > target) && (x_pulse2 > target)) {
02086             true_migimae_data[0]     = 0x80;
02087             true_migiusiro_data[0]   = 0x80;
02088             true_hidarimae_data[0]   = 0x80;
02089             true_hidariusiro_data[0] = 0x80;
02090         }
02091 }
02092 
02093 void turn_right_PID(int target) {
02094 
02095         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02096         turn_right_migimae.setInputLimits(-2147483648,     2147483647);
02097         turn_right_migiusiro.setInputLimits(-2147483648,   2147483647);
02098         turn_right_hidarimae.setInputLimits(-2147483648,   2147483647);
02099         turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);
02100 
02101         //制御量の最小、最大
02102         //右旋回(目標に達してない)
02103         if(sum_pulse < target) {
02104             turn_right_migimae.setOutputLimits(0x10,   0x7B);
02105             turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
02106             turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
02107             turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
02108         }
02109         //停止(目標より行き過ぎ)
02110         else if(sum_pulse > target) {
02111             turn_right_migimae.setOutputLimits(0x7C,     0x83);
02112             turn_right_migiusiro.setOutputLimits(0x7C,   0x83);
02113             turn_right_hidarimae.setOutputLimits(0x7C,   0x83);
02114             turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
02115         }
02116 
02117         //よくわからんやつ
02118         turn_right_migimae.setMode(AUTO_MODE);
02119         turn_right_migiusiro.setMode(AUTO_MODE);
02120         turn_right_hidarimae.setMode(AUTO_MODE);
02121         turn_right_hidariusiro.setMode(AUTO_MODE);
02122 
02123         //目標値
02124         turn_right_migimae.setSetPoint(target);
02125         turn_right_migiusiro.setSetPoint(target);
02126         turn_right_hidarimae.setSetPoint(target);
02127         turn_right_hidariusiro.setSetPoint(target);
02128 
02129         //センサ出力
02130         turn_right_migimae.setProcessValue(sum_pulse);
02131         turn_right_migiusiro.setProcessValue(sum_pulse);
02132         turn_right_hidarimae.setProcessValue(sum_pulse);
02133         turn_right_hidariusiro.setProcessValue(sum_pulse);
02134 
02135         //制御量(計算結果)
02136         migimae_data[0]      = turn_right_migimae.compute();
02137         migiusiro_data[0]    = turn_right_migiusiro.compute();
02138         hidarimae_data[0]    = turn_right_hidarimae.compute();
02139         hidariusiro_data[0]  = turn_right_hidariusiro.compute();
02140 
02141         //制御量をPWM値に変換
02142         //右旋回(目標に達してない)
02143         if(sum_pulse < target) {
02144             true_migimae_data[0]     = 0x7B - migimae_data[0];
02145             true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
02146             true_hidarimae_data[0]   = hidarimae_data[0];
02147             true_hidariusiro_data[0] = hidariusiro_data[0];
02148         }
02149         //停止(目標より行き過ぎ)
02150         else if(sum_pulse > target) {
02151             true_migimae_data[0]     = 0x80;
02152             true_migiusiro_data[0]   = 0x80;
02153             true_hidarimae_data[0]   = 0x80;
02154             true_hidariusiro_data[0] = 0x80;
02155         }
02156 }
02157 
02158 void turn_left_PID(int target) {
02159 
02160         //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
02161         turn_left_migimae.setInputLimits(-2147483648,     2147483647);
02162         turn_left_migiusiro.setInputLimits(-2147483648,   2147483647);
02163         turn_left_hidarimae.setInputLimits(-2147483648,   2147483647);
02164         turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);
02165 
02166         //制御量の最小、最大
02167         //左旋回(目標に達してない)
02168         if(sum_pulse < target) {
02169             turn_left_migimae.setOutputLimits(0x94,   0xFF);
02170             turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
02171             turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
02172             turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
02173         }
02174         //停止(目標より行き過ぎ)
02175         else if(sum_pulse > target) {
02176             turn_left_migimae.setOutputLimits(0x7C,     0x83);
02177             turn_left_migiusiro.setOutputLimits(0x7C,   0x83);
02178             turn_left_hidarimae.setOutputLimits(0x7C,   0x83);
02179             turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
02180         }
02181 
02182         //よくわからんやつ
02183         turn_left_migimae.setMode(AUTO_MODE);
02184         turn_left_migiusiro.setMode(AUTO_MODE);
02185         turn_left_hidarimae.setMode(AUTO_MODE);
02186         turn_left_hidariusiro.setMode(AUTO_MODE);
02187 
02188         //目標値
02189         turn_left_migimae.setSetPoint(target);
02190         turn_left_migiusiro.setSetPoint(target);
02191         turn_left_hidarimae.setSetPoint(target);
02192         turn_left_hidariusiro.setSetPoint(target);
02193 
02194         //センサ出力
02195         turn_left_migimae.setProcessValue(sum_pulse);
02196         turn_left_migiusiro.setProcessValue(sum_pulse);
02197         turn_left_hidarimae.setProcessValue(sum_pulse);
02198         turn_left_hidariusiro.setProcessValue(sum_pulse);
02199 
02200         //制御量(計算結果)
02201         migimae_data[0]      = turn_left_migimae.compute();
02202         migiusiro_data[0]    = turn_left_migiusiro.compute();
02203         hidarimae_data[0]    = turn_left_hidarimae.compute();
02204         hidariusiro_data[0]  = turn_left_hidariusiro.compute();
02205 
02206         //制御量をPWM値に変換
02207         //左旋回(目標に達してない)
02208         if(sum_pulse < target) {
02209             true_migimae_data[0]     = migimae_data[0];
02210             true_migiusiro_data[0]   = migiusiro_data[0];
02211             true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
02212             true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
02213         }
02214         //左旋回(目標より行き過ぎ)
02215         else if(sum_pulse > target) {
02216             true_migimae_data[0]     = 0x80;
02217             true_migiusiro_data[0]   = 0x80;
02218             true_hidarimae_data[0]   = 0x80;
02219             true_hidariusiro_data[0] = 0x80;
02220         }
02221 }