File content as of revision 4:df334779a69e:

/* ------------------------------------------------------------------- */
/* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
/* ------------------------------------------------------------------- */
/* このプログラムは上記のロボット専用の制御プログラムである。 */
/* ------------------------------------------------------------------- */
/* 対応機種: NUCLEO-F446RE */
/* ------------------------------------------------------------------- */
/* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
/* ------------------------------------------------------------------- */
/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
#include "hcsr04.h"
#define PI  3.14159265359
#define Kp 20.0
#define Ki 0.02
#define Kd 0.0
//#define roller_Kp 0.5
//#define roller_Ki 0.3
#define roller_Kp 2.5
#define roller_Ki 0.01

//右前オムニ
PID migimae(Kp, Ki, Kd, 0.001);
//右後オムニ
PID migiusiro(Kp, Ki, Kd, 0.001);
//左前オムニ
PID hidarimae(Kp, Ki, Kd, 0.001);
//左後オムニ
PID hidariusiro(Kp, Ki, Kd, 0.001);
//前ローラー
PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
//後ローラー
PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);

//MDとの通信ポート
I2C i2c(PB_9, PB_8);  //SDA, SCL
//PCとの通信ポート
Serial pc(USBTX, USBRX);    //TX, RX
//フォトインタラプタ制御基板からの受信ポート
Serial photo(NC, PC_11);    //TX, RX
//TWE-Liteからの受信ポート
Serial twe(PC_12, PD_2);    //TX, RX

//超音波センサ1
HCSR04 sonic(PB_3, PB_10);    //Trig, Echo
//超音波センサ2
//HCSR04 sonic2(PC_13, PA_15);    //Trig, Echo
//超音波センサ3
//HCSR04 sonic3(PC_15, PC_14);    //Trig, Echo
//超音波センサ4
//HCSR04 sonic4(PH_1 , PH_0 );    //Trig, Echo

//赤、青ゾーン切り替え用スイッチ
DigitalIn side(PC_1);
//スタートボタン
DigitalIn start_button(PC_4);
//スイッチ1
DigitalIn user_switch1(PB_0);
//スイッチ2
DigitalIn user_switch2(PA_4);
//スイッチ3
DigitalIn user_switch3(PA_3);
//スイッチ4
//以下の文を入れるとロリコンが読めなくなる
//DigitalIn user_switch4(PA_2);
//スイッチ5
DigitalIn user_switch5(PA_10);

//フォトインタラプタ
DigitalIn photo_interrupter(PA_15);

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

//赤ゾーンLED
DigitalOut blue_side(PC_0);
//青ゾーンLED
DigitalOut red_side(PC_3);
//スタートLED
DigitalOut start_LED(PA_8);
//LED1
DigitalOut myled1(PC_6);
//LED2
DigitalOut myled2(PC_5);
//LED3
DigitalOut myled3(PA_12);
//LED4
DigitalOut myled4(PB_1);
//LED5
DigitalOut myled5(PA_5);

//前ローラー
QEI front_roller_wheel(PA_0, PA_1, NC, 624);
//後ローラー
QEI back_roller_wheel(PB_5, PB_4, NC, 624);
//計測オムニ1
//QEI measure1_wheel(PC_9, PC_8, NC, 624);
//計測オムニ2(使用不可)
//QEI measure2_wheel(PB_3, PB_10, NC, 624);
//右前オムニ
QEI migimae_wheel(PB_6 , PA_7 , NC, 624);
//右後オムニ
QEI migiusiro_wheel(PA_11, PB_12, NC, 624);
//左前オムニ
QEI hidarimae_wheel(PB_2, PB_15, NC, 624);
//左後オムニ
QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);

Ticker get_rps;
Ticker get_distance;
Timer timer;

int roller_flag = 0;
int loading_state = 0;

int migimae_rpm;
int migiusiro_rpm;
int hidarimae_rpm;
int hidariusiro_rpm;
int measure1_rpm;
//int measure2_rpm;
int front_roller_rpm;
int back_roller_rpm;

int migimae_pulse;
int migiusiro_pulse;
int hidarimae_pulse;
int hidariusiro_pulse;
int measure1_pulse;
//int measure2_pulse;
int front_roller_pulse;
int back_roller_pulse;

int ave_migimae_pulse[10];
int ave_migiusiro_pulse[10];
int ave_hidarimae_pulse[10];
int ave_hidariusiro_pulse[10];
int ave_measure1_pulse[10];
//int ave_measure2_pulse[10];
int ave_front_roller_pulse[10];
int ave_back_roller_pulse[10];


int migimae_counter;
int migiusiro_counter;
int hidarimae_counter;
int hidariusiro_counter;
int measure1_counter;
//int measure2_counter;
int front_roller_counter;
int back_roller_counter;

char send_data[1];
char true_send_data[1];

char migimae_data[1];
char migiusiro_data[1];
char hidarimae_data[1];
char hidariusiro_data[1];
char front_roller_data[1];
char back_roller_data[1];
char loading_data[1];
char cylinder_data[1];

char true_migimae_data[1];
char true_migiusiro_data[1];
char true_hidarimae_data[1];
char true_hidariusiro_data[1];

int line_state = 0;

unsigned int distance;

/* ロリコン処理関数 */
void flip();
int front_PID(int RF, int RB, int LF, int LB);
int back_PID(int RF, int RB, int LF, int LB);
int rihgt_PID(int RF, int RB, int LF, int LB);
int left_PID(int RF, int RB, int LF, int LB);
int right_front_PID(int RB, int LF);
int right_back_PID(int RF, int LB);
int left_front_PID(int RF, int LB);
int left_back_PID(int RB, int RF);
int turn_right_PID(int RF, int RB, int LF, int LB);
int turn_left_PID(int RF, int RB, int LF, int LB);
int roller_PID(int front, int back);
void linetrace();
void ultrasonic();

void flip() {
    migimae_pulse      = migimae_wheel.getPulses();
    migiusiro_pulse    = migiusiro_wheel.getPulses();
    hidarimae_pulse    = hidarimae_wheel.getPulses();
    hidariusiro_pulse  = hidariusiro_wheel.getPulses();
    //measure1_pulse     = measure1_wheel.getPulses();
    //measure2_pulse     = measure2_wheel.getPulses();
    front_roller_pulse = front_roller_wheel.getPulses();
    back_roller_pulse  = back_roller_wheel.getPulses();

    //((40ms*25 = 1s)* 60 = 1min) / 分解能
    migimae_rpm = migimae_pulse           * 25 * 60 / 1200;
    migiusiro_rpm = migiusiro_pulse       * 25 * 60 / 1200;
    hidarimae_rpm = hidarimae_pulse       * 25 * 60 / 1200;
    hidariusiro_rpm = hidariusiro_pulse   * 25 * 60 / 1200;
    //measure1_rpm = measure1_pulse         * 25 * 60 / 1200;
    //measure2_rpm = measure2_pulse         * 25 * 60 / 1200;
    front_roller_rpm = front_roller_pulse * 25 * 60 / 1200;
    back_roller_rpm = back_roller_pulse   * 25 * 60 / 1200;
    
    //pc.printf("RF: %d   RB: %d  LF: %d  LB: %d\n", migimae_rpm, migiusiro_rpm, hidarimae_rpm, hidariusiro_rpm);
    //pc.printf("前: %d, 後: %d, %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance);
    //pc.printf("%d\n", abs(back_roller_rpm));
    //pc.printf("RF: %d, RB: %d\n", migimae_rpm, migiusiro_rpm);

    migimae_wheel.reset();
    migiusiro_wheel.reset();
    hidarimae_wheel.reset();
    hidariusiro_wheel.reset();
    //measure1_wheel.reset();
    //measure2_wheel.reset();
    front_roller_wheel.reset();
    back_roller_wheel.reset();
}
int front_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x0C, 0x7C);
        migiusiro.setOutputLimits(0x0C, 0x7C);
        hidarimae.setOutputLimits(0x0C, 0x7C);
        hidariusiro.setOutputLimits(0x0C, 0x7C);

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

        // 目標値 
        migimae.setSetPoint(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = 0x7D - migimae_data[0];
        true_migiusiro_data[0]   = 0x7D - migiusiro_data[0];
        true_hidarimae_data[0]   = 0x7D - hidarimae_data[0];
        true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
        
        return 0;
}
int back_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x84, 0xFF);
        migiusiro.setOutputLimits(0x84, 0xFF);
        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(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();
        
        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];
       
        return 0;
}
int right_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x84, 0xFF);
        migiusiro.setOutputLimits(0x0C, 0x7C);
        hidarimae.setOutputLimits(0x0C, 0x7C);
        hidariusiro.setOutputLimits(0x84, 0xFF);

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

        // 目標値 
        migimae.setSetPoint(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = migimae_data[0];
        true_migiusiro_data[0]   = 0x7D - migiusiro_data[0];
        true_hidarimae_data[0]   = 0x7D - hidarimae_data[0];
        true_hidariusiro_data[0] = hidariusiro_data[0];
        
        return 0;
}
int left_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x0C, 0x7C);
        migiusiro.setOutputLimits(0x84, 0xFF);
        hidarimae.setOutputLimits(0x84, 0xFF);
        hidariusiro.setOutputLimits(0x0C, 0x7C);

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

        // 目標値 
        migimae.setSetPoint(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = 0x7D - migimae_data[0];
        true_migiusiro_data[0]   = migiusiro_data[0];
        true_hidarimae_data[0]   = hidarimae_data[0];
        true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
        
        return 0;
}
//斜め右前
int right_front_PID(int RB, int LF) {
        // センサ出力値の最小、最大 
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migiusiro.setOutputLimits(0x0C, 0x7C);
        hidarimae.setOutputLimits(0x0C, 0x7C);

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

        // 目標値 
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        
        // センサ出力 
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        
        // 制御量(計算結果) 
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();

        // 制御量をPWM値に変換 
        true_migiusiro_data[0]   = 0x7D - migiusiro_data[0];
        true_hidarimae_data[0]   = 0x7D - hidarimae_data[0];
        
        return 0;
}
//斜め右後
int right_back_PID(int RF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x84, 0xFF);
        hidariusiro.setOutputLimits(0x84, 0xFF);
        
        // よくわからんやつ 
        migimae.setMode(AUTO_MODE);
        hidariusiro.setMode(AUTO_MODE);
        
        // 目標値 
        migimae.setSetPoint(RF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
        migimae.setProcessValue(abs(migimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();
        
        true_migimae_data[0]      = migimae_data[0];
        true_hidariusiro_data[0]  = hidariusiro_data[0];
       
        return 0;
}
//斜め左前
int left_front_PID(int RF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x0C, 0x7C);
        hidariusiro.setOutputLimits(0x0C, 0x7C);

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

        // 目標値 
        migimae.setSetPoint(RF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = 0x7D - migimae_data[0];
        true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
        
        return 0;
}
//斜め左後
int left_back_PID(int RB, int LF) {
        // センサ出力値の最小、最大 
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migiusiro.setOutputLimits(0x84, 0xFF);
        hidarimae.setOutputLimits(0x84, 0xFF);
        
        // よくわからんやつ 
        migiusiro.setMode(AUTO_MODE);
        hidarimae.setMode(AUTO_MODE);
        
        // 目標値 
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        
        // センサ出力 
        //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        
        // 制御量(計算結果) 
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        
        true_migiusiro_data[0]    = migiusiro_data[0];
        true_hidarimae_data[0]    = hidarimae_data[0];
       
        return 0;
}
int turn_right_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x84, 0xFF);
        migiusiro.setOutputLimits(0x84, 0xFF);
        hidarimae.setOutputLimits(0x0C, 0x7C);
        hidariusiro.setOutputLimits(0x0C, 0x7C);

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

        // 目標値 
        migimae.setSetPoint(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = migimae_data[0];
        true_migiusiro_data[0]   = migiusiro_data[0];
        true_hidarimae_data[0]   = 0x7D - hidarimae_data[0];
        true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
        
        return 0;
}
int turn_left_PID(int RF, int RB, int LF, int LB) {
        // センサ出力値の最小、最大 
        migimae.setInputLimits(-400, 400);
        migiusiro.setInputLimits(-400, 400);
        hidarimae.setInputLimits(-400, 400);
        hidariusiro.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        migimae.setOutputLimits(0x0C, 0x7C);
        migiusiro.setOutputLimits(0x0C, 0x7C);
        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(RF);
        migiusiro.setSetPoint(RB);
        hidarimae.setSetPoint(LF);
        hidariusiro.setSetPoint(LB);
        
        // センサ出力 
        migimae.setProcessValue(abs(migimae_rpm));
        migiusiro.setProcessValue(abs(migiusiro_rpm));
        hidarimae.setProcessValue(abs(hidarimae_rpm));
        hidariusiro.setProcessValue(abs(hidariusiro_rpm));
        
        // 制御量(計算結果) 
        migimae_data[0]      = migimae.compute();
        migiusiro_data[0]    = migiusiro.compute();
        hidarimae_data[0]    = hidarimae.compute();
        hidariusiro_data[0]  = hidariusiro.compute();

        // 制御量をPWM値に変換 
        true_migimae_data[0]     = 0x7D - migimae_data[0];
        true_migiusiro_data[0]   = 0x7D - migiusiro_data[0];
        true_hidarimae_data[0]   = hidarimae_data[0];
        true_hidariusiro_data[0] = hidariusiro_data[0];
        
        return 0;
}
int roller_PID(int front, int back) {
        front_roller.setInputLimits(-2000, 2000);
        back_roller.setInputLimits(-2000, 2000);
        
        front_roller.setOutputLimits(0x84, 0xFF);
        back_roller.setOutputLimits(0x84, 0xFF);
        
        front_roller.setMode(AUTO_MODE);
        back_roller.setMode(AUTO_MODE);
        
        front_roller.setSetPoint(front);
        back_roller.setSetPoint(back);
        
        front_roller.setProcessValue(abs(front_roller_rpm));
        back_roller.setProcessValue(abs(back_roller_rpm));
        
        front_roller_data[0] = front_roller.compute();
        back_roller_data[0]  = back_roller.compute();
        
        return 0;
}
void linetrace() {
    line_state = photo.getc();
}
void ultrasonic() {
        sonic.start();
        wait(0.01);
        //get_dist_cm関数は、計測から得られた距離(cm)を返します。
        distance = sonic.get_dist_cm();
        //pc.printf("%d[cm]\r\n", distance);  
}
int main(void) {
    pc.printf("HelloWorld\n");
    emergency = 0;
    /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
    get_rps.attach_us(&flip, 40000);
    //get_distance.attach_us(&ultrasonic, 100000);
    photo.baud(115200);
    photo.attach(linetrace, Serial::RxIrq);
    side.mode(PullDown);
    
    // setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
    sonic.setTemp(25);
    
    send_data[0] = 0x80;
    i2c.write(0x10, send_data, 1);
    i2c.write(0x12, send_data, 1);
    i2c.write(0x14, send_data, 1);
    i2c.write(0x16, send_data, 1);
    i2c.write(0x20, send_data, 1);
    i2c.write(0x22, send_data, 1);
    i2c.write(0x30, send_data, 1);
    i2c.write(0x40, send_data, 1);
    wait(0.1);
    
    while(1) {
        ultrasonic();
        //pc.printf("%d[cm]\r\n", distance);
        if(side == 1){
            red_side = 1;
            blue_side = 0;
        } else {
            red_side = 0;
            blue_side = 1;
        }
        if(start_button == 1){
            /*
            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);
            */
            cylinder_data[0] = 0x80;
            myled1 = 0;
            i2c.write(0x40, cylinder_data, 1);
        } else {
            /*
            front_PID(270, 270, 270, 270);
            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);
            */
            myled1 = 1;
            cylinder_data[0] = 0x0F;
            i2c.write(0x40, cylinder_data, 1);
        }   
        if(user_switch2 == 0 && user_switch3 == 1){
            loading_data[0] = 0x0C;
            myled2 = 1;
            i2c.write(0x30, loading_data, 1);
        } 
        else if(user_switch3 == 0 && user_switch2 == 1){
            loading_data[0] = 0xFF;
            myled3 = 1;
            i2c.write(0x30, loading_data, 1);
        } else {
            loading_data[0] = 0x80;
            myled2 = 0;
            myled3 = 0;
            i2c.write(0x30, loading_data, 1);
        } 
        
        /*
        loading_data[0] = 0x0C;
        i2c.write(0x30, loading_data, 1);
        if(photo_interrupter.read() == 1) {
            myled4 = 0;
            loading_state = 1;
            loading_data[0] = 0x80;
            i2c.write(0x30, loading_data, 1);            
        }    
        */
        
        /*
        if(user_switch5 == 0){
            roller_flag = 1;
        }
        if(roller_flag == 1) {
            myled5 = 1;
            timer.reset();
            timer.start();
            roller_PID(325, 650);
            i2c.write(0x20, front_roller_data, 1, false);
            i2c.write(0x22, back_roller_data,  1, false);
            wait_us(20); 
            
            if((timer.read() >= 5.0f) && (timer.read() < 5.5f)) {
                cylinder_data[0] = 0x0F;
                i2c.write(0x40, cylinder_data, 1);
            }
            else if((timer.read() >= 5.5f) && (timer.read() < 6.0f)) {
                cylinder_data[0] = 0x80;
                i2c.write(0x40, cylinder_data, 1);
            }
            else if((timer.read() >= 6.0f) && (timer.read() < 6.5f)) {
                cylinder_data[0] = 0x0F;
                i2c.write(0x40, cylinder_data, 1);
            }
            else if((timer.read() >= 6.5f) && (timer.read() < 7.0f)) {
                cylinder_data[0] = 0x80;
                i2c.write(0x40, cylinder_data, 1);
            }      
            else if((timer.read() >= 7.0f) && (timer.read() < 7.5f)) {
                cylinder_data[0] = 0x0F;
                i2c.write(0x40, cylinder_data, 1);
            } else {
                cylinder_data[0] = 0x80;
                i2c.write(0x40, cylinder_data, 1);
            }
            timer.stop();
        } else {
            myled5 = 0;
            front_roller_data[0] = 0x80;
            back_roller_data[0] = 0x80;
            i2c.write(0x20, front_roller_data, 1, false);
            i2c.write(0x22, back_roller_data,  1, false);
        }
        */
        
        /*    
        if(distance < 30 && distance != 0) {
            //front_PID(200, 200, 200, 200);
            //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);
        } else {
            //send_data[0] = 0x80;
            //i2c.write(0x10, send_data, 1, false);
            //i2c.write(0x12, send_data, 1, false);
            //i2c.write(0x14, send_data, 1, false);
            //i2c.write(0x16, send_data, 1, false);
        }
        */
        
        //pc.printf("%d\n", line_state);
        if(line_state == 0){
            pc.printf("真ん中\n");
            front_PID(70, 70, 70, 70);
            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);
        }
        else if(line_state == 1) {
            pc.printf("右\n");
            turn_right_PID(30, 30, 30, 30);
            //right_front_PID(50, 50);
            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);
        }
        else if(line_state == 2) {
            pc.printf("左\n");
            turn_left_PID(30, 30, 30, 30);
            //left_front_PID(50, 50);
            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);
        } 
        else if(line_state == 3) {
            pc.printf("範囲外\n");
            send_data[0] = 0x80;
            i2c.write(0x10, send_data, 1, false);
            i2c.write(0x12, send_data, 1, false);
            i2c.write(0x14, send_data, 1, false);
            i2c.write(0x16, send_data, 1, false);
        }
        
        /*
        front_PID(270, 270, 270, 270);
        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);
        
        back_PID(270, 270, 270, 270);
        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);
        
        right_PID(100, 100, 100, 100);
        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);
        
        left_PID(100, 100, 100, 100);
        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);
        
        //斜め右前
        right_front_PID(270, 270);
        true_migiusiro_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);
        
        //斜め右後
        right_back_PID(270, 270);
        true_migimae_data[0] = 0x80;
        true_hidarimae_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);

        //斜め左前        
        left_front_PID(270, 270);
        true_migimae_data[0] = 0x80;
        true_hidarimae_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);

        //斜め左後        
        left_back_PID(270, 270);
        true_migiusiro_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);

        turn_right_PID(200, 200, 200, 200);
        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);

        turn_left_PID(100, 100, 100, 100);
        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);
        
        roller_PID(1000, 1000);
        i2c.write(0x20, front_roller_data, 1, false);
        i2c.write(0x22, back_roller_data,  1, false);
        wait_us(20);    
        */
        
        /*
        //どんどん加速(逆転)
        for(send_data[0] = 0x81; send_data[0] < 0xF5; send_data[0]++){
            //ice(0x88, send_data);
            //ice(0xA2, send_data);
            
            i2c.write(0xA0, send_data, 1);
            i2c.write(0xA2, send_data, 1);
            i2c.write(0xA4, send_data, 1);
            i2c.write(0xA6, send_data, 1);
            
            i2c.write(0x20, send_data, 1);
            wait(0.1);
        }
        //だんだん減速(逆転)
        for(send_data[0] = 0xF4; send_data[0] > 0x80; send_data[0]--){
            //ice(0x88, send_data);
            //ice(0xA2, send_data);
            
            i2c.write(0xA0, send_data, 1);
            i2c.write(0xA2, send_data, 1);
            i2c.write(0xA4, send_data, 1);
            i2c.write(0xA6, send_data, 1);
            
            i2c.write(0x20, send_data, 1);
            wait(0.1);
        }
        send_data[0] = 0x80;
        i2c.write(0x20, send_data, 1);
        wait(5);
        */
    }
}