関東甲信越地区大会でのプログラム 青ゾーンで回収時にアームを引っ込めないバグがまだあるので注意

Dependencies:   mbed QEI PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

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