File content as of revision 4:9f74525eb37f:

#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[19][6] = {{0.0, 0.0, 0.0, 0, 90, 0},//初期位置
                         {-3.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化
                         {-3.825, 20.0, 0.0, 0, 82, 0},//y移動
                         {-3.825, 20.0, 0.0, 35, 82, 0},//発射
                         {-3.825, 0.0, 0.0, 0, 90, 0},//y移動,角度戻す, P上昇
                         
                         {-4.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
                         {-4.825, 20.0, 0.0, 0, 82, 0},//y移動
                         {-4.825, 20.0, 0.0, 35, 82, 0},//発射
                         {-4.825, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
                         
                         {-5.825, 0.0, 0.0, 0, 82, 0},//x移動, 角度変化, 下降&バットマン駆動
                         {-5.825, 20.0, 0.0, 0, 82, 0},//y移動
                         {-5.825, 20.0, 0.0, 35, 82, 0},//発射
                         {-5.825, 0.0, 0.0, 0, 90, 1},//y移動,角度戻す, P上昇
                         
                         {-1.825, 0.0, 0.0, 0, 82, 0.0},//x移動, 角度変化, 下降&バットマン駆動
                         {-1.825, 20.0, 0.0, 0, 82, 0.0},//y移動
                         {-1.825, 20.0, 0.0, 18, 82, 0.0},//下段に発射
                         {-1.825, 20.0, 0.0, 0, 90, 0.0},//角度戻す, P上昇
                         {-1.825, 20.0, 0.0, 0, 85, 0.0},//角度変化, 下降&バットマン駆動
                         {-1.825, 20.0, 0.0, 18, 85, 0.0},//上段に発射
                        };

controller getPropoData();
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.6, &timer);
float target_uss = 8.0;
int posi_num = 0;

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

    //IMUのキャリブレーション
    wait(1);
    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();
    
    //許容誤差
    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(1) 
    {
        controller cmd = getPropoData();
        
        can.set(1, 1, int(position[posi_num][3]));
        can.set(1, 2, int(position[posi_num][4]));
        //can.set(1, 3, int(position[posi_num][5]));
        while(can.send() == 0);
        
        can.read();
        
        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)
                posi_num++;
        }
        
        if(posi_num >= 19)
            posi_num = 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][1] >= 10)
        {
            *pidUss.target = position[posi_num][1] - 8;
            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.01);
    }
}

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