Diff: hardwareConfig.h

Revision:

0:56a2c0ed99c5

Child:

1:0f33a68d1390

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hardwareConfig.h	Mon Oct 21 14:25:38 2019 +0000
@@ -0,0 +1,161 @@
+#include "DriveController.h"
+#include "IMU.h"
+#include "MDD.h"
+#include "MDD2.h"
+#include "Mycan.h"
+#include "Odometer.h"
+#include "PID.h"
+#include "PID_VEL.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, &timer);
+//PID pidRobotX(0.95, 0.0, 0.082, PID_PERIOD, 0.40, &timer);
+PID pidRobotX(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, &timer);
+float target_x = 0;
+
+//y軸補正用 PID
+//PID pidRobotY(0.8, 0.0, 0.085, PID_PERIOD, 0.50, &timer);
+//PID pidRobotY(0.95, 0.0, 0.082, PID_PERIOD, 0.40, &timer);
+PID pidRobotY(P_GAIN, 0, 0.23, PID_PERIOD, 0.3, &timer);
+float target_y = 0;
+
+//yaw角補正用 (Pgain, Igain, Dgain, 制御ループ時間[s], 計算出力100%定義)
+//PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, &timer);
+PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, &timer);
+float target_yaw = 0;
+
+//昇降機構PID
+PID pidLift(3.5, 0, 0.04, PID_PERIOD, 0.9, &timer);
+float target_lift = 0;
+
+//幅可変機構PID
+PID pidWide(3.5, 0, 0.01, PID_PERIOD, 0.95, &timer);
+float target_wide = 0;
+
+//装填機構PID
+PID pidSupply(4.0, 0, 0, PID_PERIOD, 0.95, &timer);
+float target_supply = 0;
+/*
+//ホイール速度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);
\ No newline at end of file