File content as of revision 16:05b26003da50:

/* -------------------------------------------------------------------  */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic                      */
/* Nucleo Type: F446RE                                                  */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com            */
/* Sensor: encorder*4                                                   */
/* -------------------------------------------------------------------  */
/* 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(10000000.0, 0.0, 0.0, 0.001);
PID right_migiusiro(10000000.0, 0.0, 0.0, 0.001);
PID right_hidarimae(10000000.0, 0.0, 0.0, 0.001);
PID right_hidariusiro(10000000.0, 0.0, 0.0, 0.001);

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

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

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

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

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

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

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

DigitalIn start_switch(PB_12);
//InterruptIn 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);

Ticker  look_switch;

//エンコーダ値格納変数
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];

//関数のプロトタイプ宣言
void incriment(void);
void init(void);
void init_send(void);
void get_pulses(void);
void print_pulses(void);
void front(unsigned int target);
void back(unsigned int target);
void right(unsigned int target);
void left(unsigned int target);
void turn_right(unsigned int target);
void turn_left(unsigned int target);
void front_PID(unsigned int target);
void back_PID(unsigned int target);
void right_PID(unsigned int target);
void left_PID(unsigned int target);
void turn_right_PID(unsigned int target);
void turn_left_PID(unsigned int target);
void dondonkasoku(void);

int main(void) {
    
    init();
    init_send();
    
    //look_switch.attach_us(&incriment, 100000.0);
    //start_switch.fall(&incriment);
    
    while(1) {
        if(!start_switch)
            break;
    }
    
    while(1) {
        
        get_pulses();
        print_pulses();
        
        if(start_switch == 0) {
            USR_LED1 = 1;   USR_LED2 = 1;   USR_LED3 = 1;   USR_LED4 = 1;
        } else { 
            USR_LED1 = 0;   USR_LED2 = 0;   USR_LED3 = 0;   USR_LED4 = 0;
        }
                
        if(phase == 0) {
            front(10000);
        }
        else if(phase == 1) {
            right(10000);
        }
        else if(phase == 2) {
            back(10000);
        }
        else if(phase == 3) {
            left(10000);
        }
        
        /*
        switch(phase) {
            case 0:
                turn_right(600);
                break;
            case 1:
                right(14000);
                break;
            case 2: 
                front(5000);
                break;
            case 3:
                right(5000);
                break;
            case 4:
                front(20000);
                break;
            default:
                break;
        }
        */
        
        //back(5000);
        //right(14000);
        //left(14000);  //直進補正済
        //turn_right(600);
        //turn_left(300);
    }
}

void incriment(void) {
    if(start_switch == 0) {
        phase++;
    }
}
void init(void) {
    
    //緊急停止用信号ピンをLow
    emergency = 0;
    //USR_LED1 = 1;   USR_LED2 = 1;   USR_LED3 = 1;   USR_LED4 = 1;

    //通信ボーレートの設定
    pc.baud(460800);
    //pc.baud(9600);
    
    start_switch.mode(PullUp);
    
    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_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("phase: %d\n\r", phase);
        //pc.printf("x1: %d, x2: %d, y1: %d, y2: %d, p: %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 front(unsigned 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(unsigned 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(unsigned 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(unsigned 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(unsigned 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(unsigned 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 front_PID(unsigned 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_hidarimae.setOutputLimits(0x84,   0xFF);
            front_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //逆転(目標より行き過ぎ)
        else if((y_pulse1 > target) || (y_pulse2 > target)) {
            //front_migimae.setOutputLimits(0x00,     /*0x7B*/0x79);
            //front_migiusiro.setOutputLimits(0x00,   /*0x7B*/0x76);
            //front_hidarimae.setOutputLimits(0x00,   /*0x7B*/0x79);
            //front_hidariusiro.setOutputLimits(0x00, /*0x7B*/0x79);
            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 > target) || (y_pulse2 > target)) {
            //true_migimae_data[0]     = 0x79 - migimae_data[0];
            //true_migiusiro_data[0]   = 0x76 - migiusiro_data[0];
            //true_hidarimae_data[0]   = 0x79 - hidarimae_data[0];
            //true_hidariusiro_data[0] = 0x79 - hidariusiro_data[0];
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        } else {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

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

        //制御量の最小、最大
        //逆転(目標に達してない)
        if((abs(y_pulse1) < target) || (abs(y_pulse2) < target)) {
            back_migimae.setOutputLimits(0x00,     0x7B);
            back_migiusiro.setOutputLimits(0x00,   0x7B);
            back_hidarimae.setOutputLimits(0x00,   0x73);
            back_hidariusiro.setOutputLimits(0x00, 0x73);
        }
        //正転(目標より行き過ぎ)
        else if((abs(y_pulse1) > target) || (abs(y_pulse2) > target)) {
            //back_migimae.setOutputLimits(0x84,     0xFF);
            //back_migiusiro.setOutputLimits(0x84,   0xFF);
            //back_hidarimae.setOutputLimits(0x84,   0xFF);
            //back_hidariusiro.setOutputLimits(0x84, 0xFF);
            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);
        back_migiusiro.setSetPoint(target);
        back_hidarimae.setSetPoint(target);
        back_hidariusiro.setSetPoint(target);

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

        //制御量(計算結果)
        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((abs(y_pulse1) < target) || (abs(y_pulse2) < target)) {
            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((abs(y_pulse1) > target) || (abs(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];
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        } else {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void right_PID(unsigned int target) {

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

        //制御量の最小、最大
        //右進(目標まで達していない)
        if((abs(x_pulse1) < target) || (abs(x_pulse2) < target)) {
            right_migimae.setOutputLimits(0x00,     0x6C);
            right_migiusiro.setOutputLimits(0x84,   0xFF);
            right_hidarimae.setOutputLimits(0x84,   0xF0);
            right_hidariusiro.setOutputLimits(0x00, 0x7B);
            //right_migiusiro.setOutputLimits(0x84,   0xF1);
            //right_hidariusiro.setOutputLimits(0x0E, 0x7B);
        }
        //左進(目標より行き過ぎ)
        else if((abs(x_pulse1) > target) || (abs(x_pulse2) > target)) {
            //right_migimae.setOutputLimits(0x84,     0xFF);
            //right_migiusiro.setOutputLimits(0x00,   0x7B);
            //right_hidarimae.setOutputLimits(0x00,   0x7B);
            //right_hidariusiro.setOutputLimits(0x84, 0xFF);
            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);
        right_migiusiro.setSetPoint(target);
        right_hidarimae.setSetPoint(target);
        right_hidariusiro.setSetPoint(target);

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

        //制御量(計算結果)
        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((abs(x_pulse1) < target) || (abs(x_pulse2) < target)) {     
            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((abs(x_pulse1) > target) || (abs(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];
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        } else {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void left_PID(unsigned 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,   0x69);
            left_hidariusiro.setOutputLimits(0x84, 0xFF);
            //left_migimae.setOutputLimits(0x84,     0xF6);
            //left_hidarimae.setOutputLimits(0x09,   0x7B);
        }
        //右進(目標より行き過ぎ)
        else if((x_pulse1 > target) || (x_pulse2 > target)) {
            //left_migimae.setOutputLimits(0x00,     0x7B);
            //left_migiusiro.setOutputLimits(0x84,   0xFF);
            //left_hidarimae.setOutputLimits(0x84,   0xFF);
            //left_hidariusiro.setOutputLimits(0x00, 0x7B);
            //left_migimae.setOutputLimits(0x09,     0x7B);
            //left_migiusiro.setOutputLimits(0x88,   0xFF);
            //left_hidarimae.setOutputLimits(0x84,   0xF6);
            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 > target) || (x_pulse2 > target)) {
            //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];
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        } else {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void turn_right_PID(unsigned 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((abs(x_pulse1) < target) || (x_pulse2 < target)) {
        //if((x_pulse1 < target) || (abs(x_pulse2) < target) || (abs(y_pulse1) < target) || (y_pulse2 < target)) {
        if(abs(x_pulse1) < target) {
            turn_right_migimae.setOutputLimits(0x00,   0x7B);
            turn_right_migiusiro.setOutputLimits(0x00, 0x7B);
            turn_right_hidarimae.setOutputLimits(0x84, 0xFF);
            turn_right_hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //左旋回(目標より行き過ぎ)
        //else if((abs(x_pulse1) > target) || (x_pulse2 > target)) {
        //else if((x_pulse1 > target) || (abs(x_pulse2) > target) || (abs(y_pulse1) > target) || (y_pulse2 > target)) {
        else if(abs(x_pulse1) > target) {
            //turn_right_migimae.setOutputLimits(0x84,     0xFF);
            //turn_right_migiusiro.setOutputLimits(0x84,   0xFF);
            //turn_right_hidarimae.setOutputLimits(0x00,   0x7B);
            //turn_right_hidariusiro.setOutputLimits(0x00, 0x7B);
            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(abs(x_pulse1));
        turn_right_migiusiro.setProcessValue(abs(x_pulse1));
        turn_right_hidarimae.setProcessValue(abs(x_pulse1));
        turn_right_hidariusiro.setProcessValue(abs(x_pulse1));

        //制御量(計算結果)
        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((abs(x_pulse1) < target) || (x_pulse2 < target)) {
        //if((x_pulse1 < target) || (abs(x_pulse2) < target) || (abs(y_pulse1) < target) || (y_pulse1 < target)) {
        if(abs(x_pulse1) < 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((abs(x_pulse1) > target) || (x_pulse2 > target)) {
        //else if((x_pulse1 > target) || (abs(x_pulse2) > target) || (abs(y_pulse1) > target) || (y_pulse2 > target)) {
        else if(abs(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];
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        } else {
            true_migimae_data[0]     = 0x80;
            true_migiusiro_data[0]   = 0x80;
            true_hidarimae_data[0]   = 0x80;
            true_hidariusiro_data[0] = 0x80;
        }
}

void turn_left_PID(unsigned 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) || (abs(x_pulse2) < target)) {
        //if((abs(x_pulse1) < target) || (x_pulse2 < target) || (y_pulse1 < target) || (abs(y_pulse2) < target)) {
        if(x_pulse1 < target) {
            turn_left_migimae.setOutputLimits(0x84,   0xFF);
            turn_left_migiusiro.setOutputLimits(0x84, 0xFF);
            turn_left_hidarimae.setOutputLimits(0x00, 0x7B);
            turn_left_hidariusiro.setOutputLimits(0x00, 0x7B);
        }
        //左旋回(目標より行き過ぎ)
        //else if((x_pulse1 > target) || (abs(x_pulse2) > target)) { 
        //else if((abs(x_pulse1) > target) || (x_pulse2 > target) || (y_pulse1 > target) || (abs(y_pulse2) > target)) {
        else if(x_pulse1 > target) {
            turn_left_migimae.setOutputLimits(0x00,     0x7B);
            turn_left_migiusiro.setOutputLimits(0x00,   0x7B);
            turn_left_hidarimae.setOutputLimits(0x84,   0xFF);
            turn_left_hidariusiro.setOutputLimits(0x84, 0xFF);
        }

        //よくわからんやつ
        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) || (abs(x_pulse2) < target)) {
        //if((abs(x_pulse1) < target) || (x_pulse2 < target) || (y_pulse1 < target) || (abs(y_pulse2) < target)) {
        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) || (abs(x_pulse2) > target)) { 
        //else if((abs(x_pulse1) > target) || (x_pulse2 > target) || (y_pulse1 > target) || (abs(y_pulse2) > target)) {
        else if(x_pulse1 > 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 {
            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);
        }
}