Make sundry improvements/bug fixes to auto-aiming
This includes:
-Adding some velocity and voltage limits to prevent excessive
velocity and acceleration.
-Logging more information in the status message.
-Fixing calculation of the desired turret velocity.
Change-Id: I83840bc1db9dda003be63e594d826fc97fa509e8
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 8947f85..0d740e9 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -40,7 +40,8 @@
if (drivetrain_status_fetcher_.Fetch()) {
aos::Alliance alliance = aos::Alliance::kInvalid;
- if (joystick_state_fetcher_.Fetch()) {
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() != nullptr) {
alliance = joystick_state_fetcher_->alliance();
}
const turret::Aimer::WrapMode mode =
@@ -120,7 +121,8 @@
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
- const double turret_velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ const double turret_velocity_sign =
+ turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index 81293d3..dc6116c 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -36,6 +36,11 @@
turret_velocity:double;
// Whether we are currently aiming for the inner port.
aiming_for_inner_port:bool;
+ // The current distance to the target, in meters.
+ target_distance:double;
+ // The current "shot distance." When shooting on the fly, this may be
+ // different from the static distance to the target.
+ shot_distance:double;
}
table Status {
diff --git a/y2020/control_loops/superstructure/turret/aiming.cc b/y2020/control_loops/superstructure/turret/aiming.cc
index 360fd1a..0ce4871 100644
--- a/y2020/control_loops/superstructure/turret/aiming.cc
+++ b/y2020/control_loops/superstructure/turret/aiming.cc
@@ -50,8 +50,10 @@
// outer port entirely.
constexpr double kMaxInnerPortAngle = 20.0 * M_PI / 180.0;
-// Distance (in meters) from the edge of the field to the port.
-constexpr double kEdgeOfFieldToPort = 2.404;
+// 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
+// that reported by the cameras.
+constexpr double kEdgeOfFieldToPort = 2.404 + .0034;
// The amount (in meters) that the inner port is set back from the outer port.
constexpr double kInnerPortBackset = 0.743;
@@ -70,7 +72,7 @@
// Minimum distance that we must be from the inner port in order to attempt the
// shot--this is to account for the fact that if we are too close to the target,
// then we won't have a clear shot on the inner port.
-constexpr double kMinimumInnerPortShotDistance = 4.0;
+constexpr double kMinimumInnerPortShotDistance = 3.0;
// Amount of buffer, in radians, to leave to help avoid wrapping. I.e., any time
// that we are in kAvoidEdges mode, we will keep ourselves at least
@@ -123,7 +125,7 @@
Pose InnerPortPose(aos::Alliance alliance) {
const Pose target({kFieldLength / 2 + kInnerPortBackset,
-kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
- 0.0);
+ M_PI);
if (alliance == aos::Alliance::kRed) {
return ReverseSideOfField(target);
}
@@ -133,7 +135,7 @@
Pose OuterPortPose(aos::Alliance alliance) {
Pose target(
{kFieldLength / 2, -kFieldWidth / 2.0 + kEdgeOfFieldToPort, kPortHeight},
- 0.0);
+ M_PI);
if (alliance == aos::Alliance::kRed) {
return ReverseSideOfField(target);
}
@@ -164,9 +166,16 @@
const double inner_port_angle = robot_pose_from_inner_port.heading();
const double inner_port_distance = robot_pose_from_inner_port.xy_norm();
+ // Add a bit of hysteresis so that we don't jump between aiming for the inner
+ // and outer ports.
+ const double max_inner_port_angle =
+ aiming_for_inner_port_ ? 1.2 * kMaxInnerPortAngle : kMaxInnerPortAngle;
+ const double min_inner_port_distance =
+ aiming_for_inner_port_ ? 0.8 * kMinimumInnerPortShotDistance
+ : kMinimumInnerPortShotDistance;
aiming_for_inner_port_ =
- (std::abs(inner_port_angle) < kMaxInnerPortAngle) &&
- (inner_port_distance > kMinimumInnerPortShotDistance);
+ (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
// current velocity, to allow for shooting on-the-fly.
@@ -177,6 +186,7 @@
Pose virtual_goal;
{
const Pose goal = aiming_for_inner_port_ ? inner_port : outer_port;
+ target_distance_ = goal.Rebase(&robot_pose).xy_norm();
virtual_goal = goal;
if (shot_mode == ShotMode::kShootOnTheFly) {
for (int ii = 0; ii < 3; ++ii) {
@@ -191,7 +201,7 @@
const double heading_to_goal = virtual_goal.heading();
CHECK(status->has_localizer());
- distance_ = virtual_goal.xy_norm();
+ shot_distance_ = virtual_goal.xy_norm();
// The following code all works to calculate what the rate of turn of the
// turret should be. The code only accounts for the rate of turn if we are
@@ -201,6 +211,12 @@
const double rel_x = virtual_goal.rel_pos().x();
const double rel_y = virtual_goal.rel_pos().y();
const double squared_norm = rel_x * rel_x + rel_y * rel_y;
+ // rel_xdot and rel_ydot are the derivatives (with respect to time) of rel_x
+ // and rel_y. Since these are in the robot's coordinate frame, and since we
+ // are ignoring lateral velocity for this exercise, rel_ydot is zero, and
+ // rel_xdot is just the inverse of the robot's velocity.
+ const double rel_xdot = -linear_angular(0);
+ const double rel_ydot = 0.0;
// If squared_norm gets to be too close to zero, just zero out the relevant
// term to prevent NaNs. Note that this doesn't address the chattering that
@@ -208,11 +224,12 @@
// Note that x and y terms are swapped relative to what you would normally see
// in the derivative of atan because xdot and ydot are the derivatives of
// robot_pos and we are working with the atan of (target_pos - robot_pos).
- const double atan_diff = (squared_norm < 1e-3)
- ? 0.0
- : (rel_y * xdot - rel_x * ydot) / squared_norm;
+ const double atan_diff =
+ (squared_norm < 1e-3) ? 0.0 : (rel_x * rel_ydot - rel_y * rel_xdot) /
+ squared_norm;
// heading = atan2(relative_y, relative_x) - robot_theta
- // dheading / dt = (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
+ // dheading / dt =
+ // (rel_x * rel_y' - rel_y * rel_x') / (rel_x^2 + rel_y^2) - dtheta / dt
const double dheading_dt = atan_diff - linear_angular(1);
double range = kTurretRange;
@@ -234,7 +251,8 @@
}
goal_.mutable_message()->mutate_unsafe_goal(turret_heading);
- goal_.mutable_message()->mutate_goal_velocity(dheading_dt);
+ goal_.mutable_message()->mutate_goal_velocity(
+ std::clamp(dheading_dt, -2.0, 2.0));
}
flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
@@ -243,6 +261,8 @@
builder.add_turret_position(goal_.message().unsafe_goal());
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_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 3b3071e..854518c 100644
--- a/y2020/control_loops/superstructure/turret/aiming.h
+++ b/y2020/control_loops/superstructure/turret/aiming.h
@@ -56,7 +56,7 @@
const Goal *TurretGoal() const { return &goal_.message(); }
// Returns the distance to the goal, in meters.
- double DistanceToGoal() const { return distance_; }
+ double DistanceToGoal() const { return shot_distance_; }
flatbuffers::Offset<AimerStatus> PopulateStatus(
flatbuffers::FlatBufferBuilder *fbb) const;
@@ -64,7 +64,11 @@
private:
aos::FlatbufferDetachedBuffer<Goal> goal_;
bool aiming_for_inner_port_ = false;
- double distance_ = 0.0;
+ // Distance of the shot to the virtual target, used for calculating hood
+ // position and shooter speed.
+ double shot_distance_ = 0.0; // meters
+ // Real-world distance to the target.
+ double target_distance_ = 0.0; // meters
};
} // namespace turret
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index cb95b56..ab600fa 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -178,7 +178,7 @@
// angle on it.
TEST_F(AimerTest, InnerPort) {
const Pose target = InnerPortPose(aos::Alliance::kRed);
- const Goal *goal = Update({.x = target.abs_pos().x() + 1.0,
+ const Goal *goal = Update({.x = target.abs_pos().x() + 10.0,
.y = target.abs_pos().y() + 0.0,
.theta = 0.0,
.linear = 0.0,
@@ -186,6 +186,7 @@
aos::Alliance::kRed);
EXPECT_EQ(0.0, goal->unsafe_goal());
EXPECT_EQ(0.0, goal->goal_velocity());
+ EXPECT_LT(1.0, aimer_.DistanceToGoal());
}
// Confirms that when we move the turret heading so that it would be entirely