File content as of revision 17:1409cdfb6043:

#include "mbed.h"
#include "DriveController.h"
#include "IMU.h"
#include "MDD.h"
#include "MDD2.h"
#include "Mycan.h"
#include "Odometer.h"
#include "PID.h"
#include "RotaryEncoder.h"
#include "SBUS.h"
#include "USS.h"
#include "hardwareConfig.h"
#include "stateLib.h"

elements getRobotVelocity(state);
state getTargetState();
int updateStateNum(int tops_num);
int updateTopsNum(int num);
int changeNextTable(int num);
state updateTopsState(float val, int tops_num);
bool isConvergenceTops(int);
bool isConvergenceSupply(int);
controller getPropoData();
void riseUssTriger();

//x軸補正用 PID
PID pidRobotX(4, 0, 0, 0.01, 0.5, &timer);
float target_x = 0;

//y軸補正用 PID
PID pidRobotY(2, 0, 0, 0.01, 0.5, &timer);
float target_y = 0;

//yow角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
PID pidRobotYow(0.05, 0, 0, 0.01, 0.3, &timer);
float target_yow = 0;

//USS用
PID pidUss(0.025, 0, 0, 0.1, 0.3, &timer);
float target_uss = 0;

//オドメーターの定義
float matrix[3][3] = {{1, 0, 0},
                      {0, 1, 0},
                      {0, 0, 0}
                     };
Odometer odm(matrix, 0.050);

int table_sw[9];
    
int main()
{
    //タイマー３の優先度を最低にする
    NVIC_SetPriority(TIMER3_IRQn, 3);

    //IMUのキャリブレーション
    wait(1);
    imu.performCalibration();
    imu.startAngleComputing();
    
    enc[0].changeDirection();
    //enc[1].changeDirection();
    enc[2].changeDirection();

    float tmp = 0;
    float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
    odm.setupOdometerSensors(encoders, &imu.angle[2]);
    odm.startComputingOdometry(0.01, 0, 0, 0);
    mecanum.imu_yow = &imu.angle[2];
    
    //許容誤差
    pidRobotX.allowable_error = 0.03;
    pidRobotY.allowable_error = 0.1;
    pidRobotYow.allowable_error = 2;
    pidUss.allowable_error = 3;
    
    //PID設定
    float tmp1 = 0;
    pidRobotX.sensor = &odm.x;
    pidRobotX.target = &target_x;
    pidRobotX.start();
    pidRobotY.sensor = &odm.y;
    pidRobotY.target = &target_y;
    pidRobotY.start();
    pidRobotYow.sensor = &imu.angle[2];
    pidRobotYow.target = &target_yow;
    pidRobotYow.start();
    pidUss.sensor = &tmp1;
    pidUss.target = &target_uss;
    pidUss.start();
    
    //マイクロスイッチ
    sw1.mode(PullUp);
    sw2.mode(PullUp);
    
    odm.x = 0;
    timer.start();
    
    while(1)
    {
        riseUssTriger();
        can.read();
        
        //目指すべきロボットの状態を取得
        state tar_state = getTargetState();
        
        int can_angle = 900 - int(tar_state.angle * 10);
        //canに置く
        //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
        can.set(1, 1, tar_state.shoot);
        can.set(1, 2, int(can_angle));
        can.set(1, 3, tar_state.supply);
        
        //送信周期調整
        static double pre_time = 0;
        double now_time = timer.read();
        if(now_time - pre_time >= 0.01)
        {
            if(can.send())
                pre_time = now_time;
        }
        
        //ロボット速度を取得
        elements robot_vel = getRobotVelocity(tar_state);
        float robot_velocity[3] = {robot_vel.x, robot_vel.y, robot_vel.theta};
        
        //ホイール速度計算
        mecanum.setVelG(robot_velocity);
        mecanum.computeWheelVel();
        mecanum.rescaleWheelVel();
        
        //モーターの駆動
        for(int i = 0; i < 4; i++)
            Motor[i].drive(mecanum.wheel_vel[i]);
        
        /*
        pc.printf("%.2f\t", odm.x);
        pc.printf("%.2f\t", odm.y);
        pc.printf("%.2f\t", imu.angle[2]);
        
        int angle_pid = can.get(3, 2);
        float angle_val = can.get(3, 4);
        angle_val = 90 - (angle_val / 10.0);
        
        pc.printf("%d\t", angle_pid);
        pc.printf("%.2f\t", angle_val);
        pc.printf("%d\t", can_angle);
        pc.printf("%.2f\t", tar_state.angle);
        
        pc.printf("%.2f\t", uss[0].distance);
        pc.printf("%.2f\t", uss[1].distance);
        
        pc.printf("%.2f\t", enc[0].rotations);
        pc.printf("%.2f\t", enc[1].rotations);
        pc.printf("%.2f\t", enc[2].rotations);
        */
        
        for(int i = 0; i < 6; i++)
            table_sw[i] = can.get(6, i + 1);
        for(int i = 1; i < 4; i++)
            table_sw[i + 5] = can.get(7, i);
        for(int i = 0; i < 9; i++)
            pc.printf("%d\t", table_sw[i]);
        
        pc.printf("\n");
    }
}
state getTargetState()
{
    static bool start_flag = 0;
    
    int num[2];
    
    /*
    スタートボタン待機
    whileにするとほかの処理がすべて止めるためこれを回避
    */
    if(sw1 == 1)
        start_flag = 1;
    
    //sw1が1度も押されていないとき
    if(!start_flag)
        num[0] = num[1] = odm.x = odm.y = imu.angle[2] = 0;
    else 
    {
        num[0] = updateStateNum(num[1]);
        num[1] = updateTopsNum(num[0]);
    }
    
    //再びスタート待機
    if(num[0] == 0)
        start_flag = 0;
    if(num[1] == 0)
        num[0] = changeNextTable(num[0]);
    
    //states辞典から値を引っ張る
    state a;
    
    table_sw[8] = can.get(7, 3);
    if(table_sw[8] == 0)
    {
        a.x = -1 * state_lib[num[0]][0];
        a.theta = -1 * state_lib[num[0]][2];
    }
    else
    {
        a.x = state_lib[num[0]][0];
        a.theta = state_lib[num[0]][2];
    }
    
    a.y = state_lib[num[0]][1];
    updateTopsState(state_lib[num[0]][3], num[1]);
    
    return a;
}
int updateStateNum(int tops_num)
{
    /*
    状態収束を判定して次のステップに進む
    */
    float t = 0.1;
    static int num = 0;
    //x方向、y方向、yow方向、topsの判定をはっきりしておく
    bool flag_x = 0, flag_y = 0, flag_yow = 0;
    
    if(pidRobotYow.isConvergence(t))
        flag_yow = 1;
    
    if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
    {
        if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
            flag_x = flag_y = 1;
    }
    else if(pidRobotX.isConvergence(t) == 1)
        flag_x = 1;
        
    //超音波で近づくとき
    if(state_lib[num][1] >= 10 || state_lib[num][1] <= -10)
    {
        if(pidUss.isConvergence(t+1) && isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
            flag_y = flag_x = 1;
    }
    else if(pidRobotY.isConvergence(t) == 1)
        flag_y = 1;
    
    //全部が収束してるか
    if(flag_x && flag_y && flag_yow)
        num++;
    
    //振り出しに戻る
    if(num >= LIBNUM)
        num = 0;
    
    return num;
}
int updateTopsNum(int num)
{
    static int tops_num = 0;
    bool flag_tops = 0;
    
    if(isConvergenceTops(tops_num) && isConvergenceSupply(tops_num))
    {
        if(state_lib[num][4] == 1)
            flag_tops = 1;
        else
        {
            if(tops_num == 1)
                flag_tops = 0;
            else flag_tops = 1;
        }
    }
    if(flag_tops)
        tops_num++;
    if(tops_num >= 4)
        tops_num = 0;
    
    return tops_num;
}
int changeNextTable(int num)
{
    for(int i = 0; i < 6; i++)
        table_sw[i] = can.get(6, i + 1);
    for(int i = 1; i < 3; i++)
        table_sw[i + 5] = can.get(7, i);
    
    if(table_sw[3] == 1 && table_sw[4] == 0)
    {
        if(num == 5)
         num = 17;
    }
    else if(table_sw[3] == 0 && table_sw[4] == 1)
    {
        if(num == 1)
            num = 5;
        if(num == 9)
            num = 17;
    }
    else if(table_sw[3] == 1 && table_sw[4] == 1)
    {
        if(num == 1)
            num = 9;
    }
    else 
    {
        if(num == 1)
            num = 18;
    }
    if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
    {
        if(num == 17)
            num = 43;
    }
    if(table_sw[5] == 0)
    {
        if(num == 18)
            num = 22;
    }
    if(table_sw[6] == 0)
    {
        if(num == 22)
            num = 26;
    }
    if(table_sw[7] == 0)
    {
        if(num == 26)
            num = 30;
    }
    if(table_sw[3] == 0 || table_sw[4] == 0 || table_sw[5] == 0 || table_sw[6] == 0 || table_sw[7] == 0)
    {
        if(table_sw[0] == 0)
        {
            if(num == 30)
                num = 34;
        }
        if(table_sw[1] == 0)
        {
            if(num == 34)
                num = 38;
        }
        if(table_sw[2] == 0)
        {
            if(num == 38)
                num = 42;
        }
        if(table_sw[1] == 0 || table_sw[2] == 0)
        {
            if(num == 42)
                num = 43;
        }
    }
    
    return num;
}
state updateTopsState(float val, int tops_num)
{
    state a;
    if(val == 1)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = A_1[tops_num][i];
    }
    else if(val == 2)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = A_2[tops_num][i];
    }
    else if(val == 3)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = A_3[tops_num][i];
    }
    else if(val == 4)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = B_1[tops_num][i];
    }
    else if(val == 5)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = B_2[tops_num][i];
    }
    else if(val == 6)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = C_1[tops_num][i];
    }
    else if(val == 7)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = C_2[tops_num][i];
    }
    else if(val == 8)
    {
        for(int i = 0; i < 3; i++)
            state_tops[tops_num][i] = C_3[tops_num][i];
    }
    else if(val == 9)
    {
        state_tops[0][0] = 0;
        state_tops[0][1] = 90;
        state_tops[0][2] = 1;
    }
    else
    {
        state_tops[0][0] = 0;
        state_tops[0][1] = 90;
        state_tops[0][2] = 0;
    }
    a.shoot = state_tops[tops_num][0];
    a.angle = state_tops[tops_num][1];
    a.supply = state_tops[tops_num][2];
    
    return a;
}
elements getRobotVelocity(state a)
{
    elements vel;
    
    //yow角調整処理
    *pidRobotYow.target = a.theta;
    vel.theta = pidRobotYow.output;
    
    float right_dis = uss[0].distance;
    float left_dis = uss[1].distance;
    float error = right_dis - left_dis;
    if(right_dis < left_dis)
        *pidUss.sensor = right_dis;
    else *pidUss.sensor = left_dis;
    
    if(a.x >= 10 || a.x <= -10 || a.y >= 10 || a.y <= -10)
    {
        if(a.x >= 10)
        {
            if(error > 100)
                vel.y = 0.2;
            else if(error < -100)
                vel.y = -0.2;
            else vel.y = 0;
            
            *pidUss.target = a.x - 10;
            vel.x = -pidUss.output;
        }
        else if(a.x <= -10)
        {
            if(error > 100)
                vel.y = -0.2;
            else if(error < -100)
                vel.y = 0.2;
            else vel.y = 0;
            
            *pidUss.target = -a.x - 10;
            vel.x = pidUss.output;
        }
        else if(a.y >= 10)
        {
            if(error > 100)
                vel.x = -0.2;
            else if(error < -100)
                vel.x = 0.2;
            else vel.x = 0;
            
            *pidUss.target = a.y - 10;
            vel.y = -pidUss.output;
        }
        else if(a.y <= -10)
        {
            if(error > 100)
                vel.x = 0.2;
            else if(error < -100)
                vel.x = -0.2;
            else vel.x = 0;
            
            *pidUss.target = -a.y - 10;
            vel.y = pidUss.output;
        }
    }
    else 
    {
        *pidUss.target = 0; 
        *pidRobotX.target = a.x;
        *pidRobotY.target = a.y;
         vel.x = pidRobotX.output;
        vel.y = pidRobotY.output;
    }
    return vel;
}
bool isConvergenceTops(int tops_num)
{
    int velocity_pid = can.get(3, 1);
    int angle_pid    = can.get(3, 2);
    int velocity_val = can.get(3, 3);
    float angle_val    = can.get(3, 4);
    angle_val = 90 - (angle_val / 10.0);
    
    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_tops[tops_num][0] && angle_val == state_tops[tops_num][1])
        return 1;
    else return 0;
}
bool isConvergenceSupply(int num)
{
    int is_supply_done = can.get(3, 5);
        
    if(state_tops[num][2] == 1)
    {
        if(is_supply_done == 1)
            return 1;
        else return 0;
    }else return 1;
}
controller getPropoData()
{
    float dead_zone = 0.05;
    controller propo;
    sbus.isFailSafe();
    
    //propo直接コントロール
    if(sbus.isFailSafe())
    {
        propo.LX = propo.LY = propo.RX = propo.RY = 0;
        propo.H = propo.A = propo.D = propo.F = propo.G = 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.H = sbus.getSwitchVal(0);
        propo.C = sbus.getSwitchVal(1);
        propo.E = sbus.getSwitchVal(2);
        propo.F = sbus.getSwitchVal(3);
        propo.G = sbus.getSwitchVal(4);
    }
    
    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 riseUssTriger()
{
    static double start_time = timer.read();
    static bool flag = 1;
    double now_time = timer.read();
    if(now_time - start_time > 0.025)
    {
        if(flag)
        {
            uss[0].riseTrigerEvent();
            flag = 0;
        }
        else
        {
            uss[1].riseTrigerEvent();
            flag = 1;
        }
        start_time = now_time;
    }
}