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/constants.cc b/y2022/constants.cc
index cb85e94..e7c0ca3 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -137,16 +137,42 @@
{1.9, {0.1, 19.0}},
{2.12, {0.15, 18.8}},
{2.9, {0.25, 19.2}},
+ {3.2, {0.28, 20.5}},
- {3.8, {0.35, 20.6}},
- {4.9, {0.4, 21.9}},
- {6.0, {0.40, 24.0}},
- {7.0, {0.40, 25.5}},
+ {3.86, {0.35, 20.9}},
+ {4.9, {0.4, 21.9}},
+ {5.4, {0.4, 23.9}},
+ {6.0, {0.40, 25.0}},
+ {7.0, {0.37, 27.1}},
- {7.8, {0.35, 26.9}},
- {10.0, {0.35, 26.9}},
+ {7.8, {0.35, 28.0}},
+ {10.0, {0.35, 28.0}},
});
+ if (false) {
+ // 1.5 meters -> 2.7
+ // 2.3 meters -> 4.7
+ // 4.5 meters -> 7.0
+ // 7.0 meters -> 9.0
+
+ constexpr double kShotVelocity = 9.0;
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {1.0, {kShotVelocity}},
+ {10.0, {kShotVelocity}},
+ });
+ } else {
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {1.0, {2.7}},
+ {1.5, {2.7}},
+ {2.3, {4.7}},
+ {4.5, {7.0}},
+ {7.0, {9.0}},
+ {10.0, {9.0}},
+ });
+ }
+
switch (team) {
// A set of constants for tests.
case 1:
@@ -155,6 +181,12 @@
{5, {0.6, 10.0}},
});
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {2, {2.0}},
+ {5, {4.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
@@ -224,6 +256,12 @@
{5, {0.6, 10.0}},
});
+ r.shot_velocity_interpolation_table =
+ InterpolationTable<Values::ShotVelocityParams>({
+ {2, {2.0}},
+ {5, {4.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
diff --git a/y2022/constants.h b/y2022/constants.h
index 610ddc0..31f8a0c 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -218,13 +218,28 @@
static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
using ::frc971::shooter_interpolation::Blend;
return ShotParams{
- Blend(coefficient, a1.shot_angle, a2.shot_angle),
- Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
+ .shot_angle = Blend(coefficient, a1.shot_angle, a2.shot_angle),
+ .shot_velocity =
+ Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
};
}
};
+ struct ShotVelocityParams {
+ // Speed over ground to use for shooting-on-the-fly.
+ double shot_speed_over_ground;
+
+ static ShotVelocityParams BlendY(double coefficient, ShotVelocityParams a1,
+ ShotVelocityParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return ShotVelocityParams{Blend(coefficient, a1.shot_speed_over_ground,
+ a2.shot_speed_over_ground)};
+ }
+ };
+
InterpolationTable<ShotParams> shot_interpolation_table;
+
+ InterpolationTable<ShotVelocityParams> shot_velocity_interpolation_table;
};
// Creates and returns a Values instance for the constants.
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_;
};
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 60af1f8..93c72d7 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -133,7 +133,7 @@
}
// Threshold for mean distance from a blob centroid to a circle.
- constexpr double kCircleDistanceThreshold = 10.0;
+ constexpr double kCircleDistanceThreshold = 1.0;
// We should only expect to see blobs between these angles on a circle.
constexpr double kDegToRad = M_PI / 180.0;
constexpr double kMinBlobAngle = 50.0 * kDegToRad;
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 61789cb..f59f2cf 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -105,7 +105,7 @@
camera_yaw = 0.0
T = np.array([-7.5 * 0.0254, -3.5 * 0.0254, 34.0 * 0.0254])
elif pi_number == "pi3":
- camera_yaw = 179.0 * np.pi / 180.0
+ camera_yaw = 178.5 * np.pi / 180.0
T = np.array([-1.0 * 0.0254, 8.5 * 0.0254, 34.25 * 0.0254])
elif pi_number == "pi4":
camera_yaw = -90.0 * np.pi / 180.0
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
index 22794d3..9320152 100644
--- a/y2022/vision/camera_reader_main.cc
+++ b/y2022/vision/camera_reader_main.cc
@@ -10,7 +10,7 @@
DEFINE_double(duty_cycle, 0.6, "Duty cycle of the LEDs");
DEFINE_uint32(exposure, 5,
"Exposure time, in 100us increments; 0 implies auto exposure");
-DEFINE_uint32(outdoors_exposure, 2,
+DEFINE_uint32(outdoors_exposure, 20,
"Exposure time when using --use_outdoors, in 100us increments; 0 "
"implies auto exposure");