青ゾーン&赤ゾーンで掛ける位置まで移動できたやつ

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