File content as of revision 15:d022288aec51:

/* -------------------------------------------------------------------  */
/* NHK ROBOCON 2019 Ibaraki Kosen A team Automatic                      */
/* Nucleo Type: F446RE                                                  */
/* designed by Yuhi Takaku from 5D, mail: rab1sy23@gmail.com            */
/* Sensor: encorder*4                                                   */
/* -------------------------------------------------------------------  */
/* PID関数を各方向毎に追加した                                              */
/* -------------------------------------------------------------------  */
#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

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(4500000.0, 0.0, 0.0, 0.001);
PID right_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID right_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID right_hidariusiro(4500000.0, 0.0, 0.0, 0.001);

//左進
PID left_migimae(4500000.0, 0.0, 0.0, 0.001);
PID left_migiusiro(4500000.0, 0.0, 0.0, 0.001);
PID left_hidarimae(4500000.0, 0.0, 0.0, 0.001);
PID left_hidariusiro(4500000.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(PC_9);
DigitalOut USR_LED2(PC_8);
DigitalOut USR_LED3(PC_6);
DigitalOut USR_LED4(PC_5);

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);

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

//操作の段階変数
unsigned int phase = 0;

//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 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();

    while(1) {
        
        get_pulses();
        print_pulses();
        front(500);
        //back(500);
        //right(500);
        //left(500);
        //turn_right(500);
        //turn_left(500);
    }
}

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

    //通信ボーレートの設定
    //pc.baud(460800);
    pc.baud(9600);
    
    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("x1: %d, x2: %d, y1: %d, y2: %d\n\r", x_pulse1, x_pulse2, y_pulse1, y_pulse2);
}

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型が持てる範囲に設定)
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //正転(目標に達してない)
        if((y_pulse1 < target) && (y_pulse2 < target)) {
            migimae.setOutputLimits(0x84,     0xFF);
            migiusiro.setOutputLimits(0x84,   0xFF);
            hidarimae.setOutputLimits(0x84,   0xFF);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //逆転(目標より行き過ぎ)
        else if((y_pulse1 > target) && (y_pulse2 > target)) {
            migimae.setOutputLimits(0x00,     0x7B);
            migiusiro.setOutputLimits(0x00,   0x7B);
            hidarimae.setOutputLimits(0x00,   0x7B);
            hidariusiro.setOutputLimits(0x00, 0x7B);
        } else {
            migimae.setOutputLimits(0x7C,     0x83);
            migiusiro.setOutputLimits(0x7C,   0x83);
            hidarimae.setOutputLimits(0x7C,   0x83);
            hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(y_pulse1);
        migiusiro.setProcessValue(y_pulse1);
        hidarimae.setProcessValue(y_pulse2);
        hidariusiro.setProcessValue(y_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = 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]     = 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 {
            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型が持てる範囲に設定)
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //逆転(目標に達してない)
        if((abs(y_pulse1) < target) && (abs(y_pulse2) < target)) {
            migimae.setOutputLimits(0x00,     0x7B);
            migiusiro.setOutputLimits(0x00,   0x7B);
            hidarimae.setOutputLimits(0x00,   0x7B);
            hidariusiro.setOutputLimits(0x0, 0x7B);
        }
        //正転(目標より行き過ぎ)
        else if((abs(y_pulse1) > target) && (abs(y_pulse2) > target)) {
            migimae.setOutputLimits(0x84,     0xFF);
            migiusiro.setOutputLimits(0x84,   0xFF);
            hidarimae.setOutputLimits(0x84,   0xFF);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        } else {
            migimae.setOutputLimits(0x7C,     0x83);
            migiusiro.setOutputLimits(0x7C,   0x83);
            hidarimae.setOutputLimits(0x7C,   0x83);
            hidariusiro.setOutputLimits(0x7C, 0x83);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(abs(y_pulse1));
        migiusiro.setProcessValue(abs(y_pulse1));
        hidarimae.setProcessValue(abs(y_pulse2));
        hidariusiro.setProcessValue(abs(y_pulse2));

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = 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];
        } 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) {

        //センサ出力値の最小、最大
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //右進(目標まで達していない)
        if((x_pulse1 < target) && (x_pulse2 < target)) {
            migimae.setOutputLimits(0x00,     0x7B);
            migiusiro.setOutputLimits(0x84,   0xFF);
            hidarimae.setOutputLimits(0x84,   0xFF);
            hidariusiro.setOutputLimits(0x00, 0x7B);
        }
        //左進(目標より行き過ぎ)
        else if((x_pulse1 > target) && (x_pulse2 > target)) {
            migimae.setOutputLimits(0x84,     0xFF);
            migiusiro.setOutputLimits(0x00,   0x7B);
            hidarimae.setOutputLimits(0x00,   0x7B);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(x_pulse1);
        migiusiro.setProcessValue(x_pulse1);
        hidarimae.setProcessValue(x_pulse2);
        hidariusiro.setProcessValue(x_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        //制御量をPWM値に変換
        //右進(目標まで達していない)
        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];
        }
        //左進(目標より行き過ぎ)
        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]   = 0x7B - 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 left_PID(unsigned int target) {
    
        //センサ出力値の最小、最大
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //左進(目標まで達していない)
        if((abs(x_pulse1) < target) && (abs(x_pulse2) < target)) {
            migimae.setOutputLimits(0x84,     0xFF);
            migiusiro.setOutputLimits(0x00,   0x7B);
            hidarimae.setOutputLimits(0x00,   0x7B);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //右進(目標より行き過ぎ)
        else if((abs(x_pulse1) > target) && (abs(x_pulse2) > target)) {
            migimae.setOutputLimits(0x00,     0x7B);
            migiusiro.setOutputLimits(0x84,   0xFF);
            hidarimae.setOutputLimits(0x84,   0xFF);
            hidariusiro.setOutputLimits(0x00, 0x7B);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(abs(x_pulse1));
        migiusiro.setProcessValue(abs(x_pulse1));
        hidarimae.setProcessValue(abs(x_pulse2));
        hidariusiro.setProcessValue(abs(x_pulse2));

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        //制御量をPWM値に変換
        //左進(目標まで達していない)
        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];
        }
        //右進(目標より行き過ぎ)
        else 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 {
            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型が持てる範囲に設定)
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //右旋回(目標に達してない)
        if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_pulse2 < target)) {
            migimae.setOutputLimits(0x00,   0x7B);
            migiusiro.setOutputLimits(0x00, 0x7B);
            hidarimae.setOutputLimits(0x84, 0xFF);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        }
        //左旋回(目標より行き過ぎ)
        else if((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > target)) {
            migimae.setOutputLimits(0x84,     0xFF);
            migiusiro.setOutputLimits(0x84,   0xFF);
            hidarimae.setOutputLimits(0x00,   0x7B);
            hidariusiro.setOutputLimits(0x00, 0x7B);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(x_pulse1);
        migiusiro.setProcessValue(x_pulse2);
        hidarimae.setProcessValue(y_pulse1);
        hidariusiro.setProcessValue(y_pulse2);

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        //制御量をPWM値に変換
        //右旋回(目標に達してない)
        if((x_pulse1 < target) && (x_pulse2 < target) && (y_pulse1 < target) && (y_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((x_pulse1 > target) && (x_pulse2 > target) && (y_pulse1 > target) && (y_pulse2 > 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 {
            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型が持てる範囲に設定)
        migimae.setInputLimits(-2147483648,     2147483647);
        migiusiro.setInputLimits(-2147483648,   2147483647);
        hidarimae.setInputLimits(-2147483648,   2147483647);
        hidariusiro.setInputLimits(-2147483648, 2147483647);

        //制御量の最小、最大
        //右旋回(目標に達してない)
        if((abs(x_pulse1) < target) && (x_pulse2 < target) && (y_pulse1 < target) && (abs(y_pulse2) < target)) {
            migimae.setOutputLimits(0x84,   0xFF);
            migiusiro.setOutputLimits(0x84, 0xFF);
            hidarimae.setOutputLimits(0x00, 0x7B);
            hidariusiro.setOutputLimits(0x00, 0x7B);
        }
        //左旋回(目標より行き過ぎ)
        else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (y_pulse1 > target) && (abs(y_pulse2) > target)) {
            migimae.setOutputLimits(0x00,     0x7B);
            migiusiro.setOutputLimits(0x00,   0x7B);
            hidarimae.setOutputLimits(0x84,   0xFF);
            hidariusiro.setOutputLimits(0x84, 0xFF);
        }

        //よくわからんやつ
        migimae.setMode(AUTO_MODE);
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);

        //目標値
        migimae.setSetPoint(target);
        migiusiro.setSetPoint(target);
        hidarimae.setSetPoint(target);
        hidariusiro.setSetPoint(target);

        //センサ出力
        migimae.setProcessValue(abs(x_pulse1));
        migiusiro.setProcessValue(abs(y_pulse1));
        hidarimae.setProcessValue(abs(y_pulse2));
        hidariusiro.setProcessValue(abs(x_pulse2));

        //制御量(計算結果)
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        //制御量をPWM値に変換
        //右旋回(目標に達してない)
        if((abs(x_pulse1) < target) && (x_pulse2 < target) && (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]   = 0x7B - hidarimae_data[0];
            true_hidariusiro_data[0] = 0x7B - hidariusiro_data[0];
        }
        //左旋回(目標より行き過ぎ)
        else if((abs(x_pulse1) > target) && (x_pulse2 > target) && (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]   = 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);
        }
}