File content as of revision 18:851f783ec516:

/* -------------------------------------------------------------------  */
/* 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"

//PIDGain of wheels
#define Kp 4500000.0
//#define Kp 10000000.0
#define Ti 0.0
#define Td 0.0

#define RED     0
#define BLUE    1

PID migimae(Kp, Ti, Td, 0.001);
PID migiusiro(Kp, Ti, Td, 0.001);
PID hidarimae(Kp, Ti, Td, 0.001);
PID hidariusiro(Kp, Ti, Td, 0.001);

//前進
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 wheel1(D2, D3, NC, 624);
//QEI wheel2(D5, D4, NC, 624);

Timer counter;

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

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

//MD送信データ変数
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 RDATA;
char baff;
int flug = 0;

int limit_data = 0;

unsigned int start_switch_counter = 0;

bool front_limit = 0;
bool right_limit = 0;
bool back_limit = 0;

//関数のプロトタイプ宣言
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 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;
    phase = 1;
    
    while(1) {
    
        get_pulses();
        print_pulses();
        get_emergency();
        read_limit();

        if(zone == BLUE) {
            switch(phase) {
                case 0:
                    if(!start_switch) {
                        phase = 1;
                    }
                case 1:
                    left(12000);
                    if((x_pulse1 > 12000) || (x_pulse2 > 12000)) {
                        phase = 2;
                    }
                    break;
                case 2:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 3;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;
                case 3:
                    counter.reset();
                    turn_right(535);
                    if(x_pulse2 > 535) {
                        phase = 4;
                    }
                    break;
                case 4:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        //本当は5だけど今はリミットスイッチ無の為phase7まで飛ばす
                        //phase = 5;
                        phase = 7;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;
                case 5:
                    counter.reset();
                    right(-500);
                    if((x_pulse1*-1 > 500) || (x_pulse2*-1 > 500)) {
                        phase = 6;
                    }
                    //if(!right_limit) {
                        //phase = 6;
                    //}
                    break;
                case 6:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 7;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;
                case 7:
                    counter.reset();
                    front(3000);
                    if((y_pulse1 > 3000) || (y_pulse2 > 3000)) {
                        phase = 8;
                    }
                    break;
                case 8:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 9;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;
                case 9:
                    counter.reset();
                    right(-3000);
                    if((x_pulse1*-1 > 3000) || (x_pulse2*-1 > 3000)) {
                        phase = 10;
                    }
                    //if(!right_limit) {
                        //phase = 10;
                    //}
                    break;
                case 10:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 11;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;
                case 11:
                    counter.reset();
                    front(21500);
                    if((y_pulse1 > 21500) || (y_pulse2 > 21500)) {
                        phase = 12;
                    }
                    break;
                case 12:
                    stop();
                    counter.start();
                    if(counter.read() > 1.0f) {
                        phase = 13;
                        wheel_x1.reset();
                        wheel_x2.reset();
                        wheel_y1.reset();
                        wheel_y2.reset();
                    }
                    break;                
                case 13:
                    left(8000);
                    if((x_pulse1 > 8000) || (x_pulse2 > 8000)) {
                        phase = 14;
                    }
                    break;
                case 14:
                    stop();
                default:
                    break;
            }
        }
                
        /*
        if(counter.read() < 5.00f) {
            counter.start();
            front(3000);
        }
        else if(counter.read() >= 5.00f && counter.read() < 10.00f) {
            right(-3000);
        }
        else if(counter.read() >= 10.00f && counter.read() < 15.00f) {
            back(-3000);
        }
        else if(counter.read() >= 15.00f && counter.read() < 20.00f) {
            left(3000);
        }
        else if(counter.read() >= 20.00f && counter.read() < 25.00f) {
            turn_right(535);
        }
        else if(counter.read() >= 25.00f && counter.read() < 30.00f) {
            turn_left(674);
        }
        else if(counter.read() >= 30.00f) {
            counter.reset();
        }
        */
    }
}

void init(void) {
    
    //緊急停止用信号ピンをLow
    //emergency = 0;

    //通信ボーレートの設定
    pc.baud(460800);
    //pc.baud(9600);
    
    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;
}

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);
    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("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();
        
        if((limit_data & 0b00000001) == 0x01) {
            USR_LED1 = 1;   USR_LED2 = 0;   USR_LED3 = 0;   USR_LED4 = 0;
        }
        if((limit_data & 0b00000010) == 0x02) {
            USR_LED1 = 0;   USR_LED2 = 1;   USR_LED3 = 0;   USR_LED4 = 0;
        }
        if((limit_data & 0b00000011) == 0x03) {
            USR_LED1 = 1;   USR_LED2 = 1;   USR_LED3 = 0;   USR_LED4 = 0;
        }
        if((limit_data & 0b00000100) == 0x04) {
            USR_LED1 = 0;   USR_LED2 = 0;   USR_LED3 = 1;   USR_LED4 = 0;
        }
        if((limit_data & 0b00001000) == 0x08) {
            USR_LED1 = 0;   USR_LED2 = 0;   USR_LED3 = 0;   USR_LED4 = 1;
        }
        if((limit_data & 0b00001100) == 0x0C) {
            USR_LED1 = 0;   USR_LED2 = 0;   USR_LED3 = 1;   USR_LED4 = 1;
        } 
        if(limit_data == 0x00) {
            USR_LED1 = 0;   USR_LED2 = 0;   USR_LED3 = 0;   USR_LED4 = 0;
        }
}

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 < target) && (y_pulse2 > target)) {
            front_migimae.setOutputLimits(0x84,     0xFF);
            front_migiusiro.setOutputLimits(0x84,   0xFF);
            front_hidarimae.setOutputLimits(0x7C,   0x83);
            front_hidariusiro.setOutputLimits(0x7C, 0x83);
        }
        //右側が前に出ちゃった♥(左側だけ回して右側は停止)
        else if((y_pulse1 > target) && (y_pulse2 < target)) {
            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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }

        //よくわからんやつ
        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 < target) && (y_pulse2 > target)) {
            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 > target) && (y_pulse2 < target)) {
            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 < target*-1) && (y_pulse2*-1 > target*-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 > target*-1) && (y_pulse2*-1 < target*-1)) {
            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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }

        //よくわからんやつ
        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 < 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]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
        //右側が後に出ちゃった♥(左側だけ回して右側は停止)
        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]   = 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 > target*-1) && (x_pulse2*-1 < target*-1)) {
            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 < target*-1) && (x_pulse2*-1 > target*-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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }

        //よくわからんやつ
        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 > target*-1) && (x_pulse2*-1 < target*-1)) {
            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 < target*-1) && (x_pulse2*-1 > target*-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 > target) && (x_pulse2 < target)) {
            left_migimae.setOutputLimits(0x7C,     0x83);
            left_migiusiro.setOutputLimits(0x7C,   0x83);
            left_hidarimae.setOutputLimits(0x10,   0x7B);
            left_hidariusiro.setOutputLimits(0x94, 0xFF);
        }
        //後側が左に出ちゃった♥(前側だけ回して後側は停止)
        else if((x_pulse1 < target) && (x_pulse2 > target)) {
            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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }
        
        //よくわからんやつ
        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 > target) && (x_pulse2 < target)) {
            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 < target) && (x_pulse2 > target)) {
            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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }

        //よくわからんやつ
        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);
            wheel_x1.reset();
            wheel_x2.reset();
            wheel_y1.reset();
            wheel_y2.reset();
        }

        //よくわからんやつ
        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);
        }
}