File content as of revision 8:123cd1f07aea:

#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)
float position[19][6] = {{-1.825, 0.0, 0.0, 0.0, 0, 82},//0, x移動, 角度変化
                         {-1.825, 0.0, 0.0, 18.0, 0, 82},//1,y移動
                         {-1.825, 0.0, 0.0, 18.0, 35, 82},//2,発射
                         {-1.825, 0.0, 0.0, 0.0, 0, 88},//3,y移動,角度戻す, P上昇
                         
                         {-2.825, 0.0, 0.0, 0.0, 0, 88},//4, x移動, 角度変化, 下降&バットマン駆動
                         {-2.825, 0.0, 0.0, 18.0, 0, 82},//5,y移動
                         {-2.825, 0.0, 0.0, 18.0, 35, 82},//6,発射
                         {-2.825, 0.0, 0.0, 0.0, 0, 88},//7,y移動,角度戻す, P上昇
                         
                         {-3.825, 0.0, 0.0, 0.0, 0, 88},//8,x移動, 角度変化, 下降&バットマン駆動
                         {-3.825, 0.0, 0.0, 18.0, 0, 82},//9,y移動
                         {-3.825, 0.0, 0.0, 18.0, 35, 82},//10,発射
                         {-3.825, 0.0, 0.0, 0.0, 0, 88},//11,y移動,角度戻す, P上昇
                         
                         {-0.825, 0.0, 0.0, 0.0, 0, 88},//12,x移動, 角度変化, 下降&バットマン駆動
                         {-0.825, 0.0, 0.0, 18.0, 0, 82},//13,y移動
                         {-0.825, 0.0, 0.0, 18.0, 18, 82},//14,下段に発射
                         {-0.825,0.0, 0.0, 18.0, 0, 88},//15,角度戻す, P上昇
                         {-0.825, 0.0, 0.0, 18.0, 0, 85},//16,角度変化, 下降&バットマン駆動
                         {-0.825, 0.0, 0.0, 18.0, 18, 85},//17,上段に発射
                         /*
                         {-0.825, 0.0, 90.0, 0.0, 0, 88},//18
                         
                         {-0.5, 0.0, 90.0, 0.0, 0, 88},//19
                         {-0.5, 1.125, 90.0, 0.0, 0, 88},//20
                         {-0.5, 1.125, 90.0, 18.0, 0, 82},//21
                         {-0.5, 1.125, 90.0, 18.0, 30, 82},//22
                         {-0.5, 1.125, 90.0, 0.0, 0, 88},//23
                         
                         {-0.5, 1.625, 90.0, 0.0, 0, 88},//24
                         {-0.5, 1.625, 90.0, 18.0, 0, 82},//25
                         {-0.5, 1.625, 90.0, 18.0, 30, 82},//26
                         {-0.5, 1.625, 90.0, 0.0, 0, 88},//27
                         
                         {-0.5, 2.125, 90.0, 0.0, 0, 88},//28
                         {-0.5, 2.125, 90.0, 18.0, 0, 82},//29
                         {-0.5, 2.125, 90.0, 18.0, 30, 82},//30
                         {-0.5, 2.125, 90.0, 0.0, 0, 88},//31
                         */
                         {-0.0, 0.0, 0.0, 0, 0.0, 90}//18
                        };

bool isConvergetnceTops();

//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.025, 0, 0, 0.1, 0.3, &timer);
float target_uss = 8.0;
int posi_num = 0;

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

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

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

    //PID設定
    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 = &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;
    
    //マイクロスイッチ
    sw1.mode(PullUp);
    sw2.mode(PullUp);
    
    while(sw1 == 0);
    while(1)
    {
        can.read();
        /*
        int supply_pid = can.get(2, 1);
        int supply_wait = can.get(2, 2);
        int angle_wait = can.get(3, 5);
        */
        int sw[8] = {can.get(4, 1),
                     can.get(4, 2),
                     can.get(4, 3),
                     can.get(4, 4),
                     can.get(4, 5),
                     can.get(4, 6),
                     can.get(4, 7),
                     can.get(4, 8)
                    };
        
        can.set(1, 1, int(position[posi_num][4]));
        can.set(1, 2, int(position[posi_num][5]));
        /*
        if(supply_wait == 1 && angle_wait == 1)
            can.set(1, 3, 1);
        else can.set(1, 3, 0);
        */
        while(can.send() == 0);
        
        //ポジション収束判定
        if(pidRobotX.isConvergence(1) == 1 
        && pidRobotYow.isConvergence(1) == 1)
        {
            //超音波で近づくとき
            if(position[posi_num][1] >= 10)
            {
                if(pidUss.isConvergence(1) == 1
                && isConvergetnceTops() == 1)
                    posi_num++;
            }
            //超音波使わないとき
            else if(pidRobotY.isConvergence(1) == 1)
            {
                /*
                if(posi_num == 2 || posi_num == 6 || posi_num == 10 || posi_num == 14 || posi_num == 17)
                {
                    if(supply_pid == 1)
                        posi_num++;
                }
                else 
                */
                posi_num++;
            }
        }
        if(sw[0] == 1 && posi_num == 0)
            posi_num = 4;
        if(sw[1] == 1 && posi_num == 4)
            posi_num = 8;
        if(sw[2] == 1 && posi_num == 8)
            posi_num = 12;
        if(sw[3] == 1 && sw[4] == 1 && posi_num == 12)
            posi_num = 14;
        if(sw[3] == 1 && posi_num == 14)
            posi_num = 17;
        if(sw[4] == 1 && posi_num == 17)
            posi_num = 18;
        
        if(posi_num < 19)
        {
            posi_num = 0;
            while(sw1 == 0);
        }
        
        //ロボットの移動速度(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][3] >= 10)
        {
            *pidUss.target = position[posi_num][3] - 10;
            robot_velocity[1] = -pidUss.output;
        }
        else 
        {
            //y軸調整処理
            *pidRobotY.target = position[posi_num][1];
            robot_velocity[1] = pidRobotY.output;
        }
        /*
        if(sw1 == 1 && sw2 == 1)
            imu.angle[2] = odm.y = 0;
        */
        //ホイール速度計算
        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]);
        pc.printf("%.2f\t", uss.distance);
        pc.printf("\n");
        
        wait(0.02);
    }
}

bool isConvergetnceTops()
{
    int velocity_pid = can.get(3, 1);
    int angle_pid    = can.get(3, 2);
    int velocity_val = can.get(3, 3);
    int angle_val    = can.get(3, 4);
    
    if(angle_pid == 1 && velocity_pid == 1 && velocity_val == position[posi_num][3] && angle_val == position[posi_num][4])
        return 1;
    else return 0;
}