ichinoseki_Bteam_2019 / Mbed 2 deprecated 2019_AR_Itsuki

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   IMU mbed Odometer PID MDD RotaryEncoder UART Mycan DriveConroller

hardwareConfig.h@1:0f33a68d1390, 2020-01-12 (annotated)

Committer:
TanakaTarou
Date:
Sun Jan 12 08:12:42 2020 +0000
Revision:
1:0f33a68d1390
Parent:
0:56a2c0ed99c5 
Itsuki

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TanakaTarou 0:56a2c0ed99c5 1 #include "DriveController.h"
TanakaTarou 0:56a2c0ed99c5 2 #include "IMU.h"
TanakaTarou 0:56a2c0ed99c5 3 #include "MDD.h"
TanakaTarou 0:56a2c0ed99c5 4 #include "MDD2.h"
TanakaTarou 0:56a2c0ed99c5 5 #include "Mycan.h"
TanakaTarou 0:56a2c0ed99c5 6 #include "Odometer.h"
TanakaTarou 0:56a2c0ed99c5 7 #include "PID.h"
TanakaTarou 0:56a2c0ed99c5 8 #include "RotaryEncoder.h"
TanakaTarou 0:56a2c0ed99c5 9 #include "SplineCurve.h"
TanakaTarou 0:56a2c0ed99c5 10 #include "SBUS.h"
TanakaTarou 0:56a2c0ed99c5 11
TanakaTarou 0:56a2c0ed99c5 12 #define WHEEL_DIAMETER 0.100 //オムニホイール直径[mm]
TanakaTarou 0:56a2c0ed99c5 13 #define ODM_DIAMETER 0.0508 //ODMホイール直径[mm]
TanakaTarou 0:56a2c0ed99c5 14 #define PID_PERIOD 0.01 //PID計算周期
TanakaTarou 0:56a2c0ed99c5 15 #define P_GAIN 2.00 //平面座標系PIDのPゲイン, 0.3:1.5, 0.4:0.8
TanakaTarou 0:56a2c0ed99c5 16 #define ROUTE_NUM 3 //組み合わせる経路の数
TanakaTarou 0:56a2c0ed99c5 17 #define SPLINE_NUM 4 //spline経路そのものの数
TanakaTarou 0:56a2c0ed99c5 18 #define MAX_SPEED 4.72 //オムニのモータの最高回転速度
TanakaTarou 0:56a2c0ed99c5 19
TanakaTarou 0:56a2c0ed99c5 20 //オンボードLEDの使用宣言
TanakaTarou 0:56a2c0ed99c5 21 DigitalOut LED[] = {DigitalOut(LED1),
TanakaTarou 0:56a2c0ed99c5 22 DigitalOut(LED2),
TanakaTarou 0:56a2c0ed99c5 23 DigitalOut(LED3),
TanakaTarou 0:56a2c0ed99c5 24 DigitalOut(LED4),
TanakaTarou 0:56a2c0ed99c5 25 };
TanakaTarou 0:56a2c0ed99c5 26 DigitalOut servo_right(p5);
TanakaTarou 0:56a2c0ed99c5 27 DigitalOut servo_left(p6);
TanakaTarou 0:56a2c0ed99c5 28 DigitalOut servo_mode(p19);
TanakaTarou 0:56a2c0ed99c5 29
TanakaTarou 0:56a2c0ed99c5 30 DigitalIn sw[4] = {DigitalIn(p8),
TanakaTarou 0:56a2c0ed99c5 31 DigitalIn(p7),
TanakaTarou 0:56a2c0ed99c5 32 DigitalIn(p11),
TanakaTarou 0:56a2c0ed99c5 33 DigitalIn(p12),
TanakaTarou 0:56a2c0ed99c5 34 };
TanakaTarou 0:56a2c0ed99c5 35
TanakaTarou 0:56a2c0ed99c5 36 //デバック用シリアル
TanakaTarou 0:56a2c0ed99c5 37 Serial pc(USBTX, USBRX);
TanakaTarou 0:56a2c0ed99c5 38
TanakaTarou 1:0f33a68d1390 39 //逆運動学モデルのヤコビアン
TanakaTarou 0:56a2c0ed99c5 40 float jacobian[4][3] = {{ 0.7071, -0.7071, -1},
TanakaTarou 0:56a2c0ed99c5 41 { 0.7071, 0.7071, -1},
TanakaTarou 0:56a2c0ed99c5 42 {-0.7071, 0.7071, -1},
TanakaTarou 0:56a2c0ed99c5 43 {-0.7071, -0.7071, -1}
TanakaTarou 0:56a2c0ed99c5 44 };
TanakaTarou 0:56a2c0ed99c5 45
TanakaTarou 0:56a2c0ed99c5 46 //(逆運動学モデル, ホイールの計算100%定義)
TanakaTarou 0:56a2c0ed99c5 47 DriveController omni(jacobian, 1.0);
TanakaTarou 0:56a2c0ed99c5 48
TanakaTarou 0:56a2c0ed99c5 49 //sbus用 (tx, rx)
TanakaTarou 0:56a2c0ed99c5 50 SBUS sbus(p9, p10);
TanakaTarou 0:56a2c0ed99c5 51
TanakaTarou 0:56a2c0ed99c5 52 //(計算周期時間[s], sla, sld)
TanakaTarou 0:56a2c0ed99c5 53 IMU imu(0.01, p28, p27);
TanakaTarou 0:56a2c0ed99c5 54
TanakaTarou 0:56a2c0ed99c5 55 Mycan can(p30, p29, 100000);
TanakaTarou 0:56a2c0ed99c5 56 Timer timer;
TanakaTarou 0:56a2c0ed99c5 57 Ticker ticker;
TanakaTarou 0:56a2c0ed99c5 58
TanakaTarou 0:56a2c0ed99c5 59 //(A相, B相)
TanakaTarou 0:56a2c0ed99c5 60 RotaryEncoder odm_enc[2] = {RotaryEncoder(p13, p14, &timer, 460),
TanakaTarou 0:56a2c0ed99c5 61 RotaryEncoder(p16, p15, &timer, 460),
TanakaTarou 0:56a2c0ed99c5 62 };
TanakaTarou 0:56a2c0ed99c5 63 float enc[8];
TanakaTarou 0:56a2c0ed99c5 64
TanakaTarou 0:56a2c0ed99c5 65 //コントローラー入力のための構造体
TanakaTarou 0:56a2c0ed99c5 66 typedef struct controller {
TanakaTarou 0:56a2c0ed99c5 67 float LX, LY, RX, RY;
TanakaTarou 0:56a2c0ed99c5 68 int A,B,C,D,E,F,G,H;
TanakaTarou 0:56a2c0ed99c5 69 bool fail_safe;
TanakaTarou 0:56a2c0ed99c5 70 } controller;
TanakaTarou 0:56a2c0ed99c5 71
TanakaTarou 0:56a2c0ed99c5 72 //x軸補正用 PID
TanakaTarou 1:0f33a68d1390 73 //PID pidRobotX(0.8, 0.0, 0.085, PID_PERIOD, 0.50, 0.08, false, &timer);
TanakaTarou 1:0f33a68d1390 74 //PID pidRobotX(0.95, 0.0, 0.082, PID_PERIOD, 0.40, 0.08, false, &timer);
TanakaTarou 1:0f33a68d1390 75 PID pidRobotX(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, 0.08, false, &timer);
TanakaTarou 0:56a2c0ed99c5 76
TanakaTarou 0:56a2c0ed99c5 77 //y軸補正用 PID
TanakaTarou 1:0f33a68d1390 78 //PID pidRobotY(0.8, 0.0, 0.085, PID_PERIOD, 0.50, 0.08, false, &timer);
TanakaTarou 1:0f33a68d1390 79 //PID pidRobotY(0.95, 0.0, 0.082, PID_PERIOD, 0.40, 0.08, false, &timer);
TanakaTarou 1:0f33a68d1390 80 PID pidRobotY(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, 0.08, false, &timer);
TanakaTarou 0:56a2c0ed99c5 81
TanakaTarou 0:56a2c0ed99c5 82 //yaw角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
TanakaTarou 1:0f33a68d1390 83 //PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, false, &timer);
TanakaTarou 1:0f33a68d1390 84 PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, 4, false, &timer);
TanakaTarou 0:56a2c0ed99c5 85
TanakaTarou 0:56a2c0ed99c5 86 //昇降機構PID
TanakaTarou 1:0f33a68d1390 87 PID pidLift(3.5, 0, 0.04, PID_PERIOD, 0.9, 0.05, false, &timer);
TanakaTarou 1:0f33a68d1390 88 //PID pidVelLift(0, 0, 0, PID_PERIOD, 1.0, 0.5, true, &timer);
TanakaTarou 0:56a2c0ed99c5 89 float target_lift = 0;
TanakaTarou 0:56a2c0ed99c5 90
TanakaTarou 0:56a2c0ed99c5 91 //幅可変機構PID
TanakaTarou 1:0f33a68d1390 92 PID pidWide(3.5, 0, 0.01, PID_PERIOD, 0.95, 0.075, false, &timer);
TanakaTarou 0:56a2c0ed99c5 93
TanakaTarou 0:56a2c0ed99c5 94 //装填機構PID
TanakaTarou 1:0f33a68d1390 95 PID pidSupply(4.0, 0, 0, PID_PERIOD, 0.95, 0.075, false, &timer);
TanakaTarou 0:56a2c0ed99c5 96 /*
TanakaTarou 0:56a2c0ed99c5 97 //ホイール速度MAX4.72[m/s]
TanakaTarou 0:56a2c0ed99c5 98 float wheel_rps[4], robot_max_sp = 1.18, robot_speed, wheel_speed[4], error_speed[4];
TanakaTarou 0:56a2c0ed99c5 99
TanakaTarou 0:56a2c0ed99c5 100 //一定の加速度で加速し続けた時の目標速度まで到達するまでの所要時間
TanakaTarou 0:56a2c0ed99c5 101 float reach_time = 0.25, vel_max = 1.0;
TanakaTarou 0:56a2c0ed99c5 102 //一定の加速度で加速し続けた時の所要時間を目安に決めた最高加速度
TanakaTarou 0:56a2c0ed99c5 103 float acc_max = PID_PERIOD / reach_time;
TanakaTarou 0:56a2c0ed99c5 104 //0.~の値を小さくするほど収束は急になるが、加速度を調整するならこの係数は適当で良い
TanakaTarou 0:56a2c0ed99c5 105 float vel_P = acc_max / (robot_max_sp * 0.15);
TanakaTarou 0:56a2c0ed99c5 106 PID_VEL pidRobotSpeed(vel_P, 0, 0, PID_PERIOD, acc_max, vel_max, &timer);
TanakaTarou 0:56a2c0ed99c5 107 float target_speed = 0;
TanakaTarou 0:56a2c0ed99c5 108 */
TanakaTarou 0:56a2c0ed99c5 109 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},
TanakaTarou 0:56a2c0ed99c5 110 sp_0_1[2] = { 0.000, 2.200}, rp_0_1[2] = { 0.300, 2.700}, ep_0_1[2] = { 0.800, 3.000},
TanakaTarou 0:56a2c0ed99c5 111 sp_0_2[2] = { 0.800, 3.700}, rp_0_2[2] = { 1.300, 3.700}, ep_0_2[2] = { 1.800, 3.700},
TanakaTarou 0:56a2c0ed99c5 112
TanakaTarou 0:56a2c0ed99c5 113 sp_1_0[2] = { 3.540, 4.700}, rp_1_0[2] = { 2.855, 3.950}, ep_1_0[2] = { 0.800, 3.700},
TanakaTarou 0:56a2c0ed99c5 114 sp_1_1[2] = { 0.800, 3.700}, rp_1_1[2] = { 0.200, 3.500}, ep_1_1[2] = { 0.000, 2.900},
TanakaTarou 0:56a2c0ed99c5 115 sp_1_2[2] = { 0.000, 2.900}, rp_1_2[2] = { 0.000, 1.650}, ep_1_2[2] = { 0.000, 0.400},
TanakaTarou 0:56a2c0ed99c5 116
TanakaTarou 0:56a2c0ed99c5 117 sp_2_0[2] = { 0.000, 0.000}, rp_2_0[2] = { 0.000, 2.450}, ep_2_0[2] = { 0.000, 4.900},
TanakaTarou 0:56a2c0ed99c5 118 sp_2_1[2] = { 0.000, 4.900}, rp_2_1[2] = { 0.200, 5.500}, ep_2_1[2] = { 0.800, 5.700},
TanakaTarou 0:56a2c0ed99c5 119 sp_2_2[2] = { 0.800, 5.700}, rp_2_2[2] = { 1.300, 5.700}, ep_2_2[2] = { 1.800, 5.700},
TanakaTarou 0:56a2c0ed99c5 120
TanakaTarou 0:56a2c0ed99c5 121 sp_3_0[2] = { 3.500, 6.600}, rp_3_0[2] = { 2.150, 6.150}, ep_3_0[2] = { 0.800, 5.700},
TanakaTarou 0:56a2c0ed99c5 122 sp_3_1[2] = { 0.800, 5.700}, rp_3_1[2] = { 0.200, 5.500}, ep_3_1[2] = { 0.000, 4.900},
TanakaTarou 0:56a2c0ed99c5 123 sp_3_2[2] = { 0.000, 4.900}, rp_3_2[2] = { 0.000, 2.650}, ep_3_2[2] = { 0.000, 0.400};
TanakaTarou 0:56a2c0ed99c5 124
TanakaTarou 0:56a2c0ed99c5 125 //PIDライブラリの係数
TanakaTarou 0:56a2c0ed99c5 126 float pid_lib_gain[5] = {0.95, 0.0, 0.082, PID_PERIOD, 0.3};
TanakaTarou 0:56a2c0ed99c5 127
TanakaTarou 0:56a2c0ed99c5 128 SplineCurve spline[SPLINE_NUM][ROUTE_NUM] =
TanakaTarou 0:56a2c0ed99c5 129 {
TanakaTarou 0:56a2c0ed99c5 130 {
TanakaTarou 0:56a2c0ed99c5 131 SplineCurve(sp_0_0, ep_0_0, rp_0_0, 32, 50.0, 6.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 132 SplineCurve(sp_0_1, ep_0_1, rp_0_1, 32, 50.0, 2.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 133 SplineCurve(sp_0_2, ep_0_2, rp_0_2, 32, 50.0, 4.0, pid_lib_gain, 0),
TanakaTarou 0:56a2c0ed99c5 134 },{
TanakaTarou 0:56a2c0ed99c5 135 SplineCurve(sp_1_0, ep_1_0, rp_1_0, 32, 50.0, 6.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 136 SplineCurve(sp_1_1, ep_1_1, rp_1_1, 32, 50.0, 2.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 137 SplineCurve(sp_1_2, ep_1_2, rp_1_2, 32, 50.0, 6.0, pid_lib_gain, 0),
TanakaTarou 0:56a2c0ed99c5 138 },{
TanakaTarou 0:56a2c0ed99c5 139 SplineCurve(sp_2_0, ep_2_0, rp_2_0, 32, 50.0, 9.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 140 SplineCurve(sp_2_1, ep_2_1, rp_2_1, 32, 50.0, 3.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 141 SplineCurve(sp_2_2, ep_2_2, rp_2_2, 32, 50.0, 4.0, pid_lib_gain, 0),
TanakaTarou 0:56a2c0ed99c5 142 },{
TanakaTarou 0:56a2c0ed99c5 143 SplineCurve(sp_3_0, ep_3_0, rp_3_0, 32, 50.0, 6.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 144 SplineCurve(sp_3_1, ep_3_1, rp_3_1, 32, 50.0, 3.0, pid_lib_gain, 1),
TanakaTarou 0:56a2c0ed99c5 145 SplineCurve(sp_3_2, ep_3_2, rp_3_2, 32, 50.0, 9.0, pid_lib_gain, 0),
TanakaTarou 0:56a2c0ed99c5 146 }
TanakaTarou 0:56a2c0ed99c5 147 };
TanakaTarou 0:56a2c0ed99c5 148
TanakaTarou 0:56a2c0ed99c5 149 //(始点[m], 終点[m], 曲線基準点[m], 組み合わせる直線の本数【8・16・32】, 途中目標点の定数[倍], 軌道修正の係数, 終点は停止[0]するか通過するか[1])
TanakaTarou 0:56a2c0ed99c5 150
TanakaTarou 0:56a2c0ed99c5 151 //オドメーターの定義
TanakaTarou 0:56a2c0ed99c5 152 float matrix[3][3] = {{1, 0, 0},
TanakaTarou 0:56a2c0ed99c5 153 {0, 1, 0},
TanakaTarou 0:56a2c0ed99c5 154 {0, 0, 0}
TanakaTarou 0:56a2c0ed99c5 155 };
TanakaTarou 0:56a2c0ed99c5 156 Odometer odm(matrix, ODM_DIAMETER, &timer);