ichinoseki_Bteam_2019
/
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
Revision 1:0f33a68d1390, committed 2020-01-12
- Comitter:
- TanakaTarou
- Date:
- Sun Jan 12 08:12:42 2020 +0000
- Parent:
- 0:56a2c0ed99c5
- Commit message:
- Itsuki
Changed in this revision
--- a/PID.lib Mon Oct 21 14:25:38 2019 +0000 +++ b/PID.lib Sun Jan 12 08:12:42 2020 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/ichinoseki_Bteam_2019/code/PID/#925e92d4d4e8 +https://os.mbed.com/teams/ichinoseki_Bteam_2019/code/PID/#aa402cc1ef2b
--- a/SplineCurve.lib Mon Oct 21 14:25:38 2019 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/teams/ichinoseki_Bteam_2019/code/SplineCurve/#65dca185cd59
--- a/hardwareConfig.h Mon Oct 21 14:25:38 2019 +0000
+++ b/hardwareConfig.h Sun Jan 12 08:12:42 2020 +0000
@@ -5,7 +5,6 @@
#include "Mycan.h"
#include "Odometer.h"
#include "PID.h"
-#include "PID_VEL.h"
#include "RotaryEncoder.h"
#include "SplineCurve.h"
#include "SBUS.h"
@@ -37,7 +36,7 @@
//デバック用シリアル
Serial pc(USBTX, USBRX);
-//メカナム逆運動学モデルのヤコビアン
+//逆運動学モデルのヤコビアン
float jacobian[4][3] = {{ 0.7071, -0.7071, -1},
{ 0.7071, 0.7071, -1},
{-0.7071, 0.7071, -1},
@@ -71,33 +70,29 @@
} 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;
+//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, &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;
+//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, &timer);
-PID pidRobotYaw(0.009, 0, 0.00015, PID_PERIOD, 0.12, &timer);
-float target_yaw = 0;
+//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, &timer);
+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, &timer);
-float target_wide = 0;
+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, &timer);
-float target_supply = 0;
+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];
--- a/main.cpp Mon Oct 21 14:25:38 2019 +0000
+++ b/main.cpp Sun Jan 12 08:12:42 2020 +0000
@@ -45,14 +45,15 @@
}
//許容誤差
- pidRobotX.allowable_error = 0.08; //[m]
- pidRobotY.allowable_error = 0.08; //[m]
- pidRobotYaw.allowable_error = 4; //[°]
- pidLift.allowable_error = 0.05; //×0.3180[m] = 15.9[mm]
- pidWide.allowable_error = 0.075; //×0.1000[m] = 7.5[mm]
- pidSupply.allowable_error = 0.075; //×0.0625[m] = 4.7[mm]
+ //pidRobotX.allowable_error = 0.08; //[m]
+ //pidRobotY.allowable_error = 0.08; //[m]
+ //pidRobotYaw.allowable_error = 4; //[°]
+ //pidLift.allowable_error = 0.05; //×0.3180[m] = 15.9[mm]
+ //pidWide.allowable_error = 0.075; //×0.1000[m] = 7.5[mm]
+ //pidSupply.allowable_error = 0.075; //×0.0625[m] = 4.7[mm]
//PID設定
+ float target_x = 0, target_y = 0, target_yaw = 0, target_lift = 0, target_wide = 0, target_supply = 0;
pidRobotX.sensor = &odm.x;
pidRobotX.target = &target_x;
pidRobotX.start();
@@ -65,6 +66,11 @@
pidLift.sensor = &enc[4];
pidLift.target = &target_lift;
pidLift.start();
+ /*
+ pidVelLift.sensor = &tmp_val;
+ pidVelLift.target = &tmp_val;
+ pidVelLift.start();
+ */
pidWide.sensor = &enc[5];
pidWide.target = &target_wide;
pidWide.start();
@@ -81,10 +87,10 @@
pc.printf("%.3f\t", imu.angle[2]);
pc.printf("%.3f\t", lift_val);
+ pc.printf("%.3f\t", *pidLift.target);
pc.printf("%.3f\t", enc[4]);
- pc.printf("%.3f\t", wide_val);
- pc.printf("%.3f\t", enc[5]);
pc.printf("%.3f\t", supply_val);
+ pc.printf("%.3f\t", *pidSupply.target);
pc.printf("%.3f\t", enc[6]);
bool _sw[4];
@@ -98,14 +104,17 @@
pc.printf("%.3f\t", read_line_data[1]);
pc.printf("%.3f\t", robot_velocity[0]);
pc.printf("%.3f\t", robot_velocity[1]);
- //pc.printf("%.3f\t", robot_velocity[2]);
pc.printf("%.3f\t", *pidRobotX.target);
pc.printf("%.3f\t", *pidRobotY.target);
- //pc.printf("%.3f\t", *pidLift.target);
- //pc.printf("%.3f\t", pidLift.output);
pc.printf("%d\t", now_state);
pc.printf("%d\t", now_tops);
-
+ /*
+ pc.printf("%d\t", S_sw);
+ pc.printf("%d\t", B_sw[0]);
+ pc.printf("%d\t", B_sw[1]);
+ pc.printf("%d\t", B_sw[2]);
+ pc.printf("%d\t", B_sw[3]);
+ */
pc.printf("\n");
}
}
@@ -128,12 +137,12 @@
}
else
{
- robot_velocity[0] = 0;
- robot_velocity[1] = 0;
- robot_velocity[2] = 0;
- if(cmd.A == 2)
- *pidLift.target = 5.13;
- else *pidLift.target = 0.095;
+ robot_velocity[0] = cmd.LX;
+ robot_velocity[1] = cmd.LY;
+ robot_velocity[2] = cmd.RX;
+ if(cmd.A == 0)
+ *pidLift.target = 0.095;
+ else *pidLift.target = 5.13;
*pidWide.target = 0.00;
*pidSupply.target = 0.0;
//lift_val = cmd.RY;
@@ -189,7 +198,8 @@
void canFnc()
{
- readSwitchData();
+ if(now_state == 0)
+ readSwitchData();
can.readF();
enc[4] = -can.get(4, 0);
@@ -231,7 +241,11 @@
if(safety == 2) {
for(int i = 0; i < 4; i++)
can.setI(1, i, (short int)(omni.wheel_vel[i] * 200));
- can.setI(1, 4, (short int)(lift_val * 200));
+
+ if((odm.y > 4.35 && odm.y < 5.20)
+ || (odm.y > 6.35 && odm.y < 7.20))
+ can.setI(1, 4, 0);
+ else can.setI(1, 4, (short int)(lift_val * 200));
//can.setI(1, 5, (short int)(wide_val * 200));
can.setI(1, 5, 0);
can.setI(1, 6, (short int)(supply_val * 200 * -1));
@@ -421,17 +435,31 @@
}
} else {
if(!match) {
- for(int i=0; i<6; i++)
- state_lib[i] = SQ_lib[state_num][i];
- if(state_num > 2 && state_num < 6) {
- state_lib[0] += error_x;
- state_lib[1] += error_y;
- if(state_lib[1] == S_y_2)
- state_lib[1] += line_error;
+ if(B_sw[0] || B_sw[1] || B_sw[2]) {
+ for(int i=0; i<6; i++)
+ state_lib[i] = SBQ_lib[state_num][i];
+ if(state_num > 2 && state_num < 6) {
+ state_lib[0] += error_x;
+ state_lib[1] += error_y;
+ if(state_lib[1] == S_y_2 || state_lib[1] == B_y_3)
+ state_lib[1] += line_error;
+ }
+ max_num = SBQ_NUM;
+ if(state_num < max_num - 2)
+ r_point = SBQ_lib[state_num + 2][5];
+ } else {
+ for(int i=0; i<6; i++)
+ state_lib[i] = SQ_lib[state_num][i];
+ if(state_num > 2 && state_num < 6) {
+ state_lib[0] += error_x;
+ state_lib[1] += error_y;
+ if(state_lib[1] == S_y_2)
+ state_lib[1] += line_error;
+ }
+ max_num = SQ_NUM;
+ if(state_num < max_num - 2)
+ r_point = SQ_lib[state_num + 2][5];
}
- max_num = SQ_NUM;
- if(state_num < max_num - 2)
- r_point = SQ_lib[state_num + 2][5];
}
else {
for(int i=0; i<6; i++)
@@ -456,7 +484,7 @@
if(state_num > 2 && state_num < 16) {
state_lib[0] += error_x;
state_lib[1] += error_y;
- if(state_lib[1] == B_y_2)
+ if(state_lib[1] == B_y_3)
state_lib[1] += line_error;
}
max_num = BQ_NUM;
@@ -484,7 +512,7 @@
if(state_num > 2 && state_num < 21) {
state_lib[0] += error_x;
state_lib[1] += error_y;
- if(state_lib[1] == B_y_2)
+ if(state_lib[1] == B_y_3)
state_lib[1] += line_error;
}
max_num = BF_NUM;
@@ -627,6 +655,10 @@
if(convergence_tops)
flag_tops = 1;
}
+ else if(S_sw && (B_sw[0] || B_sw[1] || B_sw[2]) && !match && (lib_num > 4 && lib_num < 7)) {
+ if(convergence_tops)
+ flag_tops = 1;
+ }
else {
if(num < lib_num - 2 && release_point == 1) {
if(is_release)
@@ -641,8 +673,8 @@
before_count = 1;
//実際のフィールドのラインの誤差
if(S_sw)
- tape_posi = -0.018;
- else tape_posi = -0.013;
+ tape_posi = 0.0;
+ else tape_posi = 0.0;
if(state_lib[1] == T_y_1 || state_lib[1] == B_y_1 || state_lib[1] == S_y_1) {
//Y軸targetがライン上の時, ラインセンサで誤差を検出
state tar;
@@ -724,12 +756,12 @@
if(odm.y < 2.45)
num++;
}
- else if(_laundry == 2) {
- if(odm.y < 4.70)
+ else if(_laundry == 2) {
+ if(odm.y < 4.30)
num++;
}
else if(_laundry == 3) {
- if(odm.y < 6.45 && _tops == 0)
+ if(odm.y < 6.30 && _tops == 0)
num++;
}
} else {
@@ -767,8 +799,12 @@
num = 0;
}
else if(_laundry == 3) {
- if(num >= 1)
- num = 0;
+ if(B_sw[0] || B_sw[1] || B_sw[2]) {
+ if(num >= 4)
+ num = 0;
+ }
+ else if(num >= 1)
+ num = 0;
}
else num = 0;
@@ -827,7 +863,7 @@
{
if(!S_sw) {
if(!match) {
- if(B_sw[0] || B_sw[1] || B_sw[2] || B_sw[3]) {
+ if(B_sw[0] || B_sw[1] || B_sw[2]) {
if(!B_sw[0] && num > 1 && num < 6) {
num = 6;
}
@@ -838,7 +874,7 @@
num = 16;
}
}
- else if(T_sw[0] || T_sw[1] || T_sw[2] || T_sw[3]) {
+ else if(T_sw[0] || T_sw[1] || T_sw[2]) {
if(!T_sw[0] && num > 1 && num < 6) {
num = 6;
}
@@ -880,6 +916,20 @@
}
}
}
+ } else {
+ if(!match) {
+ if(B_sw[0] || B_sw[1] || B_sw[2]) {
+ if(!B_sw[0] && num > 7 && num < 12) {
+ num = 12;
+ }
+ if(!B_sw[1] && num > 11 && num < 17) {
+ num = 17;
+ }
+ if(!B_sw[2] && num > 16 && num < 22) {
+ num = 22;
+ }
+ }
+ }
}
return num;
--- a/stateLib.h Mon Oct 21 14:25:38 2019 +0000
+++ b/stateLib.h Sun Jan 12 08:12:42 2020 +0000
@@ -4,6 +4,7 @@
#define BF_NUM 24
#define SQ_NUM 9
#define SF_NUM 21
+#define SBQ_NUM 25
#define TEST_NUM 6
#define TOPSNUM 5
@@ -36,7 +37,7 @@
#define BF_x_4 3.445f //決勝４つ目
#define B_y_1 3.700f //バスタオルラインの位置
-#define B_y_2 4.350f //バスタオルかける直前
+#define B_y_2 4.250f //バスタオルかける直前
#define B_y_3 4.800f //バスタオルかける時
#define S_x_1 1.830f //シーツをかける時
@@ -206,6 +207,40 @@
{0.00, 0.00, 0.0, -1, 0, 2},
};
+float SBQ_lib[30][6] =
+{
+ {0.00, 0.00, 0.0, -1, 0, 2},
+
+ {0.00, S_y_1, 0.0, -1, Sheets, 0},
+ {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0},
+ {BQ_x_2, S_y_2, 0.0, -1, Sheets, 0},
+ {BQ_x_2, S_y_2, 0.0, -1, Sheets, 1},
+ {BQ_x_2, S_y_1, 0.0, -1, Sheets, 0},
+ {0.00, S_y_1, 0.0, -1, BathTowel, 0},
+
+ {0.00, B_y_1, 0.0, -1, BathTowel, 0},
+//ここから
+ {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_2, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_2, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_2, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_3, 0.0, -1, BathTowel, 0},
+ {BF_x_3, B_y_3, 0.0, -1, BathTowel, 1},
+ {BF_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+//ここまで変更箇所
+ {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_2, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 0},
+ {BQ_x_3, B_y_3, 0.0, -1, BathTowel, 1},
+ {BQ_x_3, B_y_1, 0.0, -1, BathTowel, 0},
+ {0.00, B_y_1, 0.0, -1, NEUTRAL, 2},
+
+ {0.00, 0.35, 0.0, -1, 0, 2},
+ {0.00, 0.00, 0.0, -1, 0, 2},
+};
+
float Test_lib[SF_NUM][6] =
{
{0.00, 0.00, 0.0, -1, NEUTRAL, 2},