ichinoseki_Bteam_2019 / Mbed 2 deprecated 2019_AR_Itsuki

Dependencies:   IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller

hardwareConfig.h

Committer:
TanakaTarou
Date:
2020-01-12
Revision:
1:0f33a68d1390
Parent:
0:56a2c0ed99c5

File content as of revision 1:0f33a68d1390:

#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 "SplineCurve.h"
#include "SBUS.h"

#define WHEEL_DIAMETER 0.100   //オムニホイール直径[mm]
#define ODM_DIAMETER 0.0508    //ODMホイール直径[mm]
#define PID_PERIOD 0.01        //PID計算周期
#define P_GAIN 2.00            //平面座標系PIDのPゲイン, 0.3:1.5, 0.4:0.8
#define ROUTE_NUM 3            //組み合わせる経路の数
#define SPLINE_NUM 4           //spline経路そのものの数
#define MAX_SPEED 4.72         //オムニのモータの最高回転速度

//オンボードLEDの使用宣言
DigitalOut LED[] = {DigitalOut(LED1),
                    DigitalOut(LED2),
                    DigitalOut(LED3),
                    DigitalOut(LED4),
                   };
DigitalOut servo_right(p5);
DigitalOut servo_left(p6);
DigitalOut servo_mode(p19);

DigitalIn sw[4] = {DigitalIn(p8),
                   DigitalIn(p7),
                   DigitalIn(p11),
                   DigitalIn(p12),
                  };

//デバック用シリアル
Serial pc(USBTX, USBRX);

//逆運動学モデルのヤコビアン
float jacobian[4][3] = {{ 0.7071, -0.7071, -1},
                        { 0.7071,  0.7071, -1},
                        {-0.7071,  0.7071, -1},
                        {-0.7071, -0.7071, -1}
                       };
                       
//(逆運動学モデル, ホイールの計算100%定義)
DriveController omni(jacobian, 1.0);

//sbus用 (tx, rx)
SBUS sbus(p9, p10);

//(計算周期時間[s], sla, sld)
IMU imu(0.01, p28, p27);

Mycan can(p30, p29, 100000);
Timer timer;
Ticker ticker;

//(A相, B相)
RotaryEncoder odm_enc[2] = {RotaryEncoder(p13, p14, &timer, 460),
                            RotaryEncoder(p16, p15, &timer, 460),
                           };
float enc[8];

//コントローラー入力のための構造体
typedef struct controller {
    float LX, LY, RX, RY;
    int A,B,C,D,E,F,G,H;
    bool fail_safe;
} controller;

//x軸補正用 PID
//PID pidRobotX(0.8, 0.0, 0.085, PID_PERIOD, 0.50, 0.08, false, &timer);
//PID pidRobotX(0.95, 0.0, 0.082, PID_PERIOD, 0.40, 0.08, false, &timer);
PID pidRobotX(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, 0.08, false, &timer);

//y軸補正用 PID
//PID pidRobotY(0.8, 0.0, 0.085, PID_PERIOD, 0.50, 0.08, false, &timer);
//PID pidRobotY(0.95, 0.0, 0.082, PID_PERIOD, 0.40, 0.08, false, &timer);
PID pidRobotY(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, 0.08, false, &timer);

//yaw角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
//PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, false, &timer);
PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, 4, false, &timer);

//昇降機構PID
PID pidLift(3.5, 0, 0.04, PID_PERIOD, 0.9, 0.05, false, &timer);
//PID pidVelLift(0, 0, 0, PID_PERIOD, 1.0, 0.5, true, &timer);
float target_lift = 0;

//幅可変機構PID
PID pidWide(3.5, 0, 0.01, PID_PERIOD, 0.95, 0.075, false, &timer);

//装填機構PID
PID pidSupply(4.0, 0, 0, PID_PERIOD, 0.95, 0.075, false, &timer);
/*
//ホイール速度MAX4.72[m/s]
float wheel_rps[4], robot_max_sp = 1.18, robot_speed, wheel_speed[4], error_speed[4];

//一定の加速度で加速し続けた時の目標速度まで到達するまでの所要時間
float reach_time = 0.25, vel_max = 1.0;
//一定の加速度で加速し続けた時の所要時間を目安に決めた最高加速度
float acc_max = PID_PERIOD / reach_time;
//0.~の値を小さくするほど収束は急になるが、加速度を調整するならこの係数は適当で良い
float vel_P = acc_max / (robot_max_sp * 0.15);
PID_VEL pidRobotSpeed(vel_P, 0, 0, PID_PERIOD, acc_max, vel_max, &timer);
float target_speed = 0;
*/
float sp_0_0[2] = { 0.000,  0.000}, rp_0_0[2] = { 0.000,  1.100}, ep_0_0[2] = { 0.000,  2.200},
      sp_0_1[2] = { 0.000,  2.200}, rp_0_1[2] = { 0.300,  2.700}, ep_0_1[2] = { 0.800,  3.000},
      sp_0_2[2] = { 0.800,  3.700}, rp_0_2[2] = { 1.300,  3.700}, ep_0_2[2] = { 1.800,  3.700},
      
      sp_1_0[2] = { 3.540,  4.700}, rp_1_0[2] = { 2.855,  3.950}, ep_1_0[2] = { 0.800,  3.700},
      sp_1_1[2] = { 0.800,  3.700}, rp_1_1[2] = { 0.200,  3.500}, ep_1_1[2] = { 0.000,  2.900},
      sp_1_2[2] = { 0.000,  2.900}, rp_1_2[2] = { 0.000,  1.650}, ep_1_2[2] = { 0.000,  0.400},
      
      sp_2_0[2] = { 0.000,  0.000}, rp_2_0[2] = { 0.000,  2.450}, ep_2_0[2] = { 0.000,  4.900},
      sp_2_1[2] = { 0.000,  4.900}, rp_2_1[2] = { 0.200,  5.500}, ep_2_1[2] = { 0.800,  5.700},
      sp_2_2[2] = { 0.800,  5.700}, rp_2_2[2] = { 1.300,  5.700}, ep_2_2[2] = { 1.800,  5.700},
      
      sp_3_0[2] = { 3.500,  6.600}, rp_3_0[2] = { 2.150,  6.150}, ep_3_0[2] = { 0.800,  5.700},
      sp_3_1[2] = { 0.800,  5.700}, rp_3_1[2] = { 0.200,  5.500}, ep_3_1[2] = { 0.000,  4.900},
      sp_3_2[2] = { 0.000,  4.900}, rp_3_2[2] = { 0.000,  2.650}, ep_3_2[2] = { 0.000,  0.400};

//PIDライブラリの係数
float pid_lib_gain[5] = {0.95, 0.0, 0.082, PID_PERIOD, 0.3};

SplineCurve spline[SPLINE_NUM][ROUTE_NUM] = 
{
    {
        SplineCurve(sp_0_0, ep_0_0, rp_0_0, 32, 50.0, 6.0, pid_lib_gain, 1),
        SplineCurve(sp_0_1, ep_0_1, rp_0_1, 32, 50.0, 2.0, pid_lib_gain, 1),
        SplineCurve(sp_0_2, ep_0_2, rp_0_2, 32, 50.0, 4.0, pid_lib_gain, 0),
    },{
        SplineCurve(sp_1_0, ep_1_0, rp_1_0, 32, 50.0, 6.0, pid_lib_gain, 1),
        SplineCurve(sp_1_1, ep_1_1, rp_1_1, 32, 50.0, 2.0, pid_lib_gain, 1),
        SplineCurve(sp_1_2, ep_1_2, rp_1_2, 32, 50.0, 6.0, pid_lib_gain, 0),
    },{
        SplineCurve(sp_2_0, ep_2_0, rp_2_0, 32, 50.0, 9.0, pid_lib_gain, 1),
        SplineCurve(sp_2_1, ep_2_1, rp_2_1, 32, 50.0, 3.0, pid_lib_gain, 1),
        SplineCurve(sp_2_2, ep_2_2, rp_2_2, 32, 50.0, 4.0, pid_lib_gain, 0),
    },{
        SplineCurve(sp_3_0, ep_3_0, rp_3_0, 32, 50.0, 6.0, pid_lib_gain, 1),
        SplineCurve(sp_3_1, ep_3_1, rp_3_1, 32, 50.0, 3.0, pid_lib_gain, 1),
        SplineCurve(sp_3_2, ep_3_2, rp_3_2, 32, 50.0, 9.0, pid_lib_gain, 0),
    }
};

//(始点[m], 終点[m], 曲線基準点[m], 組み合わせる直線の本数【8・16・32】, 途中目標点の定数[倍], 軌道修正の係数, 終点は停止[0]するか通過するか[1])

//オドメーターの定義
float matrix[3][3] = {{1, 0, 0},
                      {0, 1, 0},
                      {0, 0, 0}
                     };
Odometer odm(matrix, ODM_DIAMETER, &timer);