File content as of revision 1:0f33a68d1390:

#include "mbed.h"
#include "stateLib.h"
#include "hardwareConfig.h"

void mainLoop();
controller getPropoData();
void canFnc();
void readLineData();
void readSwitchData();
void encCorrection();
state getTargetState();
int updateStateNum(int, int);
int updateTopsNum(int, int);
void updateSupplyTarget(int, int);
int updateRouteNum(int);
void imuCalibration();
void reset_System();
int changeNextNumber(int);
elements getRobotVelocity(state);
int safety, now_state, now_tops, now_spline_num, now_route_num, read_sw_data[2];
float robot_velocity[3], lift_val, wide_val, supply_val, read_line_data[2], line_error;
bool S_sw, B_sw[4], T_sw[4], zone, match, reset_sw = false, do_roop = false;

int main() 
{
    for(int i=0; i<4; i++)
        sw[i].mode(PullUp);
    
    //IMUのキャリブレーション
    imuCalibration();
    
    float tmp = 0;
    float *encoders[3] = {&odm_enc[0].rotations, &odm_enc[1].rotations, &tmp};
    odm.setupOdometerSensors(encoders, &imu.angle[2]);
    odm.startComputingOdometry(0.01, 0, 0, 0);
    omni.imu_yaw = &imu.angle[2];
    
    for(int i = 0; i < SPLINE_NUM; i++)
    {
        for(int j = 0; j < ROUTE_NUM; j++) {
            spline[i][j].odm[0] = &odm.x;
            spline[i][j].odm[1] = &odm.y;
            spline[i][j].start();
        }
    }
    
    //許容誤差
    //pidRobotX.allowable_error = 0.08;   //[m]
    //pidRobotY.allowable_error = 0.08;   //[m]
    //pidRobotYaw.allowable_error = 4;    //[°]
    //pidLift.allowable_error = 0.05;     //×0.3180[m] = 15.9[mm]
    //pidWide.allowable_error = 0.075;    //×0.1000[m] =  7.5[mm]
    //pidSupply.allowable_error = 0.075;  //×0.0625[m] =  4.7[mm]
    
    //PID設定
    float target_x = 0, target_y = 0, target_yaw = 0, target_lift = 0, target_wide = 0, target_supply = 0;
    pidRobotX.sensor = &odm.x;
    pidRobotX.target = &target_x;
    pidRobotX.start();
    pidRobotY.sensor = &odm.y;
    pidRobotY.target = &target_y;
    pidRobotY.start();
    pidRobotYaw.sensor = &imu.angle[2];
    pidRobotYaw.target = &target_yaw;
    pidRobotYaw.start();
    pidLift.sensor = &enc[4];
    pidLift.target = &target_lift;
    pidLift.start();
    /*
    pidVelLift.sensor = &tmp_val;
    pidVelLift.target = &tmp_val;
    pidVelLift.start();
    */
    pidWide.sensor = &enc[5];
    pidWide.target = &target_wide;
    pidWide.start();
    pidSupply.sensor = &enc[6];
    pidSupply.target = &target_supply;
    pidSupply.start();
    
    ticker.attach(&mainLoop, 0.00725);
    
    while(1) 
    {
        pc.printf("%.3f\t", odm.x);
        pc.printf("%.3f\t", odm.y);
        pc.printf("%.3f\t", imu.angle[2]);
        
        pc.printf("%.3f\t", lift_val);
        pc.printf("%.3f\t", *pidLift.target);
        pc.printf("%.3f\t", enc[4]);
        pc.printf("%.3f\t", supply_val);
        pc.printf("%.3f\t", *pidSupply.target);
        pc.printf("%.3f\t", enc[6]);
        
        bool _sw[4];
        for(int i = 0; i < 4; i++) 
            _sw[i] = sw[i];
        pc.printf("%d\t", (int)(_sw[0] + _sw[1] + _sw[2] + _sw[3]));
        
        pc.printf("%d\t", read_sw_data[0]);
        pc.printf("%d\t", read_sw_data[1]);
        pc.printf("%.3f\t", read_line_data[0]);
        pc.printf("%.3f\t", read_line_data[1]);
        pc.printf("%.3f\t", robot_velocity[0]);
        pc.printf("%.3f\t", robot_velocity[1]);
        pc.printf("%.3f\t", *pidRobotX.target);
        pc.printf("%.3f\t", *pidRobotY.target);
        pc.printf("%d\t", now_state);
        pc.printf("%d\t", now_tops);
        /*
        pc.printf("%d\t", S_sw);
        pc.printf("%d\t", B_sw[0]);
        pc.printf("%d\t", B_sw[1]);
        pc.printf("%d\t", B_sw[2]);
        pc.printf("%d\t", B_sw[3]);
        */
        pc.printf("\n");
    }
}

void mainLoop()
{
    reset_System();
    canFnc();
    controller cmd = getPropoData();
    
    if(cmd.fail_safe == false)
    {
        safety = 2;
        state tar_state = getTargetState();
        
        elements robot_vel = getRobotVelocity(tar_state);
        robot_velocity[0] = robot_vel.x;
        robot_velocity[1] = robot_vel.y;
        robot_velocity[2] = robot_vel.yaw;
    }
    else 
    {
        robot_velocity[0] = cmd.LX;
        robot_velocity[1] = cmd.LY;
        robot_velocity[2] = cmd.RX;
        if(cmd.A == 0)
            *pidLift.target = 0.095;
        else *pidLift.target = 5.13;
        *pidWide.target = 0.00;
        *pidSupply.target = 0.0;
        //lift_val = cmd.RY;
        //wide_val = cmd.RY;
        //supply_val = cmd.RY;
        safety = cmd.F;
        if(cmd.H == 2)
             servo_mode = 1;
        else servo_mode = 0;
        
        if(cmd.D == 2) 
            servo_right = servo_left = 1;
        else servo_right = servo_left = 0;
    }
    
    //ホイール速度計算
    omni.setVelG(robot_velocity);
    omni.computeWheelVel();
    omni.rescaleWheelVel();
}

controller getPropoData()
{
    float dead_zone = 0.05;
    controller propo;
    sbus.isFailSafe();
    
    //propo電源off
    if(sbus.isFailSafe())
    {
        propo.LX = propo.LY = propo.RX = propo.RY = 0;
        propo.A = propo.D = propo.F = propo.H = propo.fail_safe = 0;
    }
    else
    {
        propo.LX = sbus.getStickVal(0) / 255.0;
        propo.LY = sbus.getStickVal(1) / 255.0;
        propo.RX = sbus.getStickVal(2) / 255.0;
        propo.RY = sbus.getStickVal(3) / 255.0;
        propo.A = sbus.getSwitchVal(0);
        propo.D = sbus.getSwitchVal(1);
        propo.F = sbus.getSwitchVal(2);
        propo.H = sbus.getSwitchVal(3);
        propo.fail_safe = true;
    }
    
    if(propo.RX < dead_zone && propo.RX > -dead_zone)  propo.RX = 0;
    if(propo.RY < dead_zone && propo.RY > -dead_zone)  propo.RY = 0;
    if(propo.LX < dead_zone && propo.LX > -dead_zone)  propo.LX = 0;
    if(propo.LY < dead_zone && propo.LY > -dead_zone)  propo.LY = 0;
    return propo;
}

void canFnc()
{
    if(now_state == 0)
        readSwitchData();
    
    can.readF();
    enc[4] = -can.get(4, 0);
    enc[5] = -can.get(4, 1);
    can.readF();
    enc[6] = can.get(5, 0);
    enc[7] = can.get(5, 1);
    
    lift_val = pidLift.output;
    wide_val = pidWide.output;
    supply_val = pidSupply.output;
    
    static short int calibration[3] = {0, 0, 0};
    float calibrate_output_a[3] = {-0.75, -0.45, 0.85};   //キャリブレーション, ON前出力値
    float calibrate_output_b[3] = {0.13, 0.2, -0.2};   //キャリブレーション, ON後出力値
    bool _sw[3] = {sw[1], sw[2], sw[3]};
    
    if(calibration[0] != 2 || calibration[2] != 2) {
        servo_mode = 0;
        for(int i = 0; i < 4; i++) 
        {
            if(!_sw[i] && calibration[i] == 0)
                can.setI(1, i+4, (short int)(calibrate_output_a[i] * 200));
            
            else if(_sw[i] && calibration[i] == 0) {
                can.setI(1, i+4, 0);
                calibration[i] = 1;
            }
            
            else if(_sw[i] && calibration[i] == 1)
                can.setI(1, i+4, (short int)(calibrate_output_b[i] * 200));
            
            else if(!_sw[i] && calibration[i] == 1) {
                can.setI(1, i+4, 0);
                calibration[i] = 2;
            }
        }
    } else {
        if(safety == 2) {
            for(int i = 0; i < 4; i++)
                can.setI(1, i, (short int)(omni.wheel_vel[i] * 200));
            
            if((odm.y > 4.35 && odm.y < 5.20)
            || (odm.y > 6.35 && odm.y < 7.20)) 
                can.setI(1, 4, 0);
            else can.setI(1, 4, (short int)(lift_val * 200));
            //can.setI(1, 5, (short int)(wide_val * 200));
            can.setI(1, 5, 0);
            can.setI(1, 6, (short int)(supply_val * 200 * -1));
        } else {
            for(int i = 0; i < 7; i++)
                can.setI(1, i, 0);
        }
    }
    LED[0] = can.send();
    
    encCorrection();   //スイッチを使ってenc値補正
}

void readSwitchData() 
{
    int residue_data;
    
    can.readI();
    read_sw_data[0] = can.get(2, 0);
    read_sw_data[1] = can.get(2, 1);
    read_line_data[1] = 0.5 * can.get(2, 2);
    read_line_data[0] = 0.5 * can.get(2, 3);
    
    residue_data = read_sw_data[0];
    
    if(residue_data >= 64) {
        residue_data -= 64;
        S_sw = 1;
    } else S_sw = 0;
    
    if(residue_data >= 32) {
        residue_data -= 32;
        B_sw[0] = 1;
    } else B_sw[0] = 0;
    
    if(residue_data >= 16) {
        residue_data -= 16;
        B_sw[1] = 1;
    } else B_sw[1] = 0;
    
    if(residue_data >= 8) {
        residue_data -= 8;
        B_sw[2] = 1;
    } else B_sw[2] = 0;
    
    if(residue_data >= 4) {
        residue_data -= 4;
        B_sw[3] = 1;
    } else B_sw[3] = 0;
    
    if(residue_data >= 2) {
        residue_data -= 2;
        T_sw[0] = 1;
    } else T_sw[0] = 0;
    
    if(residue_data >= 1) {
        residue_data -= 1;
        T_sw[1] = 1;
    } else T_sw[1] = 0;
    
    residue_data = read_sw_data[1];
    
    if(residue_data >= 8) {
        residue_data -= 8;
        T_sw[2] = 1;
    } else T_sw[2] = 0;
    
    if(residue_data >= 4) {
        residue_data -= 4;
        T_sw[3] = 1;
    } else T_sw[3] = 0;
    
    if(residue_data >= 2) {
        residue_data -= 2;
        zone = 1;
    } else zone = 0;
    
    if(residue_data >= 1) {
        residue_data -= 1;
        match = 1;
    } else match = 0;
}

void encCorrection()    //swを使ってencの値を補正
{
    bool _sw[3] = {sw[1], sw[2], sw[3]};
    static float a_val[3];
    float b_val[3] = {-0.040, -0.030, -0.035};
    for(int i=0; i<3; i++) {
        if(_sw[i]) {
            a_val[i] = enc[i+4] - b_val[i];
            enc[i+4] = b_val[i];
        }
        else if(!_sw[i])
            enc[i+4] -= a_val[i];
    }
}

state getTargetState()
{
    static bool start_flag = false;
    int state_num;
    int tops_num;
    int route_num;
    short int use_spline;
    static short int max_num, r_point, laundry, tops_state;
    float error_x, error_y;
    //緊急座標調整の設定
    if(zone) {
        error_x = 0.0;
        error_y = 0.0;
    } else {
        error_x = 0.0;
        error_y = 0.0;
    }
    
    bool start_sw = sw[0];
    
    if(start_sw && !do_roop) 
        start_flag = true;
    
    if(!start_flag) {
        state_num = now_state = tops_num = now_tops = route_num = now_route_num = odm.x = odm.y = imu.angle[2] = 0;
        
        for(int i = 0; i < SPLINE_NUM; i++) {
            for(int j = 0; j < ROUTE_NUM; j++) 
                spline[i][j].now_count = 0;
        }
    }
    else {
        tops_num = now_tops = updateTopsNum(laundry, tops_state);
        state_num = now_state = updateStateNum(max_num, r_point);
        do_roop = true;
    }
    
    if(state_num == 0) {
        start_flag = 0;
        //IMUキャリブレーションのタイミング
        if(do_roop) 
            reset_sw = true;
        else reset_sw = false;
    }
    
    state tar;
    
    bool tops_sw = false;   //スイッチングで動作確認をするための変数, ONで動作確認, OFFで通常動作
    if(S_sw) {
        if((B_sw[0] && T_sw[0]) || (B_sw[1] && T_sw[1]) || (B_sw[2] && T_sw[2]) || (B_sw[3] && T_sw[3])) {
            if(B_sw[0] && T_sw[0]) {
                for(int i=0; i<6; i++)
                    state_lib[i] = Test_lib[state_num][i];
                max_num = TEST_NUM;
                r_point = 0;
            }
            else if(B_sw[1] && T_sw[1]) {
                state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0;
                state_lib[3] = -1;
                state_lib[5] = 2;
                if(match) 
                    tar.lift = 4.20;
                else tar.lift = 0.0;
                tar.wide = tar.supply = 0.0;
                tops_sw = true;
                max_num = r_point = 0;
            }
            else if(B_sw[2] && T_sw[2]) {
                state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0;
                state_lib[3] = -1;
                state_lib[5] = 2;
                if(match) 
                    tar.wide = 2.35;
                else tar.wide = 0.0;
                tar.lift = tar.supply = 0.0;
                tops_sw = true;
                max_num = r_point = 0;
            }
            else if(B_sw[3] && T_sw[3]) {
                state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0;
                state_lib[3] = -1;
                state_lib[5] = 2;
                if(match) 
                    tar.supply = 3.20;
                else tar.supply = 0.0;
                tar.lift = tar.wide = 0.0;
                tops_sw = true;
                max_num = r_point = 0;
            }
        } else {
            if(!match) {
                if(B_sw[0] || B_sw[1] || B_sw[2]) {
                    for(int i=0; i<6; i++)
                        state_lib[i] = SBQ_lib[state_num][i];
                    if(state_num > 2 && state_num < 6) {
                        state_lib[0] += error_x;
                        state_lib[1] += error_y;
                        if(state_lib[1] == S_y_2 || state_lib[1] == B_y_3)
                            state_lib[1] += line_error;
                    }
                    max_num = SBQ_NUM;
                    if(state_num < max_num - 2)
                        r_point = SBQ_lib[state_num + 2][5];
                } else {
                    for(int i=0; i<6; i++)
                        state_lib[i] = SQ_lib[state_num][i];
                    if(state_num > 2 && state_num < 6) {
                        state_lib[0] += error_x;
                        state_lib[1] += error_y;
                        if(state_lib[1] == S_y_2)
                            state_lib[1] += line_error;
                    }
                    max_num = SQ_NUM;
                    if(state_num < max_num - 2)
                        r_point = SQ_lib[state_num + 2][5];
                }
            }
            else {
                for(int i=0; i<6; i++)
                    state_lib[i] = SF_lib[state_num][i];
                if(state_num > 2 && state_num < 18) {
                    state_lib[0] += error_x;
                    state_lib[1] += error_y;
                    if(state_lib[1] == S_y_2 || state_lib[1] == S_y_3 || state_lib[1] == S_y_4)
                        state_lib[1] += line_error;
                }
                max_num = SF_NUM;
                if(state_num < max_num - 2)
                    r_point = SF_lib[state_num + 2][5];
            }
        }
    }
    else if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3] || T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) {
        if(!match) {
            if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) {
                for(int i=0; i<6; i++)
                    state_lib[i] = BQ_lib[state_num][i];
                if(state_num > 2 && state_num < 16) {
                    state_lib[0] += error_x;
                    state_lib[1] += error_y;
                    if(state_lib[1] == B_y_3)
                        state_lib[1] += line_error;
                }
                max_num = BQ_NUM;
                if(state_num < max_num - 2)
                    r_point = BQ_lib[state_num + 2][5];
            }
            else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) {
                for(int i=0; i<6; i++)
                    state_lib[i] = TQ_lib[state_num][i];
                if(state_num > 2 && state_num < 16) {
                    state_lib[0] += error_x;
                    state_lib[1] += error_y;
                    if(state_lib[1] == T_y_2)
                        state_lib[1] += line_error;
                }
                max_num = TQ_NUM;
                if(state_num < max_num - 2)
                    r_point = TQ_lib[state_num + 2][5];
            }
        }
        else if(match) {
            if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) {
                for(int i=0; i<6; i++)
                    state_lib[i] = BF_lib[state_num][i];
                if(state_num > 2 && state_num < 21) {
                    state_lib[0] += error_x;
                    state_lib[1] += error_y;
                    if(state_lib[1] == B_y_3)
                        state_lib[1] += line_error;
                }
                max_num = BF_NUM;
                if(state_num < max_num - 2)
                    r_point = BF_lib[state_num + 2][5];
            }
            else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) {
                for(int i=0; i<6; i++)
                    state_lib[i] = TF_lib[state_num][i];
                if(state_num > 2 && state_num < 21) {
                    state_lib[0] += error_x;
                    state_lib[1] += error_y;
                    if(state_lib[1] == T_y_2)
                        state_lib[1] += line_error;
                }
                max_num = TF_NUM;
                if(state_num < max_num - 2)
                    r_point = TF_lib[state_num + 2][5];
            }
        }
    } else {
        state_lib[0] = state_lib[1] = state_lib[2] = state_lib[4] = 0;
        state_lib[3] = -1;
        state_lib[5] = 2;
        tar.lift = tar.wide = tar.supply = 0.0;
        tops_sw = true;
        max_num = r_point = 0;
    }
    
    use_spline = state_lib[3];
    laundry = state_lib[4];
    tops_state = state_lib[5];
    
    //直線距離の長いタイミングだけ最大出力を上げる
    if(state_num == 1 || state_num == 2 || state_num >= max_num - 3 || use_spline != -1) {
        pidRobotX.setParameter(0.95, 0, 0.085, PID_PERIOD, 0.40);
        pidRobotY.setParameter(0.95, 0, 0.085, PID_PERIOD, 0.40);
        pidRobotX.allowable_error = 0.10;
        pidRobotX.allowable_error = 0.10;
    } else {
        pidRobotX.setParameter(2.00, 0, 0.230, PID_PERIOD, 0.30);
        pidRobotY.setParameter(2.00, 0, 0.230, PID_PERIOD, 0.30);
        pidRobotX.allowable_error = 0.055;
        pidRobotX.allowable_error = 0.055;
    }
    
    tops_lib[0][0] = para_lib[0].lift;
    tops_lib[1][0] = tops_lib[2][0] = 0.095;
    tops_lib[3][0] = tops_lib[4][0] = para_lib[laundry].lift;
    
    for(int i = 0; i < TOPSNUM; i++) 
        tops_lib[i][1] = para_lib[laundry].wide;
    //tops_lib[3][1] = tops_lib[4][1] += 0.23;
    
    if(laundry == 5 || laundry == 6) {
        if(tops_state == 1)
            tops_lib[4][3] = 0;
        else if(tops_state == 0)
            tops_lib[4][3] = 1;
        tops_lib[4][4] = 0;
    } else if(laundry == 3) {
        if(SF_lib[state_num - 1][4] == Clothespin || SF_lib[state_num + 1][4] == Clothespin)
            tops_lib[4][3] = 1;
        else tops_lib[4][3] = 0;
        tops_lib[4][4] = 1;
    } else if(laundry == 4) {
        tops_lib[4][3] = 1;
        tops_lib[4][4] = 1;
    } else {
        tops_lib[4][3] = 0;
        tops_lib[4][4] = 1;
    }
    
    short int change_zone = 0;
    if(zone)
        change_zone = 1;
    else if(!zone)
        change_zone = -1;
    
    if(use_spline >= 0) {
        route_num = now_route_num = updateRouteNum(use_spline);
        tar.x = change_zone * (spline[use_spline][route_num].giveTarget[0]);
        tar.y = spline[use_spline][route_num].giveTarget[1];
    } else {
        tar.x = change_zone * state_lib[0];
        tar.y = state_lib[1];
    }
    tar.theta = state_lib[2];
    if(!tops_sw) {
        if(!start_flag && !do_roop)
            tar.lift = 4.2;
        else tar.lift = tops_lib[tops_num][0];
        tar.wide = tops_lib[tops_num][1];
        tar.supply = tops_lib[tops_num][2];
    } else {
        tar.lift = para_lib[0].lift;
        tar.wide = para_lib[0].wide;
        tar.supply = supply_lib[0];
    }
    tar.hand = tops_lib[tops_num][3];
    tar.hand_mode = tops_lib[tops_num][4];
    tar.now_spline = use_spline;
    tar.now_route = route_num;
    
    return tar;
}

bool is_release = 0;
bool convergence_tops = 0;

int updateStateNum(int lib_num, int release_point) 
{
    float t = 0.7, plus_t;
    
    static int num;
    static bool before_count = 0;
    float tape_posi = 0.0;
    bool flag_x = 0, flag_y = 0, flag_yaw = 0, flag_tops = 0;
    
    if(S_sw && num > 1 && num < 12)
        plus_t = 1.0;
    else if(num == lib_num - 2)
        plus_t = -0.5;
    else plus_t = 0.0;
    
    if(pidRobotX.isConvergence(t + plus_t))
        flag_x = 1;
    
    if(pidRobotY.isConvergence(t + plus_t))
        flag_y = 1;
    
    if(pidRobotYaw.isConvergence(t))
        flag_yaw = 1;
    
    LED[1] = flag_x;
    LED[2] = flag_y;
    LED[3] = flag_yaw;
    
    if(state_lib[5] == 1 || state_lib[4] == 4 || state_lib[4] == 5 || state_lib[4] == 6) {
        if(convergence_tops)
            flag_tops = 1;
    }
    else if(S_sw && (B_sw[0] || B_sw[1] || B_sw[2]) && !match && (lib_num > 4 && lib_num < 7)) {
        if(convergence_tops)
            flag_tops = 1;
    }
    else {
        if(num < lib_num - 2 && release_point == 1) {
            if(is_release)
                flag_tops = 1;
        }
        else if(!before_count)
            flag_tops = 1;
    }
    
    if(flag_x && flag_y && flag_yaw && flag_tops) {
        num++;
        before_count = 1;
        //実際のフィールドのラインの誤差
        if(S_sw)
            tape_posi = 0.0;
        else tape_posi = 0.0;
        if(state_lib[1] == T_y_1 || state_lib[1] == B_y_1 || state_lib[1] == S_y_1) {
            //Y軸targetがライン上の時, ラインセンサで誤差を検出
            state tar;
            if(read_line_data[1] == 0.0)
                line_error = 0.0; 
            else if(read_line_data[1] > 0.0) 
                line_error = ((read_line_data[1] - 8.5) * -0.012) - tape_posi + (tar.y - odm.y);
        }
    } else before_count = 0;
    
    if(num >= lib_num) 
        num = 0;
    
    num = changeNextNumber(num);
    
    return num;
}

int updateTopsNum(int _laundry, int _tops)
{
    float t = 0.35;
    
    static int num = 0;
    bool flag_lift = 0, flag_wide = 0, flag_supply = 0, flag_hand = 0;
    static bool last_hand = 0;
    static float start_time;
    
    updateSupplyTarget(num, _laundry);
    
    if(pidLift.isConvergence(t)) 
        flag_lift = 1;
    else flag_lift = 0;
    /*
    if(pidWide.isConvergence(t))
        flag_wide = 1;
    else flag_wide = 0;*/
    flag_wide = 1;
    
    if(pidSupply.isConvergence(t))
        flag_supply = 1;
    else flag_supply = 0;
    
    if(last_hand != (bool)tops_lib[num][3]) {
        float plus_t;
        
        if(_laundry == 3 || _laundry == 4 || _laundry == 5)
            plus_t = 0.35;
        else {
            if(last_hand == 0)
                plus_t = 0.0;
            else if(last_hand == 1)
                plus_t = 0.75;
        }
        
        if(timer.read() - start_time > t + plus_t) {
            flag_hand = 1;
            last_hand = (bool)tops_lib[num][3];
        }
        else flag_hand = 0;
    } else {
        flag_hand = 1;
        start_time = timer.read();
        last_hand = (bool)tops_lib[num][3];
    }
    
    if(flag_lift && flag_wide && flag_supply && flag_hand)
    {
        if(num == 3) {
            is_release = 1;
            if(_tops == 1) {
                num++;
                is_release = 0;
            }
        }
        
        else if(num == 4) {
            convergence_tops = 1;
            if(_laundry == 1) {
                if(odm.y < 2.45)
                    num++;
            }
            else if(_laundry == 2) {
                if(odm.y < 4.30)
                    num++;
            }
            else if(_laundry == 3) { 
                if(odm.y < 6.30 && _tops == 0)
                    num++;
            }
        } else {
            num++;
            convergence_tops = 0;
        }
    }
    else convergence_tops = 0;
    
    if(_laundry == 6 && _tops == 2)
        convergence_tops = 1;
    
    if(num >= TOPSNUM)
        num = 0;
    if(_tops == 2)
        num = 0;
    
    return num;
}

void updateSupplyTarget(int tops_num, int _laundry)
{
    static int num = 0;
    static int pre_tops_num = 0;
    
    if(pre_tops_num == TOPSNUM - 1 && tops_num == 0) 
        num++;
    
    if(_laundry == 1) {
        if(num >= 4)
        num = 0;
    }
    else if(_laundry == 2) {
        if(num >= 4)
        num = 0;
    }
    else if(_laundry == 3) {
        if(B_sw[0] || B_sw[1] || B_sw[2]) {
            if(num >= 4)
                num = 0;
        }
        else if(num >= 1) 
            num = 0;
    }
    else num = 0;
    
    pre_tops_num = tops_num;
    
    tops_lib[0][2] = tops_lib[1][2] = tops_lib[2][2] = supply_lib[num];
    
    if(enc[4] > 0.5) {
        if(num >= 3 || S_sw)
            tops_lib[3][2] = tops_lib[4][2] = supply_lib[0];
        else tops_lib[3][2] = tops_lib[4][2] = supply_lib[num + 1];
    } else {
        tops_lib[3][2] = tops_lib[4][2] = supply_lib[num];
    }
}

int updateRouteNum(int spline_num) 
{
    static int num = 0;
    static int last_spline_num = 0;
    
    if(last_spline_num != spline_num)
        num = 0;
    
    if(spline[spline_num][num].now_count >= spline[spline_num][num].line - 1)
        num++;
    
    if(num >= ROUTE_NUM)
        num = ROUTE_NUM - 1;
    
    last_spline_num = spline_num;
    
    return num;
}

void imuCalibration() 
{
    LED[0] = 0;
    wait(1);
    imu.performCalibration();
    imu.startAngleComputing();
    LED[0] = 1;
}

void reset_System()
{
    bool caliblation_sw = sw[0];
    static bool last_sw = false;
    if(!caliblation_sw && last_sw) 
        NVIC_SystemReset();
    else if(caliblation_sw && reset_sw) 
        last_sw = true;
}

int changeNextNumber(int num) 
{
    if(!S_sw) {
        if(!match) {
            if(B_sw[0] || B_sw[1] || B_sw[2]) {
                if(!B_sw[0] && num > 1 && num < 6) {
                    num = 6;
                }
                if(!B_sw[1] && num > 5 && num < 11) {
                    num = 11;
                }
                if(!B_sw[2] && num > 10 && num < 16) {
                    num = 16;
                }
            }
            else if(T_sw[0] || T_sw[1] || T_sw[2]) {
                if(!T_sw[0] && num > 1 && num < 6) {
                    num = 6;
                }
                if(!T_sw[1] && num > 5 && num < 11) {
                    num = 11;
                }
                if(!T_sw[2] && num > 10 && num < 16) {
                    num = 16;
                }
            }
        }
        else if(match) {
            if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) {
                if(!B_sw[0] && num > 1 && num < 6) {
                    num = 6;
                }
                if(!B_sw[1] && num > 5 && num < 11) {
                    num = 11;
                }
                if(!B_sw[2] && num > 10 && num < 16) {
                    num = 16;
                }
                if(!B_sw[3] && num > 15 && num < 21) {
                    num = 21;
                }
            }
            else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) {
                if(!T_sw[0] && num > 1 && num < 6) {
                    num = 6;
                }
                if(!T_sw[1] && num > 5 && num < 11) {
                    num = 11;
                }
                if(!T_sw[2] && num > 10 && num < 16) {
                    num = 16;
                }
                if(!T_sw[3] && num > 15 && num < 21) {
                    num = 21;
                }
            }
        }
    } else {
        if(!match) {
            if(B_sw[0] || B_sw[1] || B_sw[2]) {
                if(!B_sw[0] && num > 7 && num < 12) {
                    num = 12;
                }
                if(!B_sw[1] && num > 11 && num < 17) {
                    num = 17;
                }
                if(!B_sw[2] && num > 16 && num < 22) {
                    num = 22;
                }
            }
        }
    }
    
    return num;
}

elements getRobotVelocity(state tar) 
{
    elements vel;
    
    *pidRobotYaw.target = tar.theta;
    vel.yaw = pidRobotYaw.output;
    
    *pidRobotX.target = tar.x;
    *pidRobotY.target = tar.y;
    *pidLift.target = tar.lift;
    *pidWide.target = tar.wide;
    *pidSupply.target = tar.supply;
    
    //シーツを引っ張る際, hand機構を片方だけ閉じさせる処理
    if(tar.hand_mode == false) {
        if(zone) {
            servo_right = 0;
            servo_left = tar.hand;
        } else {
            servo_right = tar.hand;
            servo_left = 0;
        }
    } else servo_right = servo_left = tar.hand;
    servo_mode = tar.hand_mode;
    
    if(tar.now_spline >= 0) {
        spline[tar.now_spline][tar.now_route].rescaleVel(pidRobotX.output, pidRobotY.output);   //PIDを使うと起こる問題を解決する処理
        vel.x = spline[tar.now_spline][tar.now_route].velocity[0];
        vel.y = spline[tar.now_spline][tar.now_route].velocity[1];
    } else {
        vel.x = pidRobotX.output;
        vel.y = pidRobotY.output;
    }
    
    return vel;
}