File content as of revision 20:ac4954be1fe0:

/* -------------------------------------------------------------------  */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic                      */
/* Nucleo Type: F446RE                                                  */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com            */
/* Sensor: encorder*4                                                   */
/* -------------------------------------------------------------------  */
/* ファンとサーボの動作を追加した                                            */
/* -------------------------------------------------------------------  */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"

//直進補正の為の前後・左右の回転差の許容値
#define wheel_difference    100

#define RED     0
#define BLUE    1

//PID Gain of wheels(Kp, Ti, Td, control cycle)
//前進
PID front_migimae(4500000.0, 0.0, 0.0, 0.001);
PID front_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID front_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID front_hidariusiro(4500000.0, 0.0, 0.0, 0.001);

//後進
PID back_migimae(4500000.0, 0.0, 0.0, 0.001);
PID back_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID back_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID back_hidariusiro(4500000.0, 0.0, 0.0, 0.001);

//右進
PID right_migimae(6000000.0, 0.0, 0.0, 0.001);
PID right_migiusiro(6000000.0, 0.0, 0.0, 0.001);
PID right_hidarimae(6000000.0, 0.0, 0.0, 0.001);
PID right_hidariusiro(6000000.0, 0.0, 0.0, 0.001);

//左進
PID left_migimae(6000000.0, 0.0, 0.0, 0.001);
PID left_migiusiro(6000000.0, 0.0, 0.0, 0.001);
PID left_hidarimae(6000000.0, 0.0, 0.0, 0.001);
PID left_hidariusiro(6000000.0, 0.0, 0.0, 0.001);

//右旋回
PID turn_right_migimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_migiusiro(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidarimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_right_hidariusiro(30000000.0, 0.0, 0.0, 0.001);

//左旋回
PID turn_left_migimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_migiusiro(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidarimae(30000000.0, 0.0, 0.0, 0.001);
PID turn_left_hidariusiro(30000000.0, 0.0, 0.0, 0.001);

//MDとの通信ポート
I2C i2c(PB_9, PB_8);  //SDA, SCL

//PCとの通信ポート
Serial pc(USBTX, USBRX);    //TX, RX

//特小モジュールとの通信ポート
Serial pic(A0, A1);

//リミットスイッチ基板との通信ポート
Serial limit_serial(PC_12, PD_2);

//12V停止信号ピン
DigitalOut emergency(D11);

DigitalOut USR_LED1(PB_7);
DigitalOut USR_LED2(PC_13);
DigitalOut USR_LED3(PC_2);
DigitalOut USR_LED4(PC_3);

//遠隔非常停止ユニットLED
AnalogOut myled(A2);

DigitalIn start_switch(PB_12);

QEI wheel_x1(PA_8 , PA_6 , NC, 624);
QEI wheel_x2(PB_14, PB_13, NC, 624);
QEI wheel_y1(PB_1 , PB_15, NC, 624);
QEI wheel_y2(PA_12, PA_11, NC, 624);
QEI arm_enc(PB_5,   PB_4 , NC, 624);

//移動後n秒停止タイマー
Timer counter;

//エンコーダ値格納変数
int x_pulse1, x_pulse2, y_pulse1, y_pulse2;

//操作の段階変数
unsigned int phase = 0;
int kaisyu_phase = 0;
int tyokudo_phase = 0;
unsigned int start_zone = 1;
bool zone = RED;

//i2c送信データ変数
char init_send_data[1];
char migimae_data[1], migiusiro_data[1], hidarimae_data[1], hidariusiro_data[1];
char true_migimae_data[1], true_migiusiro_data[1], true_hidarimae_data[1], true_hidariusiro_data[1];
char arm_moter[1], drop_moter[1];
char fan_data[1];
char servo_data[1];
char right_arm_data[1], left_arm_data[1];

//非常停止関連変数
char RDATA;
char baff;
int flug = 0;

//リミット基板からの受信データ
int limit_data = 0;
int upper_limit_data = 0;
int lower_limit_data = 0;

//各辺のスイッチが押されたかのフラグ
//前部が壁に当たっているか
int front_limit = 0;
//右部が壁にあたあっているか
int right_limit = 0;
//後部が壁に当たっているか
int back_limit = 0;
//回収機構の下限(引っ込めてるほう)
bool kaisyu_limit = 0;
//右腕の下限
bool right_arm_lower_limit = 0;
//右腕の上限
bool right_arm_upper_limit = 0;
//左腕の下限
bool left_arm_lower_limit = 0;
//左腕の上限
bool left_arm_upper_limit = 0;
//吐き出し機構の上限
bool tyokudo_mae_limit = 0;
//吐き出し機構の下限
bool tyokudo_usiro_limit = 0;


int masked_lower_front_limit_data = 0xFF;
int masked_lower_back_limit_data = 0xFF;
int masked_lower_right_limit_data = 0xFF;
int masked_kaisyu_limit_data = 0xFF;
int masked_right_arm_lower_limit_data = 0xFF;
int masked_right_arm_upper_limit_data = 0xFF;
int masked_left_arm_lower_limit_data = 0xFF;
int masked_left_arm_upper_limit_data = 0xFF;
int masked_tyokudo_mae_limit_data = 0xFF;
int masked_tyokudo_usiro_limit_data = 0xFF;

//関数のプロトタイプ宣言
void init(void);
void init_send(void);
void get(void);
void get_pulses(void);
void print_pulses(void);
void get_emergency(void);
void read_limit(void);
void wheel_reset(void);
void kaisyu(int pulse);
void tyokudo(int pulse);
void arm_up(void);
void front(int target);
void back(int target);
void right(int target);
void left(int target);
void turn_right(int target);
void turn_left(int target);
void stop(void);
void front_PID(int target);
void back_PID(int target);
void right_PID(int target);
void left_PID(int target);
void turn_right_PID(int target);
void turn_left_PID(int target);
void dondonkasoku(void);

int main(void) {

    init();
    init_send();

    //とりあえず(後で消してね)
    zone = BLUE;

    while(1) {

        get_pulses();
        print_pulses();
        get_emergency();
        read_limit();

        //if(start_switch == 1) {
            //tyokudo(arm_enc.getPulses());
        //}
        
        //青ゾーン
        if(zone == BLUE) {

            switch(phase) {

                //スタート位置へセット
                case 0:
                    //リミットが洗濯物台に触れているか
                    if(right_limit == 3) {
                        USR_LED1 = 1;
                        //スタートスイッチが押されたか
                        if(start_switch == 1) {
                            wheel_reset();
                            phase = 1;
                        }
                    } else {
                        USR_LED1 = 0;
                    }
                    break;

                //回収
                case 1:
                    //回収アームのリミットが押されているか
                    if(kaisyu_limit == 1) {
                        kaisyu(arm_enc.getPulses());
                        USR_LED2 = 1;
                    } else {
                        USR_LED2 = 0;
                    }
                    servo_data[0] = 0x03;
                    i2c.write(0x30, servo_data, 1);
                    break;

                //1秒停止
                case 2:
                    stop();
                    servo_data[0] = 0x04;
                    i2c.write(0x30, servo_data, 1);
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 3;
                        wheel_reset();
                    }
                    break;

                //左移動
                case 3:
                    counter.reset();
                    left(12000);
                    if((x_pulse1 > 12000) && (x_pulse2 > 12000)) {
                        phase = 4;
                    }
                    break;

                //1秒停止
                case 4:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 5;
                        wheel_reset();
                    }
                    break;

                //右旋回(180°)
                case 5:
                    counter.reset();
                    turn_right(518);
                    if(x_pulse2 > 518) {
                        phase = 6;
                    }
                    break;

                //1秒停止
                case 6:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 7;
                        wheel_reset();
                    }
                    break;

                //ちょっくら右移動
                case 7:
                    counter.reset();
                    right(-1000);

                    if((x_pulse1*-1 > 500) && (x_pulse2*-1 > 500)) {
                        true_migimae_data[0] = 0x94;
                        true_migiusiro_data[0] = 0x10;
                        true_hidarimae_data[0] = 0x10;
                        true_hidariusiro_data[0] = 0x94;
                    }
                    if(right_limit == 3) {
                        phase = 8;
                    }
                    break;

                //1秒停止
                case 8:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 9;
                        wheel_reset();
                    }
                    break;

                //排出
                case 9:
                    counter.reset();
                    tyokudo(arm_enc.getPulses());
                    //ここに排出動作が来るが今回は飛ばす
                    //phase = 10;
                    break;

                //排出機構引っ込める
                case 10:
                    //ここに排出機構引っ込める動作が来るが今回は飛ばす
                    phase = 11;
                    break;

                //1秒停止
                case 11:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 12;
                        wheel_reset();
                    }
                    break;

                //前進
                case 12:
                    counter.reset();
                    front(3000);
                    if((y_pulse1 > 3000) && (y_pulse2 > 3000)) {
                        phase = 13;
                    }
                    break;

                //1秒停止
                case 13:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 14;
                        wheel_reset();
                    }
                    break;

                //右移動
                case 14:
                    counter.reset();
                    right(-4000);
                    if((x_pulse1*-1 > 3000) && (x_pulse2*-1 > 3000)) {
                        true_migimae_data[0] = 0x94;
                        true_migiusiro_data[0] = 0x10;
                        true_hidarimae_data[0] = 0x10;
                        true_hidariusiro_data[0] = 0x94;
                    }
                    if(right_limit == 3) {
                        phase = 15;
                    }
                    break;

                //シーツ装填
                case 15:
                    if(start_switch == 1) {
                        phase = 16;
                    } else {
                        stop();
                    }
                    break;

                //竿のラインまで前進
                case 16:
                    counter.reset();
                    front(21500);
                    if((y_pulse1 > 21500) && (y_pulse2 > 21500)) {
                        phase = 17;
                    }
                    break;

                //1秒停止
                case 17:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 18;
                        wheel_reset();
                    }
                    break;

                //掛けるところまで左移動
                case 18:
                    counter.reset();
                    left(10000);
                    if((x_pulse1 > 10000) && (x_pulse2 > 10000)) {
                        phase = 19;
                    }
                    break;

                //1秒停止
                case 19:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 20;
                        wheel_reset();
                    }
                    break;

                //妨害防止の右旋回
                case 20:
                    counter.reset();
                    turn_right(300);
                    if(x_pulse2 > 300) {
                        phase = 21;
                    }
                    break;

                //1秒停止
                case 21:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 22;
                        wheel_reset();
                    }
                    break;

                //アームアップ
                case 22:
                    stop();
                    counter.reset();
                    arm_up();
                    break;
                
                //シーツを掛ける
                case 23:
                    counter.start();

                    //5秒間ファン送風
                    if(counter.read() <= 4.0f) {
                        fan_data[0] = 0xFF;
                        i2c.write(0x26, fan_data, 1);
                        servo_data[0] = 0x04;
                        i2c.write(0x30, servo_data, 1);
                    }
                    //4~5秒の間でサーボを放す
                    else if((counter.read() > 4.0f) && (counter.read() <= 5.0f)) {
                        fan_data[0] = 0xFF;
                        i2c.write(0x26, fan_data, 1);
                        servo_data[0] = 0x03;
                        i2c.write(0x30, servo_data, 1);
                    }
                    //5秒過ぎたらファン停止
                    else if(counter.read() > 5.0f) {
                        fan_data[0] = 0x80;
                        i2c.write(0x26, fan_data, 1);
                        servo_data[0] = 0x04;
                        i2c.write(0x30, servo_data, 1);
                        phase = 24;
                    }
                    break;

                //終了っ！(守衛さん風)
                case 24:
                    //駆動系統OFF
                    emergency = 0;
                    break;

                default:
                    //駆動系統OFF
                    emergency = 0;
                    break;
            }
        }
    }
}

void init(void) {

    //通信ボーレートの設定
    pc.baud(460800);

    limit_serial.baud(115200);

    start_switch.mode(PullUp);

    //非常停止関連
    pic.baud(19200);
    pic.format(8, Serial::None, 1);
    pic.attach(get, Serial::RxIrq);

    x_pulse1 = 0;   x_pulse2 = 0;   y_pulse1 = 0;   y_pulse2 = 0;
    migimae_data[0] = 0x80; migiusiro_data[0] = 0x80;   hidarimae_data[0] = 0x80;   hidariusiro_data[0] = 0x80;
    true_migimae_data[0] = 0x80;    true_migiusiro_data[0] = 0x80;  true_hidarimae_data[0] = 0x80;  true_hidariusiro_data[0] = 0x80;
    fan_data[0] = 0x80;
    servo_data[0] = 0x80;
    right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
}

void init_send(void) {

    init_send_data[0] = 0x80;
    i2c.write(0x10, init_send_data, 1);
    i2c.write(0x12, init_send_data, 1);
    i2c.write(0x14, init_send_data, 1);
    i2c.write(0x16, init_send_data, 1);
    i2c.write(0x18, init_send_data, 1);
    i2c.write(0x20, init_send_data, 1);
    i2c.write(0x22, init_send_data, 1);
    i2c.write(0x24, init_send_data, 1);
    i2c.write(0x30, init_send_data, 1);
    wait(0.1);
}

void get(void) {

    baff = pic.getc();

    for(; flug; flug--)
        RDATA = baff;

    if(baff == ':')
        flug = 1;
}

void get_pulses(void) {

        x_pulse1 = wheel_x1.getPulses();
        x_pulse2 = wheel_x2.getPulses();
        y_pulse1 = wheel_y1.getPulses();
        y_pulse2 = wheel_y2.getPulses();
}

void print_pulses(void) {
        
        pc.printf("%d\n\r", tyokudo_phase);
        //pc.printf("%d\n\r", arm_enc.getPulses());
        //pc.printf("limit: 0x%x, upper: 0x%x, lower: 0x%x\n\r", limit_data, upper_limit_data, lower_limit_data);
        //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, phase: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2, phase);
        //pc.printf("RF: %x, RB: %x, LF: %x, LB: %x\n\r", true_migimae_data[0], true_migiusiro_data[0], true_hidarimae_data[0], true_hidariusiro_data[0]);
}

void get_emergency(void) {

    if(RDATA == '1') {
        myled = 1;
        emergency = 1;
    }
    else if(RDATA == '9'){
        myled = 0.2;
        emergency = 0;
    }
}

void read_limit(void) {

        limit_data = limit_serial.getc();

        //上位1bitが1ならば下のリミットのデータだと判断
        if((limit_data & 0b10000000) == 0b10000000) {
            lower_limit_data = limit_data;

        //上位1bitが0ならば上のリミットのデータだと判断
        } else {
            upper_limit_data = limit_data;
        }

        //下リミット基板からのデータのマスク処理
        masked_lower_front_limit_data = lower_limit_data & 0b00000011;
        masked_lower_back_limit_data  = lower_limit_data & 0b00001100;
        masked_lower_right_limit_data = lower_limit_data & 0b00110000;
        masked_kaisyu_limit_data      = lower_limit_data & 0b01000000;

        //上リミット基板からのデータのマスク処理
        masked_right_arm_lower_limit_data = upper_limit_data & 0b00000001;
        masked_right_arm_upper_limit_data = upper_limit_data & 0b00000010;
        masked_left_arm_lower_limit_data  = upper_limit_data & 0b00000100;
        masked_left_arm_upper_limit_data  = upper_limit_data & 0b00001000;
        masked_tyokudo_mae_limit_data     = upper_limit_data & 0b00010000;
        masked_tyokudo_usiro_limit_data   = upper_limit_data & 0b00100000;

        //前部リミット
        switch(masked_lower_front_limit_data) {
            //両方押された
            case 0x00:
                front_limit = 3;
                break;
            //右が押された
            case 0b00000010:
                front_limit = 1;
                break;
            //左が押された
            case 0b00000001:
                front_limit = 2;
                break;
            default:
                front_limit = 0;
                break;
        }

        //後部リミット
        switch(masked_lower_back_limit_data) {
            //両方押された
            case 0x00:
                back_limit = 3;
                break;
            //右が押された
            case 0b00001000:
                back_limit = 1;
                break;
            //左が押された
            case 0b00000100:
                back_limit = 2;
                break;
            default:
                back_limit = 0;
                break;
        }

        //右部リミット
        switch(masked_lower_right_limit_data) {
            //両方押された
            case 0x00:
                right_limit = 3;
                break;
            //右が押された
            case 0b00100000:
                right_limit = 1;
                break;
            //左が押された
            case 0b00010000:
                right_limit = 2;
                break;
            default:
                right_limit = 0;
                break;
        }

        //回収機構リミット
        switch(masked_kaisyu_limit_data) {
            //押された
            case 0b00000000:
                kaisyu_limit = 1;
                break;
            case 0b01000000:
                  kaisyu_limit = 0;
                break;
            default:
                kaisyu_limit = 0;
                break;
        }

        //右腕下部リミット
        switch(masked_right_arm_lower_limit_data) {
            //押された
            case 0b00000000:
                right_arm_lower_limit = 1;
                break;
            case 0b00000001:
                right_arm_lower_limit = 0;
                break;
            default:
                right_arm_lower_limit = 0;
                break;
        }

        //右腕上部リミット
        switch(masked_right_arm_upper_limit_data) {
            //押された
            case 0b00000000:
                right_arm_upper_limit = 1;
                break;
            case 0b00000010:
                right_arm_upper_limit = 0;
                break;
            default:
                right_arm_upper_limit = 0;
                break;
        }

        //左腕下部リミット
        switch(masked_left_arm_lower_limit_data) {
            //押された
            case 0b00000000:
                left_arm_lower_limit = 1;
                break;
            case 0b00000100:
                left_arm_lower_limit = 0;
                break;
            default:
                left_arm_lower_limit = 0;
                break;
        }

        //左腕上部リミット
        switch(masked_left_arm_upper_limit_data) {
            //押された
            case 0b00000000:
                left_arm_upper_limit = 1;
                break;
            case 0b00001000:
                left_arm_upper_limit = 0;
                break;
            default:
                left_arm_upper_limit = 0;
                break;
        }

        //直動の前
        switch(masked_tyokudo_mae_limit_data) {
            //押された
            case 0b00000000:
                tyokudo_mae_limit = 1;
                break;
            case 0b00010000:
                tyokudo_mae_limit = 0;
                break;
            default:
                tyokudo_mae_limit = 0;
                break;
        }

        //直動の後
        switch(masked_tyokudo_usiro_limit_data) {
            //押された
            case 0b00000000:
                tyokudo_usiro_limit = 1;
                break;
            case 0b00100000:
                tyokudo_usiro_limit = 0;
                break;
            default:
                tyokudo_usiro_limit = 0;
                break;
        }
}

void wheel_reset(void) {

    wheel_x1.reset();
    wheel_x2.reset();
    wheel_y1.reset();
    wheel_y2.reset();
}

void kaisyu(int pulse) {

    switch (kaisyu_phase) {
        
        case 0:
            //前進->減速
            //3000pulseまで高速前進
            if(pulse < 3000) {
                arm_moter[0] = 0xFF;
                //kaisyu_phase = 1;
            } 
            
            //3000pulse超えたら低速前進
            else if(pulse >= 3000) {
                arm_moter[0] = 0xB3;
                kaisyu_phase = 1;
            }
            break;
            
        case 1:
            //前進->停止->後進
            //3600pulseまで低速前進
            if(pulse < 3600) {
                arm_moter[0] = 0xB3;
                //kaisyu_phase = 2;
            } 
            
            //3600pulse超えたら停止
            else if(pulse >= 3600) {
                arm_moter[0] = 0x80;
                
                //1秒待ってから引っ込める
                counter.start();
                if(counter.read() > 1.0f) {
                    kaisyu_phase = 2;
                }
            }
                        
            break;
            
        case 2:
            //後進->減速
            //500pulseまで高速後進
            counter.reset();
            if(pulse > 500) {
                arm_moter[0] = 0x00;
                //kaisyu_phase = 3;
                
            } 
            //500pulse以下になったら低速後進
            else if(pulse <= 500) {
                arm_moter[0] = 0x4C;
                kaisyu_phase = 3;
            }
            break;
            
        case 3:
            //後進->停止
            //リミット押されるまで低速後進
            if(pulse <= 500) {
                arm_moter[0] = 0x4C;
                //kaisyu_phase = 4;
            }
            
            //リミット押されたら停止
            if(kaisyu_limit == 1) {
                arm_moter[0] = 0x80;
                kaisyu_phase = 4;
                phase = 2;   
            }
            break;
            
        default:
            arm_moter[0] = 0x80;
            break;
    }
    
    //回収MDへ書き込み
    i2c.write(0x18, arm_moter, 1);
}

void tyokudo(int pulse) {

    switch(tyokudo_phase) {
        
        case 0:
            //前進->減速
            
            //3600pulseより大きい&直堂前リミットが押されたら次のphaseへ移行
            if(tyokudo_mae_limit == 0) {
                //2000pulseまで高速前進
                if(pulse < 2000) {
                    arm_moter[0] = 0xCD;
                    drop_moter[0] = 0xE6;
                } 
                //2000pulse以上で低速前進
                else if(pulse >= 2000) {
                    arm_moter[0] = 0xC0;
                    drop_moter[0] = 0xCD;
                }
                //パルスが3600を終えたらアームのみ強制停止
                else if(pulse > 3600) {
                    arm_moter[0] = 0x80;
                    drop_moter[0] = 0xCD;
                }
            }
            
            //直動の前リミットが押されたら
            else if(tyokudo_mae_limit == 1) {
                //直動のみ強制停止
                drop_moter[0] = 0x80;
                
                //2000pulseまで高速前進
                if(pulse < 2000) {
                    arm_moter[0] = 0xCD;
                }
                //2000pulse以上で低速前進
                else if(pulse >= 2000) {
                    arm_moter[0] = 0xC0;
                }
                //パルスが3600を終えたらアームも強制停止
                else if(pulse > 3600) {
                    arm_moter[0] = 0x80;
                    tyokudo_phase = 1;
                }
            }
            break;
            
        case 1:
            //後進->減速
 
            //1600pulseより大きいとき高速後進
            if(pulse > 1600) {
                
                if(kaisyu_limit == 0) {
                    arm_moter[0] = 0x00;
                }
                //リミットが押されたら強制停止
                else if(kaisyu_limit == 1) {
                    arm_moter[0] = 0x80;
                }
                
                if(tyokudo_usiro_limit == 0) {
                    drop_moter[0] = 0x19;      
                }
                //リミットが押されたら強制停止
                else if(tyokudo_usiro_limit == 1) {
                    drop_moter[0] = 0x80;
                }
            }
            //500pulse以下でphase2へ移行
            else if(pulse <= 500) {
                tyokudo_phase = 2;
            }
            break;
            
        case 2:
            //後進->停止
            
            //直動後リミットが押されるまで後進
            if(tyokudo_usiro_limit == 0) {
                drop_moter[0] = 0x19;
            } 
            //直動後リミットが押されたら停止
            else if(tyokudo_usiro_limit == 1) {
                drop_moter[0] = 0x80;
            }
            
            //リミット押されるまで低速後進
            if(kaisyu_limit == 0) {
                arm_moter[0] = 0x4C;
            }
            else if(kaisyu_limit == 1) {
                arm_moter[1] = 0x80;
                tyokudo_phase = 3;
                phase = 10;
            }
            
            break;
            
        default:
            arm_moter[0] = 0x80;
            drop_moter[0] = 0x80;
            break;
    }
    
    i2c.write(0x18, arm_moter,  1);
    i2c.write(0x20, drop_moter, 1);
}

void arm_up(void) {
    
    //両腕、上限リミットが押されてなかったら上昇
    if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 0)) {
        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0xFF;
    }
    //右腕のみリミットが押されたら左腕のみ上昇
    else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 0)) {
        right_arm_data[0] = 0x80;   left_arm_data[0] = 0xFF;
    }
    //左腕のみリミットが押されたら右腕のみ上昇
    else if((right_arm_upper_limit) == 0 && (left_arm_upper_limit == 1)) {
        right_arm_data[0] = 0xFF;   left_arm_data[0] = 0x80;
    }
    //両腕、上限リミットが押されたら停止
    else if((right_arm_upper_limit) == 1 && (left_arm_upper_limit == 1)) {
        right_arm_data[0] = 0x80;   left_arm_data[0] = 0x80;
        phase = 23;
    }
    
    i2c.write(0x22, right_arm_data, 1);
    i2c.write(0x24, left_arm_data, 1);
}

void front(int target) {

        front_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void back(int target) {

        back_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void right(int target) {

        right_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void left(int target) {

        left_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void turn_right(int target) {

        turn_right_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void turn_left(int target) {

        turn_left_PID(target);
        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void stop(void) {

        true_migimae_data[0]     = 0x80;
        true_migiusiro_data[0]   = 0x80;
        true_hidarimae_data[0]   = 0x80;
        true_hidariusiro_data[0] = 0x80;

        i2c.write(0x10, true_migimae_data,     1, false);
        i2c.write(0x12, true_migiusiro_data,   1, false);
        i2c.write(0x14, true_hidarimae_data,   1, false);
        i2c.write(0x16, true_hidariusiro_data, 1, false);
        wait_us(20);
}

void front_PID(int target) {

        //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
        front_migimae.setInputLimits(-2147483648,     2147483647);
        front_migiusiro.setInputLimits(-2147483648,   2147483647);
        front_hidarimae.setInputLimits(-2147483648,   2147483647);
        front_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //正転(目標に達してない)
        if((y_pulse1 < target) && (y_pulse2 < target)) {
            front_migimae.setOutputLimits(0x84,     0xF7);
            front_migiusiro.setOutputLimits(0x84,   0xF7);
            //front_migimae.setOutputLimits(0x84,     0xFF);
            //front_migiusiro.setOutputLimits(0x84,   0xFF);
            front_hidarimae.setOutputLimits(0x84,   0xFF);
            front_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //左側が前に出ちゃった♥(右側だけ回して左側は停止)
        else if((y_pulse1 + wheel_difference) < y_pulse2) {
            front_migimae.setOutputLimits(0x84,     0xFF);
            front_migiusiro.setOutputLimits(0x84,   0xFF);
            front_hidarimae.setOutputLimits(0x7C,   0x83);
            front_hidariusiro.setOutputLimits(0x7C, 0x83);
        }
        //右側が前に出ちゃった♥(左側だけ回して右側は停止)
        else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
            front_migimae.setOutputLimits(0x7C,     0x83);
            front_migiusiro.setOutputLimits(0x7C,   0x83);
            front_hidarimae.setOutputLimits(0x84,   0xFF);
            front_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //停止(目標より行き過ぎ)
        else if((y_pulse1 > target) && (y_pulse2 > target)) {
            front_migimae.setOutputLimits(0x7C,     0x83);
            front_migiusiro.setOutputLimits(0x7C,   0x83);
            front_hidarimae.setOutputLimits(0x7C,   0x83);
            front_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        front_migimae.setMode(AUTO_MODE);
        front_migiusiro.setMode(AUTO_MODE);
        front_hidarimae.setMode(AUTO_MODE);
        front_hidariusiro.setMode(AUTO_MODE);

        //目標値
        front_migimae.setSetPoint(target);
        front_migiusiro.setSetPoint(target);
        front_hidarimae.setSetPoint(target);
        front_hidariusiro.setSetPoint(target);

        //センサ出力
        front_migimae.setProcessValue(y_pulse1);
        front_migiusiro.setProcessValue(y_pulse1);
        front_hidarimae.setProcessValue(y_pulse2);
        front_hidariusiro.setProcessValue(y_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = front_migimae.compute();
        migiusiro_data[0]    = front_migiusiro.compute();
        hidarimae_data[0]    = front_hidarimae.compute();
        hidariusiro_data[0]  = front_hidariusiro.compute();

        //制御量をPWM値に変換
        //正転(目標に達してない)
        if((y_pulse1 < target) && (y_pulse2 < target)) {
            true_migimae_data[0]     = migimae_data[0];
            true_migiusiro_data[0]   = migiusiro_data[0];
            true_hidarimae_data[0]   = hidarimae_data[0];
            true_hidariusiro_data[0] = hidariusiro_data[0];
        }
        //左側が前に出ちゃった♥(右側だけ回して左側は停止)
        else if((y_pulse1 + wheel_difference) < y_pulse2) {
            true_migimae_data[0]     = migimae_data[0];
            true_migiusiro_data[0]   = migiusiro_data[0];
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
        //右側が前に出ちゃった♥(左側だけ回して右側は停止)
        else if(y_pulse1 > (y_pulse2 + wheel_difference)) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = hidarimae_data[0];
            true_hidariusiro_data[0] = hidariusiro_data[0];
        }
        //停止(目標より行き過ぎ)
        else if((y_pulse1 > target) && (y_pulse2 > target)) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void back_PID(int target) {

        //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
        back_migimae.setInputLimits(-2147483648,     2147483647);
        back_migiusiro.setInputLimits(-2147483648,   2147483647);
        back_hidarimae.setInputLimits(-2147483648,   2147483647);
        back_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //逆転(目標に達してない)
        if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
            back_migimae.setOutputLimits(0x00,     0x7B);
            back_migiusiro.setOutputLimits(0x00,   0x7B);
            //back_hidarimae.setOutputLimits(0x00,   0x73);
            //back_hidariusiro.setOutputLimits(0x00, 0x73);
            back_hidarimae.setOutputLimits(0x00,   0x7B);
            back_hidariusiro.setOutputLimits(0x00, 0x7B);
        }
        //左側が後に出ちゃった♥(右側だけ回して左側は停止)
        else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){
            back_migimae.setOutputLimits(0x00,     0x7B);
            back_migiusiro.setOutputLimits(0x00,   0x7B);
            back_hidarimae.setOutputLimits(0x7C,   0x83);
            back_hidariusiro.setOutputLimits(0x7C, 0x83);
        }
        //右側が後に出ちゃった♥(左側だけ回して右側は停止)
        else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
            back_migimae.setOutputLimits(0x7C,     0x83);
            back_migiusiro.setOutputLimits(0x7C,   0x83);
            back_hidarimae.setOutputLimits(0x00,   0x7B);
            back_hidariusiro.setOutputLimits(0x00, 0x7B);
        }
        //停止(目標より行き過ぎ)
        else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
            back_migimae.setOutputLimits(0x7C,     0x83);
            back_migiusiro.setOutputLimits(0x7C,   0x83);
            back_hidarimae.setOutputLimits(0x7C,   0x83);
            back_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        back_migimae.setMode(AUTO_MODE);
        back_migiusiro.setMode(AUTO_MODE);
        back_hidarimae.setMode(AUTO_MODE);
        back_hidariusiro.setMode(AUTO_MODE);

        //目標値
        back_migimae.setSetPoint(target*-1);
        back_migiusiro.setSetPoint(target*-1);
        back_hidarimae.setSetPoint(target*-1);
        back_hidariusiro.setSetPoint(target*-1);

        //センサ出力
        back_migimae.setProcessValue(y_pulse1*-1);
        back_migiusiro.setProcessValue(y_pulse1*-1);
        back_hidarimae.setProcessValue(y_pulse2*-1);
        back_hidariusiro.setProcessValue(y_pulse2*-1);

        //制御量(計算結果)
        migimae_data[0]      = back_migimae.compute();
        migiusiro_data[0]    = back_migiusiro.compute();
        hidarimae_data[0]    = back_hidarimae.compute();
        hidariusiro_data[0]  = back_hidariusiro.compute();

        //制御量をPWM値に変換
        //逆転(目標に達してない)
        if((y_pulse1*-1 < target*-1) && (y_pulse2*-1 < target*-1)) {
            true_migimae_data[0]     = 0x7B - migimae_data[0];
            true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
            true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //左側が後に出ちゃった♥(右側だけ回して左側は停止)
        else if((y_pulse1*-1 + wheel_difference) < y_pulse2*-1){
            true_migimae_data[0]     = 0x7B - migimae_data[0];
            true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
        //右側が後に出ちゃった♥(左側だけ回して右側は停止)
        else if(y_pulse1*-1 > (y_pulse2*-1 + wheel_difference)){
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //停止(目標より行き過ぎ)
        else if((y_pulse1*-1 > target*-1) && (y_pulse2*-1 > target*-1)) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void right_PID(int target) {

        //センサ出力値の最小、最大
        right_migimae.setInputLimits(-2147483648,     2147483647);
        right_migiusiro.setInputLimits(-2147483648,   2147483647);
        right_hidarimae.setInputLimits(-2147483648,   2147483647);
        right_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //右進(目標まで達していない)
        if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
           // right_migimae.setOutputLimits(0x00,     0x6C);
            right_migimae.setOutputLimits(0x00,     0x7B);
            right_migiusiro.setOutputLimits(0x84,   0xFF);
            //right_hidarimae.setOutputLimits(0x84,   0xF0);
            right_hidarimae.setOutputLimits(0x84,   0xFF);
            right_hidariusiro.setOutputLimits(0x00, 0x7B);

        }
        //前側が右に出ちゃった♥(後側だけ回して前側は停止)
        else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){
            right_migimae.setOutputLimits(0x7C,     0x83);
            right_migiusiro.setOutputLimits(0x00,   0x7B);
            right_hidarimae.setOutputLimits(0x7C,   0x83);
            right_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //後側が右に出ちゃった♥(前側だけ回して後側は停止)
        else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
            right_migimae.setOutputLimits(0x84,     0xFF);
            right_migiusiro.setOutputLimits(0x7C,   0x83);
            right_hidarimae.setOutputLimits(0x00,   0x7B);
            right_hidariusiro.setOutputLimits(0x7C, 0x83);
        }
        //停止(目標より行き過ぎ)
        else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
            right_migimae.setOutputLimits(0x7C,     0x83);
            right_migiusiro.setOutputLimits(0x7C,   0x83);
            right_hidarimae.setOutputLimits(0x7C,   0x83);
            right_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        right_migimae.setMode(AUTO_MODE);
        right_migiusiro.setMode(AUTO_MODE);
        right_hidarimae.setMode(AUTO_MODE);
        right_hidariusiro.setMode(AUTO_MODE);

        //目標値
        right_migimae.setSetPoint(target*-1);
        right_migiusiro.setSetPoint(target*-1);
        right_hidarimae.setSetPoint(target*-1);
        right_hidariusiro.setSetPoint(target*-1);

        //センサ出力
        right_migimae.setProcessValue(x_pulse1*-1);
        right_migiusiro.setProcessValue(x_pulse2*-1);
        right_hidarimae.setProcessValue(x_pulse1*-1);
        right_hidariusiro.setProcessValue(x_pulse2*-1);

        //制御量(計算結果)
        migimae_data[0]      = right_migimae.compute();
        migiusiro_data[0]    = right_migiusiro.compute();
        hidarimae_data[0]    = right_hidarimae.compute();
        hidariusiro_data[0]  = right_hidariusiro.compute();

        //制御量をPWM値に変換
        //右進(目標まで達していない)
        if((x_pulse1*-1 < target*-1) && (x_pulse2*-1 < target*-1)) {
            true_migimae_data[0]     = 0x7B - migimae_data[0];
            true_migiusiro_data[0]   = migiusiro_data[0];
            true_hidarimae_data[0]   = hidarimae_data[0];
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //前側が右に出ちゃった♥(後側だけ回して前側は停止)
        else if(x_pulse1*-1 > (x_pulse2*-1 + wheel_difference)){
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = migiusiro_data[0];
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //後側が右に出ちゃった♥(前側だけ回して後側は停止)
        else if((x_pulse1*-1 + wheel_difference) < x_pulse2*-1){
            true_migimae_data[0]     = 0x7B - migimae_data[0];
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = hidarimae_data[0];
            true_hidariusiro_data[0] = 0x80;
        }
        //左進(目標より行き過ぎ)
        else if((x_pulse1*-1 > target*-1) && (x_pulse2*-1 > target*-1)) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void left_PID(int target) {

        //センサ出力値の最小、最大
        left_migimae.setInputLimits(-2147483648,     2147483647);
        left_migiusiro.setInputLimits(-2147483648,   2147483647);
        left_hidarimae.setInputLimits(-2147483648,   2147483647);
        left_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //左進(目標まで達していない)
        if((x_pulse1 < target) && (x_pulse2 < target)) {
            left_migimae.setOutputLimits(0x84,     0xED);
            left_migiusiro.setOutputLimits(0x00,   0x7B);
            left_hidarimae.setOutputLimits(0x00,   0x7B);
            left_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //前側が左に出ちゃった♥(後側だけ回して前側は停止)
        else if(x_pulse1 > (x_pulse2 + wheel_difference)){
            left_migimae.setOutputLimits(0x7C,     0x83);
            left_migiusiro.setOutputLimits(0x7C,   0x83);
            left_hidarimae.setOutputLimits(0x10,   0x7B);
            left_hidariusiro.setOutputLimits(0x94, 0xFF);
        }
        //後側が左に出ちゃった♥(前側だけ回して後側は停止)
        else if((x_pulse1 + wheel_difference) < x_pulse2){
            left_migimae.setOutputLimits(0x94,     0xFF);
            left_migiusiro.setOutputLimits(0x10,   0x7B);
            left_hidarimae.setOutputLimits(0x7C,   0x83);
            left_hidariusiro.setOutputLimits(0x7C, 0x83);
        }
        //停止(目標より行き過ぎ)
        else if((x_pulse1 > target) && (x_pulse2 > target)) {
            left_migimae.setOutputLimits(0x7C,     0x83);
            left_migiusiro.setOutputLimits(0x7C,   0x83);
            left_hidarimae.setOutputLimits(0x7C,   0x83);
            left_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        left_migimae.setMode(AUTO_MODE);
        left_migiusiro.setMode(AUTO_MODE);
        left_hidarimae.setMode(AUTO_MODE);
        left_hidariusiro.setMode(AUTO_MODE);

        //目標値
        left_migimae.setSetPoint(target);
        left_migiusiro.setSetPoint(target);
        left_hidarimae.setSetPoint(target);
        left_hidariusiro.setSetPoint(target);

        //センサ出力
        left_migimae.setProcessValue(x_pulse1);
        left_migiusiro.setProcessValue(x_pulse2);
        left_hidarimae.setProcessValue(x_pulse1);
        left_hidariusiro.setProcessValue(x_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = left_migimae.compute();
        migiusiro_data[0]    = left_migiusiro.compute();
        hidarimae_data[0]    = left_hidarimae.compute();
        hidariusiro_data[0]  = left_hidariusiro.compute();

        //制御量をPWM値に変換
        //左進(目標まで達していない)
        if((x_pulse1 < target) && (x_pulse2 < target)) {
            true_migimae_data[0]     = migimae_data[0];
            true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
            true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = hidariusiro_data[0];
        }
        //前側が左に出ちゃった♥(後側だけ回して前側は停止)
        else if(x_pulse1 > (x_pulse2 + wheel_difference)){
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = hidariusiro_data[0];
        }
        //後側が左に出ちゃった♥(前側だけ回して後側は停止)
        else if((x_pulse1 + wheel_difference) < x_pulse2){
            true_migimae_data[0]     = migimae_data[0];
            true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
        //停止(目標より行き過ぎ)
        else if((x_pulse1 > target) && (x_pulse2 > target)) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void turn_right_PID(int target) {

        //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
        turn_right_migimae.setInputLimits(-2147483648,     2147483647);
        turn_right_migiusiro.setInputLimits(-2147483648,   2147483647);
        turn_right_hidarimae.setInputLimits(-2147483648,   2147483647);
        turn_right_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //右旋回(目標に達してない)
        if(x_pulse2 < target) {
            turn_right_migimae.setOutputLimits(0x10,   0x7B);
            turn_right_migiusiro.setOutputLimits(0x10, 0x7B);
            turn_right_hidarimae.setOutputLimits(0x94, 0xFF);
            turn_right_hidariusiro.setOutputLimits(0x94, 0xFF);
        }
        //停止(目標より行き過ぎ)
        else if(x_pulse2 > target) {
            turn_right_migimae.setOutputLimits(0x7C,     0x83);
            turn_right_migiusiro.setOutputLimits(0x7C,   0x83);
            turn_right_hidarimae.setOutputLimits(0x7C,   0x83);
            turn_right_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        turn_right_migimae.setMode(AUTO_MODE);
        turn_right_migiusiro.setMode(AUTO_MODE);
        turn_right_hidarimae.setMode(AUTO_MODE);
        turn_right_hidariusiro.setMode(AUTO_MODE);

        //目標値
        turn_right_migimae.setSetPoint(target);
        turn_right_migiusiro.setSetPoint(target);
        turn_right_hidarimae.setSetPoint(target);
        turn_right_hidariusiro.setSetPoint(target);

        //センサ出力
        turn_right_migimae.setProcessValue(x_pulse2);
        turn_right_migiusiro.setProcessValue(x_pulse2);
        turn_right_hidarimae.setProcessValue(x_pulse2);
        turn_right_hidariusiro.setProcessValue(x_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = turn_right_migimae.compute();
        migiusiro_data[0]    = turn_right_migiusiro.compute();
        hidarimae_data[0]    = turn_right_hidarimae.compute();
        hidariusiro_data[0]  = turn_right_hidariusiro.compute();

        //制御量をPWM値に変換
        //右旋回(目標に達してない)
        if(x_pulse2 < target) {
            true_migimae_data[0]     = 0x7B - migimae_data[0];
            true_migiusiro_data[0]   = 0x7B - migiusiro_data[0];
            true_hidarimae_data[0]   = hidarimae_data[0];
            true_hidariusiro_data[0] = hidariusiro_data[0];
        }
        //停止(目標より行き過ぎ)
        else if(x_pulse2 > target) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void turn_left_PID(int target) {

        //センサ出力値の最小、最大(とりあえずint型が持てる範囲に設定)
        turn_left_migimae.setInputLimits(-2147483648,     2147483647);
        turn_left_migiusiro.setInputLimits(-2147483648,   2147483647);
        turn_left_hidarimae.setInputLimits(-2147483648,   2147483647);
        turn_left_hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //左旋回(目標に達してない)
        if(x_pulse1 < target) {
            turn_left_migimae.setOutputLimits(0x94,   0xFF);
            turn_left_migiusiro.setOutputLimits(0x94, 0xFF);
            turn_left_hidarimae.setOutputLimits(0x10, 0x7B);
            turn_left_hidariusiro.setOutputLimits(0x10, 0x7B);
        }
        //停止(目標より行き過ぎ)
        else if(x_pulse1 > target) {
            turn_left_migimae.setOutputLimits(0x7C,     0x83);
            turn_left_migiusiro.setOutputLimits(0x7C,   0x83);
            turn_left_hidarimae.setOutputLimits(0x7C,   0x83);
            turn_left_hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        turn_left_migimae.setMode(AUTO_MODE);
        turn_left_migiusiro.setMode(AUTO_MODE);
        turn_left_hidarimae.setMode(AUTO_MODE);
        turn_left_hidariusiro.setMode(AUTO_MODE);

        //目標値
        turn_left_migimae.setSetPoint(target);
        turn_left_migiusiro.setSetPoint(target);
        turn_left_hidarimae.setSetPoint(target);
        turn_left_hidariusiro.setSetPoint(target);

        //センサ出力
        turn_left_migimae.setProcessValue(x_pulse1);
        turn_left_migiusiro.setProcessValue(x_pulse1);
        turn_left_hidarimae.setProcessValue(x_pulse1);
        turn_left_hidariusiro.setProcessValue(x_pulse1);

        //制御量(計算結果)
        migimae_data[0]      = turn_left_migimae.compute();
        migiusiro_data[0]    = turn_left_migiusiro.compute();
        hidarimae_data[0]    = turn_left_hidarimae.compute();
        hidariusiro_data[0]  = turn_left_hidariusiro.compute();

        //制御量をPWM値に変換
        //左旋回(目標に達してない)
        if(x_pulse1 < target) {
            true_migimae_data[0]     = migimae_data[0];
            true_migiusiro_data[0]   = migiusiro_data[0];
            true_hidarimae_data[0]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //左旋回(目標より行き過ぎ)
        else if(x_pulse1 > target) {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void dondonkasoku(void) {

        //どんどん加速(正転)
        for(init_send_data[0] = 0x84; init_send_data[0] < 0xFF; init_send_data[0]++){
            i2c.write(0x10, init_send_data, 1);
            i2c.write(0x12, init_send_data, 1);
            i2c.write(0x14, init_send_data, 1);
            i2c.write(0x16, init_send_data, 1);
            wait(0.05);
        }
        //どんどん減速(正転)
        for(init_send_data[0] = 0xFF; init_send_data[0] >= 0x84; init_send_data[0]--){
            i2c.write(0x10, init_send_data, 1);
            i2c.write(0x12, init_send_data, 1);
            i2c.write(0x14, init_send_data, 1);
            i2c.write(0x16, init_send_data, 1);
            wait(0.05);
        }
        //だんだん加速(逆転)
        for(init_send_data[0] = 0x7B; init_send_data[0] > 0x00; init_send_data[0]--){
            i2c.write(0x10, init_send_data, 1);
            i2c.write(0x12, init_send_data, 1);
            i2c.write(0x14, init_send_data, 1);
            i2c.write(0x16, init_send_data, 1);
            wait(0.05);
        }
        //だんだん減速(逆転)
        for(init_send_data[0] = 0x00; init_send_data[0] <= 0x7B; init_send_data[0]++){
            i2c.write(0x10, init_send_data, 1);
            i2c.write(0x12, init_send_data, 1);
            i2c.write(0x14, init_send_data, 1);
            i2c.write(0x16, init_send_data, 1);
            wait(0.05);
        }
}