File content as of revision 1:d7ceb38da3d8:

#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 "CSV.h"
#include "SBUS.h"
#include "USS.h"
#include "hardwareConfig.h"

//(X, Y, θ, speed, angle, injection, 補給昇降)
float position[10][7] = {{0.0, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化
                         {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動
                         {-0.3, 15.0, 0.0, 0, 90, 0, 0.0},//発射
                         {-0.3, 0.0, 0.0, 0, 90, 0, 0.0},//y移動,角度戻す, P上昇
                         
                         {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
                         {-0.6, 15.0, 0.0, 0, 90, 0, 0.0},//y移動
                         {-0.6, 0.0, 0.0, 0, 90, 0, 0.0},//発射
                         //{6.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
                         
                         {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
                         {-0.9, 15.0, 0.0, 20, 90, 0, 0.0},//y移動
                         {-0.9, 0.0, 0.0, 20, 90, 0, 0.0},//発射
                         //{7.0, 0.0, 0.0, 20, 90, 0, 6.8},//y移動,角度戻す, P上昇
                         /*
                         {3.0, 0.0, 0.0, 20, 80, 0, 0.0},//x移動, 角度変化, 下降&バットマン駆動
                         {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//y移動
                         {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//下段に発射
                         {3.0, 2.5, 0.0, 20, 90, 0, 6.8},//角度戻す, P上昇
                         {3.0, 2.5, 0.0, 20, 80, 0, 0.0},//角度変化, 下降&バットマン駆動
                         {3.0, 2.5, 0.0, 20, 80, 1, 0.0},//上段に発射
                         {0.0, 0.0, 0.0, 20, 90, 0, 6.8} //x・y移動,スタートゾーンに戻る
                         */
                        };

controller getPropoData();
//bool isConvergence(int num);

//x軸補正用 PID
PID pidRobotX(2, 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.95, &timer);
float target_yow = 0;
    
//USS用
PID pidUss(0.02, 0, 0, 0.01, 0.3, &timer);
float target_uss = 25.0;

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

    //IMUのキャリブレーション
    imu.performCalibration();
    imu.startAngleComputing();

    for(int i; i < 3; i++)
        enc[i].changeDirection();

    //オドメーターの定義
    float matrix[3][3] = {{1, 0, 0},
                          {0, 1, 0},
                          {0, 0, 0}
                         };
    Odometer odm(matrix, 0.048);
    float tmp = 0;
    float *encoders[3] = {&enc[0].rotations, &enc[1].rotations, &tmp};
    odm.setupOdometerSensors(encoders, &imu.angle[2]);
    odm.startComputingOdometry(0.005, 0, 0, 0);

    //オドメーターX
    pidRobotX.sensor = &odm.x;
    pidRobotX.target = &target_x;
    pidRobotX.start();

    //オドメーターY
    pidRobotY.sensor = &odm.y;
    pidRobotY.target = &target_y;
    pidRobotY.start();
    
    //IMUyow
    mecanum.imu_yow = &imu.angle[2];
    pidRobotYow.sensor = &imu.angle[2];
    pidRobotYow.target = &target_yow;
    pidRobotYow.start();
    
    //USS  pid設定
    uss.startTriger();
    pidUss.sensor = &uss.distance;
    pidUss.target = &target_uss;
    pidUss.start();
    
    pidRobotX.allowable_error = 0.1;
    pidRobotY.allowable_error = 0.1;
    pidRobotYow.allowable_error = 2;
    pidUss.allowable_error = 3;
    
    while(1) 
    {
        controller cmd = getPropoData();   //getPropoData & getCanData
        
        static int posi_num = 0;
        
        if(position[posi_num][1] >= 10)
        {
            if(pidRobotX.isConvergence(1) == 1 && pidRobotYow.isConvergence(1) == 1 && pidUss.isConvergence(1) == 1)
                posi_num++;
        }
        else if(pidRobotX.isConvergence(1) + pidRobotYow.isConvergence(1) + pidRobotY.isConvergence(1) == 3)
                posi_num++;
        
        
        if(posi_num >= 9)
            posi_num = 0;
        
        
        /*
        static int reset_swich = 0;
        if(cmd.H == 2 && reset_swich == 0)
        {
            posi_num += 1;
            if(posi_num > 9)
                posi_num = 0;
            reset_swich = 1;
        }
        else if(cmd.H == 0 && reset_swich == 1)
            reset_swich = 0;
        */
        /*
        if(position[posi_num][5] == 1)
            can.set(1, 1, position[posi_num][3]);
        else
            can.set(1, 1, 0);
        can.set(1, 2, position[posi_num][4]);
        can.set(2, 1, position[posi_num][6]);
        can.send();
        */
        
        //ロボットの移動速度(LX, LY, RX)
        float robot_velocity[3];

        //yow角調整処理
        *pidRobotYow.target = position[posi_num][2];
        robot_velocity[2] = pidRobotYow.output;

        //x軸調整処理
        *pidRobotX.target = position[posi_num][0];
        robot_velocity[0] = pidRobotX.output;
        
        //USS距離調整処理
        if(position[posi_num][1] >= 10)
        {
            *pidUss.target = position[posi_num][1] - 10;
            robot_velocity[1] = -pidUss.output;
        }
        else 
        {
            //y軸調整処理
            *pidRobotY.target = position[posi_num][1];
            robot_velocity[1] = pidRobotY.output;
        }

        //角度・XYリセット
        if(cmd.F == 2) 
        {
            imu.angle[2] = 0;
            *pidRobotYow.target = 0;
            odm.x = 0;
            *pidRobotX.target = 0;
            odm.y = 0;
            *pidRobotY.target = 0;
        }

        //ホイール速度計算
        mecanum.setVelL(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", *pidRobotYow.sensor);
        
        pc.printf("%.2f\t", *pidRobotX.target);
        pc.printf("%.2f\t", *pidRobotY.target);
        pc.printf("%.2f\t", *pidRobotYow.target);
        
        pc.printf("%d\t", pidRobotX.isConvergence(1));
        pc.printf("%d\t", pidRobotY.isConvergence(1));
        pc.printf("%d\t", pidRobotYow.isConvergence(1));
        pc.printf("%d\t", pidUss.isConvergence(1));
        
        pc.printf("%d\t", posi_num);
                
        pc.printf("\n");
        wait(0.02);
    }
}

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 = 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.H = sbus.getSwitchVal(0);
        propo.C = sbus.getSwitchVal(1);
        propo.E = sbus.getSwitchVal(2);
        propo.F = sbus.getSwitchVal(3);
        propo.G = sbus.getSwitchVal(4);
        propo.fail_safe = 1;
    }

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

/*
bool isConvergence(int num)
{
    bool tf;
    if(num > 3 && num < 8)
        num -= 4;
    else if(num > 7 && num < 12)
        num -= 8;
    else if(num > 11 && num < 15)
        num -= 8;

    if(num == 0)
    {
        if(pidRobotX.output == 0)
            tf = 1;
        else
            tf = 0;
    }
    else if(num == 1)
    {
        if(pidUss.output == 0)
            tf = 1;
        else
            tf = 0;
    }
    else if(num == 2) 
    {
        wait(1.0);
        tf = 1;
    } 
    else if(num == 3)
    {
        if(pidRobotY.output == 0)
            tf = 1;
        else
            tf = 0;
    }
    else if(num == 15)
    {
        
    }
    else if(num == 16)
    {
        
    }
    else if(num == 17)
    {
        
    }

    return tf;
}
*/