Narrow down the inner port transition so we don't hit the corners
Report it out too to make it easier to debug.
Change-Id: Iacbc289afd8f88958ee7a75e342096de43e8e23b
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 16f7445..4a977b8 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -60,6 +60,8 @@
// The current "shot distance." When shooting on the fly, this may be
// different from the static distance to the target.
shot_distance:double (id: 4);
+ // Amount the shot is off from being dead-straight relative to the inner port.
+ inner_port_angle:double (id: 5);
}
table Status {
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 2cc2ce0..94047d2 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -48,7 +48,7 @@
// exactly perpendicular to the target. Larger numbers allow us to aim at the
// inner port more aggressively, at the risk of being more likely to miss the
// outer port entirely.
-constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
+constexpr double kMaxInnerPortAngle = 10.0 * M_PI / 180.0;
// Distance (in meters) from the edge of the field to the port, with some
// compensation to ensure that our definition of where the target is matches
@@ -164,7 +164,7 @@
const double xdot = linear_angular(0) * std::cos(status->theta());
const double ydot = linear_angular(0) * std::sin(status->theta());
- const double inner_port_angle = robot_pose_from_inner_port.heading();
+ inner_port_angle_ = robot_pose_from_inner_port.heading();
const double inner_port_distance = robot_pose_from_inner_port.rel_pos().x();
// Add a bit of hysteresis so that we don't jump between aiming for the inner
// and outer ports.
@@ -174,7 +174,7 @@
aiming_for_inner_port_ ? (kMinimumInnerPortShotDistance - 0.3)
: kMinimumInnerPortShotDistance;
aiming_for_inner_port_ =
- (std::abs(inner_port_angle) < max_inner_port_angle) &&
+ (std::abs(inner_port_angle_) < max_inner_port_angle) &&
(inner_port_distance > min_inner_port_distance);
// This code manages compensating the goal turret heading for the robot's
@@ -262,6 +262,7 @@
builder.add_turret_velocity(goal_.message().goal_velocity());
builder.add_aiming_for_inner_port(aiming_for_inner_port_);
builder.add_target_distance(target_distance_);
+ builder.add_inner_port_angle(inner_port_angle_);
builder.add_shot_distance(DistanceToGoal());
return builder.Finish();
}
diff --git a/y2020/control_loops/superstructure/turret/aiming.h b/y2020/control_loops/superstructure/turret/aiming.h
index 7f69d48..ed00972 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -69,6 +69,7 @@
double shot_distance_ = 0.0; // meters
// Real-world distance to the target.
double target_distance_ = 0.0; // meters
+ double inner_port_angle_ = 0.0; // radians
};
} // namespace turret