Add interpolation table for auto-aim speed-over-ground
We actually have a shot velocity that varies significantly with
distance.
Change-Id: I3a4a59ef3cb181c9f437d50f865d04f710a7f961
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b21dd0d..d23e589 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -5,6 +5,9 @@
#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
+DEFINE_bool(ignore_distance, false,
+ "If true, ignore distance when shooting and obay joystick_reader");
+
namespace y2022 {
namespace control_loops {
namespace superstructure {
@@ -28,7 +31,8 @@
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
can_position_fetcher_(
- event_loop->MakeFetcher<CANPosition>("/superstructure")) {
+ event_loop->MakeFetcher<CANPosition>("/superstructure")),
+ aimer_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -92,8 +96,9 @@
constants::Values::ShotParams shot_params;
const double distance_to_goal = aimer_.DistanceToGoal();
- if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
- distance_to_goal, &shot_params)) {
+ if (!FLAGS_ignore_distance && unsafe_goal->auto_aim() &&
+ values_->shot_interpolation_table.GetInRange(distance_to_goal,
+ &shot_params)) {
flatbuffers::FlatBufferBuilder *catapult_goal_fbb =
catapult_goal_buffer.fbb();
std::optional<flatbuffers::Offset<
@@ -243,7 +248,7 @@
.shooting = true});
// Dont shoot if the robot is moving faster than this
- constexpr double kMaxShootSpeed = 1.7;
+ constexpr double kMaxShootSpeed = 2.7;
const bool moving_too_fast = std::abs(robot_velocity()) > kMaxShootSpeed;
switch (state_) {
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 643adbc..5fe8a2c 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -1,6 +1,5 @@
#include "y2022/control_loops/superstructure/turret/aiming.h"
-#include "y2022/constants.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
namespace y2022 {
@@ -13,10 +12,6 @@
using frc971::control_loops::aiming::RobotState;
namespace {
-// Average speed-over-ground of the ball on its way to the target. Our current
-// model assumes constant ball velocity regardless of shot distance.
-constexpr double kBallSpeedOverGround = 2.0; // m/s
-
// If the turret is at zero, then it will be at this angle at which the shot
// will leave the robot. I.e., if the turret is at zero, then the shot will go
// straight out the back of the robot.
@@ -43,7 +38,8 @@
}
} // namespace
-Aimer::Aimer() : goal_(MakePrefilledGoal()) {}
+Aimer::Aimer(std::shared_ptr<const constants::Values> constants)
+ : constants_(constants), goal_(MakePrefilledGoal()) {}
void Aimer::Update(const Status *status, ShotMode shot_mode) {
const Pose robot_pose({status->x(), status->y(), 0}, status->theta());
@@ -56,15 +52,18 @@
const double xdot = linear_angular(0) * std::cos(status->theta());
const double ydot = linear_angular(0) * std::sin(status->theta());
- current_goal_ =
- frc971::control_loops::aiming::AimerGoal(
- ShotConfig{goal, shot_mode, constants::Values::kTurretRange(),
- kBallSpeedOverGround,
- /*wrap_mode=*/0.0, kTurretZeroOffset},
- RobotState{robot_pose,
- {xdot, ydot},
- linear_angular(1),
- goal_.message().unsafe_goal()});
+ // Use the previous shot distance to estimate the speed-over-ground of the
+ // ball.
+ current_goal_ = frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode, constants_->kTurretRange(),
+ constants_->shot_velocity_interpolation_table
+ .Get(current_goal_.target_distance)
+ .shot_speed_over_ground,
+ /*wrap_mode=*/0.0, kTurretZeroOffset},
+ RobotState{robot_pose,
+ {xdot, ydot},
+ linear_angular(1),
+ goal_.message().unsafe_goal()});
goal_.mutable_message()->mutate_unsafe_goal(current_goal_.position);
goal_.mutable_message()->mutate_goal_velocity(
diff --git a/y2022/control_loops/superstructure/turret/aiming.h b/y2022/control_loops/superstructure/turret/aiming.h
index 9494103..4eabe3e 100644
--- a/y2022/control_loops/superstructure/turret/aiming.h
+++ b/y2022/control_loops/superstructure/turret/aiming.h
@@ -2,10 +2,11 @@
#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
#include "aos/flatbuffers.h"
+#include "frc971/control_loops/aiming/aiming.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
-#include "frc971/control_loops/aiming/aiming.h"
+#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
namespace y2022::control_loops::superstructure::turret {
@@ -19,7 +20,7 @@
typedef frc971::control_loops::drivetrain::Status Status;
typedef frc971::control_loops::aiming::ShotMode ShotMode;
- Aimer();
+ Aimer(std::shared_ptr<const constants::Values> constants);
void Update(const Status *status, ShotMode shot_mode);
@@ -32,6 +33,7 @@
flatbuffers::FlatBufferBuilder *fbb) const;
private:
+ std::shared_ptr<const constants::Values> constants_;
aos::FlatbufferDetachedBuffer<Goal> goal_;
frc971::control_loops::aiming::TurretGoal current_goal_;
};