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
Diff: main.cpp
- Revision:
- 1:0f33a68d1390
- Parent:
- 0:56a2c0ed99c5
--- 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;