File content as of revision 12:91218718ae75:

#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();
bool isConvergenceTops(int);
bool isConvergenceSupply(int);
controller getPropoData();
void riseUssTriger();

//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.3, &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 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();
    
    changeToBlueZone();
    
    while(1)
    {
        riseUssTriger();
        can.read();
        
        //目指すべきロボットの状態を取得
        state tar_state = getTargetState();
        
        //canに置く
        //角度は2倍して送って、2で割って受け取る  角度可変が0.5度刻みにできる
        can.set(1, 1, tar_state.shoot);
        can.set(1, 2, int (tar_state.angle * 2));
        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 s1 = sw1, s2 = sw2;
        pc.printf("%d\t", s1);
        pc.printf("%d\t", s2);
        
        pc.printf("%.2f\t", uss[0].distance);
        pc.printf("%.2f\t", uss[1].distance);
        
        pc.printf("\n");
    }
}

state getTargetState()
{
    static bool start_flag = 0;
        
    int num;
    
    /*
    スタートボタン待機
    whileにするとほかの処理がすべて止めるためこれを回避
    */
    if(sw1 == 1)
        start_flag = 1;
    
    //sw1が1度も押されていないとき
    if(!start_flag)
        num = odm.x = odm.y = imu.angle[2] = 0;
    else num = updateStateNum();
    
    //再びスタート待機
    if(num == 0)
        start_flag = 0;    
    
    state a;
    //states辞典から値を引っ張る
    a.x = state_lib[num][0];
    a.y = state_lib[num][1];
    a.theta = state_lib[num][2];
    a.shoot = state_lib[num][3];
    a.angle = state_lib[num][4];
    a.supply = state_lib[num][5];
    return a;
}

int updateStateNum()
{
    /*
    状態収束を判定して次のステップに進む
    */
    float t = 0.1;
    static int num = 0;
    //x方向、y方向、yow方向の判定をはっきりしておく
    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(num) && isConvergenceSupply(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(num) && isConvergenceSupply(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;
}

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 state_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 /= 2.0;
        
    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == state_lib[state_num][3] && angle_val == state_lib[state_num][4])
        return 1;
    else return 0;
}
bool isConvergenceSupply(int state_num)
{
    int is_supply_done = can.get(3, 5);
        
    if(state_lib[state_num][5] == 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.05)
    {
        if(flag)
        {
            uss[0].riseTrigerEvent();
            flag = 0;
        }
        else
        {
            uss[1].riseTrigerEvent();
            flag = 1;
        }
        start_time = now_time;
    }
}