File content as of revision 21:4912835805a8:

#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 changeNextTable(int);
int updateTopsNum(bool);
bool isConvergenceCatapult(int);
bool isConvergenceSupply(int);
controller getPropoData();
void riseUssTriger();
void reset_System();
void discoveryTable();

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

//y軸補正用 PID
PID pidRobotY(2, 0, 0, 0.01, 0.45, &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.45, &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 now_num;

int main()
{
    pc.baud(115200);
    //タイマー３の優先度を最低にする
    NVIC_SetPriority(TIMER3_IRQn, 3);
    
    wait(1);
    imu.performCalibration();
    imu.startAngleComputing();
    LED[1] = 1;
    
    enc[0].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)
    {
        reset_System();
        riseUssTriger();
        can.read();
        
        //目指すべきロボットの状態を取得
        state tar_state = getTargetState();
        
        //横USSでC_tableマップリング
        discoveryTable();
        
        int can_angle = 900 - int(tar_state.angle * 10);
        //角度は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.03)
        {
            if(can.send())
            {
                pre_time = now_time;
                LED[0] = !LED[0];
            }
        }
        
        //ロボット速度を取得
        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", state_lib[21][1]);
        pc.printf("%.2f\t", state_lib[25][1]);
        
        /*
        pc.printf("%.2f\t", imu.angle[2]);
        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);
        
        for(int i = 0; i < 9; i++)
            pc.printf("%d\t", table_sw[i]);
        */
        pc.printf("\n");
    }
}

state getTargetState()
{
    static bool have_ever_shot = 0;
    static bool start_flag = 0;
    
    int state_num;
    int tops_num;
    
    /*
    スタートボタン待機
    whileにするとほかの処理がすべて止まるためこれを回避
    */
    if(sw1 == 1)
        start_flag = 1;
    
    //sw1が1度も押されていないとき
    if(!start_flag)
        state_num = tops_num = odm.x = odm.y = imu.angle[2] = have_ever_shot = 0;
    else state_num  = now_num = updateStateNum();
    
    //再びスタート待機
    if(state_num == 0)
        start_flag = 0;
    
    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);
    
    int target_table = state_lib[state_num][3];
    bool is_shoot = state_lib[state_num][4];
    
    have_ever_shot |= is_shoot;
    
    if(have_ever_shot)
        tops_lib[1][2] = 1;
    else tops_lib[1][2] = 0;
    
    tops_lib[3][0] = para_lib[target_table].vel;
    tops_lib[2][1] = tops_lib[3][1] = para_lib[target_table].angle;
    
    tops_num = updateTopsNum(is_shoot);
    
    pc.printf("%d\t", state_num);
    pc.printf("%d\t", tops_num);
    
    state a;
    
    //changeZoneの処理
    if(table_sw[8] == 1)
    {
        a.x = -1 * state_lib[state_num][0];
        a.theta = -1 * state_lib[state_num][2];
    } else {
        a.x = state_lib[state_num][0];
        a.theta = state_lib[state_num][2];
    }
    a.y = state_lib[state_num][1];
    a.shoot = tops_lib[tops_num][0];
    a.angle = tops_lib[tops_num][1];
    a.supply = tops_lib[tops_num][2];
    return a;
}

bool is_shot = 0;

int updateStateNum()
{
    /*
    状態収束を判定して次のステップに進む
    */
    float t = 0.1;
    static int num = 0;
    //x方向、y方向、yow方向の判定をはっきりしておく
    bool flag_x = 0, flag_y = 0, flag_yow = 0, flag_tops = 0;
    
    if(pidRobotYow.isConvergence(t))
        flag_yow = 1;
    
    if(state_lib[num][0] >= 10 || state_lib[num][0] <= -10)
    {
        if(pidUss.isConvergence(t+1))
            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(state_lib[num][4] == 1 && (uss[0].distance > 100 || uss[1].distance > 100))
            
        if(pidUss.isConvergence(t+1))
            flag_y = flag_x = 1;
    }
    else if(pidRobotY.isConvergence(t) == 1)
        flag_y = 1;
    
    if(state_lib[num][4] == 1)
    {
        if(is_shot)
            flag_tops = 1;
    }
    else flag_tops = 1;
    
    //全部が収束してるか
    if(flag_x && flag_y && flag_yow && flag_tops)
    {
        num++;
        is_shot = 0;
    }
    //振り出しに戻る
    if(num >= LIBNUM)
        num = 0;
    
    num = changeNextTable(num);
    /*
    pc.printf("%d\t", flag_x);
    pc.printf("%d\t", flag_y);
    pc.printf("%d\t", flag_yow);
    pc.printf("%d\t", flag_tops);
    pc.printf("%d\t", pidRobotX.isConvergence(0.1));
    pc.printf("%d\t", pidRobotY.isConvergence(0.1));
    pc.printf("%d\t", pidRobotYow.isConvergence(0.1));
    */
    return num;
}

int updateTopsNum(bool is_shoot)
{
    static bool pre_is_shoot = 0;
    static int num = 0;
    if(isConvergenceCatapult(num) && isConvergenceSupply(num))
    {
        if(num != 2)
            num++;
        else if(is_shoot && !pre_is_shoot)
        {
            num++;
            pre_is_shoot = 1;
        }
    }
    if(num >= 4)
    {
        num = 0;
        is_shot = 1;
    }
    if(!is_shoot)    
        pre_is_shoot = 0;
    
    return num;
}

int changeNextTable(int num)
{
    if(table_sw[3] == 1 && table_sw[4] == 0)
    {
        if(num > 4 && num < 17)
            num = 17;
    }
    else if(table_sw[3] == 0 && table_sw[4] == 1)
    {
        if(num > 0 && num < 5)
            num = 5;
        if(num > 8 && num < 17)
            num = 17;
    }
    else if(table_sw[3] == 1 && table_sw[4] == 1)
    {
        if(num > 0 && num < 9)
            num = 9;
    }
    else if(num > 0 && num < 18)
            num = 18;
    if(table_sw[5] == 0 && table_sw[6] == 0 && table_sw[7] == 0)
    {
        if(num > 16 && num < 30)
            num = 31;
    }
    else
    {
        if(table_sw[5] == 0 && num > 17 && num < 22)
            num = 22;
        if(table_sw[6] == 0 && num > 21 && num < 26)
            num = 26;
        if(table_sw[7] == 0 && num > 25 && num < 30)
            num = 30;
    }
    if(table_sw[0] == 1 || table_sw[1] == 1 || table_sw[2] == 1)
    {
        if(table_sw[0] == 0 && num > 30 && num < 35)
            num = 35;
        if(table_sw[1] == 0 && num > 34 && num < 39)
            num = 39;
        if(table_sw[2] == 0 && num > 38 && num < 43)
            num = 43;
        if(table_sw[1] == 0 && table_sw[2] == 0 && num == 43)
            num = 44;
    }
    else if(num > 29 && num < 44)
        num = 44;
    
    return num;
}

bool isConvergenceCatapult(int 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 == tops_lib[num][0] && angle_val == tops_lib[num][1])
        return 1;
    else return 0;
}

bool isConvergenceSupply(int num)
{
    int is_supply_done = can.get(3, 5);
        
    if(tops_lib[num][2] == 1)
    {
        if(is_supply_done == 1)
            return 1;
        else return 0;
    }else return 1;
}

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

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.05)
    {
        if(flag)
        {
            uss[0].riseTrigerEvent();
            flag = 0;
        }
        else
        {
            uss[1].riseTrigerEvent();
            flag = 1;
        }
        start_time = now_time;
    }
}

void reset_System()
{
    int caliblation_sw = sw2;
    static int last_sw = 1;
    if(caliblation_sw == 1 && last_sw == 0)
        NVIC_SystemReset();
    else if(caliblation_sw == 0)
        last_sw = LED[1] = 0;
}

void discoveryTable()
{
    static bool c2_discovery = 0;
    static bool c3_discovery = 0;
    int _uss;
    if(table_sw[8] == 0)
        _uss = can.get(8, 1);
    else _uss = can.get(8, 2);
    if(now_num == 19)
    {
        if(table_sw[6] == 1)
        {
            if(_uss > 40.0 && _uss < 60.0)
            {
                state_lib[21][1] = state_lib[22][1] = odm.y - 0.7;
                c2_discovery = 1;
                if(c3_discovery == 0)
                {
                    state_lib[25][1] = state_lib[26][1] = odm.y - 0.7;
                    c3_discovery = 1;
                }
            }
            else if(c2_discovery == 0)
                state_lib[21][1] = state_lib[22][1] = odm.y - 0.3;
        }
        if(table_sw[7] == 1 && c2_discovery == 0)
        {
            if(_uss > 120.0 && _uss < 160.0)
            {
                state_lib[25][1] = state_lib[26][1] = odm.y - 0.7;
                c3_discovery = 1;
            }
        }
    }
}