Add shooter interpolation
Includes interpolation values based on testing for at-home challenges
from
https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit
Change-Id: I094242f43f22da7602f60557a0ec2865a4066875
diff --git a/y2020/BUILD b/y2020/BUILD
index 90d976b..964def4 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -65,6 +65,7 @@
"//aos/stl_mutex",
"//frc971:constants",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/shooter_interpolation:interpolation",
"//y2020/control_loops/drivetrain:polydrivetrain_plants",
"//y2020/control_loops/superstructure/accelerator:accelerator_plants",
"//y2020/control_loops/superstructure/control_panel:control_panel_plants",
diff --git a/y2020/constants.cc b/y2020/constants.cc
index b3551aa..07d10ee 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -34,6 +34,20 @@
::frc971::zeroing::AbsoluteAndAbsoluteEncoderZeroingEstimator>
*const hood = &r->hood;
+ constexpr double kFeetToMeters = 0.0254 * 12.0;
+ // Approximate robot length, for converting estimates from the doc below.
+ // Rounded up from exact estimate, since I'm not sure if the original estimate
+ // includes bumpers.
+ constexpr double kRobotLength = 0.9;
+ // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ // Current settings based on
+ // https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit
+ r->shot_interpolation_table = InterpolationTable<Values::ShotParams>(
+ {{7.6 * kFeetToMeters - kRobotLength, {0.115, 197.0, 175.0}},
+ {7.6 * kFeetToMeters + kRobotLength, {0.31, 265.0, 235.0}},
+ {12.6 * kFeetToMeters + kRobotLength, {0.4, 292.0, 260.0}},
+ {17.6 * kFeetToMeters + kRobotLength, {0.52, 365.0, 325.0}}});
+
// Hood constants.
hood->zeroing_voltage = 2.0;
hood->operating_voltage = 12.0;
diff --git a/y2020/constants.h b/y2020/constants.h
index ee42245..85f9768 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -8,6 +8,7 @@
#include "frc971/constants.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.h"
#include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2020/control_loops/superstructure/accelerator/accelerator_plant.h"
#include "y2020/control_loops/superstructure/control_panel/control_panel_plant.h"
@@ -16,6 +17,8 @@
#include "y2020/control_loops/superstructure/intake/intake_plant.h"
#include "y2020/control_loops/superstructure/turret/turret_plant.h"
+using ::frc971::shooter_interpolation::InterpolationTable;
+
namespace y2020 {
namespace constants {
@@ -184,6 +187,28 @@
// Climber
static constexpr double kClimberSupplyCurrentLimit() { return 60.0; }
+
+ struct ShotParams {
+ // Measured in radians
+ double hood_angle;
+ // Angular velocity in radians per second of the slowest (lowest) wheel in the kicker.
+ // Positive is shooting the ball.
+ double accelerator_power;
+ // Angular velocity in radians per seconds of the flywheel. Positive is shooting.
+ double finisher_power;
+
+ static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return ShotParams{
+ Blend(coefficient, a1.hood_angle, a2.hood_angle),
+ Blend(coefficient, a1.accelerator_power, a2.accelerator_power),
+ Blend(coefficient, a1.finisher_power, a2.finisher_power)};
+ }
+ };
+
+ // { distance_to_target, { hood_angle, accelerator_power, finisher_power }}
+ InterpolationTable<ShotParams>
+ shot_interpolation_table;
};
// Creates (once) a Values instance for ::aos::network::GetTeamNumber() and
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c408f67..91e782e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,11 +64,40 @@
const flatbuffers::Offset<AimerStatus> aimer_status_offset =
aimer_.PopulateStatus(status->fbb());
+ const double distance_to_goal = aimer_.DistanceToGoal();
+
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+ hood_goal;
+ aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;
+
+ constants::Values::ShotParams shot_params;
+ if (constants::GetValues().shot_interpolation_table.GetInRange(
+ distance_to_goal, &shot_params)) {
+ hood_goal.Finish(frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *hood_goal.fbb(), shot_params.hood_angle));
+
+ shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
+ shot_params.accelerator_power,
+ shot_params.finisher_power));
+ } else {
+ hood_goal.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *hood_goal.fbb(), constants::GetValues().hood.range.upper));
+
+ shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(), 0.0, 0.0));
+ }
+
OutputT output_struct;
flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
hood_status_offset = hood_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+ unsafe_goal != nullptr
+ ? (unsafe_goal->hood_tracking() ? &hood_goal.message()
+ : unsafe_goal->hood())
+ : nullptr,
position->hood(),
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
@@ -109,6 +138,7 @@
? aimer_.TurretGoal()
: unsafe_goal->turret())
: nullptr;
+
flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
turret_status_offset = turret_.Iterate(
turret_goal, position->turret(),
@@ -117,7 +147,10 @@
flatbuffers::Offset<ShooterStatus> shooter_status_offset =
shooter_.RunIteration(
- unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+ unsafe_goal != nullptr
+ ? (unsafe_goal->shooter_tracking() ? &shooter_goal.message()
+ : unsafe_goal->shooter())
+ : nullptr,
position->shooter(), status->fbb(),
output != nullptr ? &(output_struct) : nullptr, position_timestamp);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 7b2b8ad..46cf18a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -909,7 +909,19 @@
class SuperstructureAllianceTest
: public SuperstructureTest,
- public ::testing::WithParamInterface<aos::Alliance> {};
+ public ::testing::WithParamInterface<aos::Alliance> {
+ protected:
+ void SendAlliancePosition() {
+ auto builder = joystick_state_sender_.MakeBuilder();
+
+ aos::JoystickState::Builder joystick_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+
+ joystick_builder.add_alliance(GetParam());
+
+ ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
+ }
+};
// Tests that the turret switches to auto-aiming when we set turret_tracking to
// true.
@@ -921,16 +933,7 @@
WaitUntilZeroed();
constexpr double kShotAngle = 1.0;
- {
- auto builder = joystick_state_sender_.MakeBuilder();
-
- aos::JoystickState::Builder joystick_builder =
- builder.MakeBuilder<aos::JoystickState>();
-
- joystick_builder.add_alliance(GetParam());
-
- ASSERT_TRUE(builder.Send(joystick_builder.Finish()));
- }
+ SendAlliancePosition();
{
auto builder = superstructure_goal_sender_.MakeBuilder();
@@ -975,6 +978,187 @@
superstructure_status_fetcher_->aimer()->turret_velocity());
}
+
+
+// Test a manual goal
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationManualGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+
+ builder.Send(goal_builder.Finish());
+ }
+
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 400.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 400.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 500.0);
+ EXPECT_NEAR(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().lower, 0.001);
+}
+
+
+// Test an out of range value with auto tracking
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationOutOfRange) {
+ SetEnabled(true);
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+ WaitUntilZeroed();
+ constexpr double kShotAngle = 1.0;
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 100m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle) * 100);
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle) * 100);
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ // Add a goal, this should be ignored with auto tracking
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_shooter_tracking(true);
+ goal_builder.add_hood_tracking(true);
+
+ builder.Send(goal_builder.Finish());
+ }
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_DOUBLE_EQ(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 0.0);
+ EXPECT_NEAR(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().upper, 0.001);
+
+}
+
+
+
+TEST_P(SuperstructureAllianceTest, ShooterInterpolationInRange) {
+ SetEnabled(true);
+ const frc971::control_loops::Pose target = turret::OuterPortPose(GetParam());
+ WaitUntilZeroed();
+ constexpr double kShotAngle = 1.0;
+
+ SendAlliancePosition();
+
+ // Test an in range value returns a reasonable result
+ {
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::LocalizerState::Builder
+ localizer_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::LocalizerState>();
+ localizer_builder.add_left_velocity(0.0);
+ localizer_builder.add_right_velocity(0.0);
+ const auto localizer_offset = localizer_builder.Finish();
+
+ DrivetrainStatus::Builder status_builder =
+ builder.MakeBuilder<DrivetrainStatus>();
+
+ // Set the robot up at kShotAngle off from the target, 2.5m away.
+ status_builder.add_x(target.abs_pos().x() + std::cos(kShotAngle) * 2.5);
+ status_builder.add_y(target.abs_pos().y() + std::sin(kShotAngle) * 2.5);
+ status_builder.add_theta(0.0);
+ status_builder.add_localizer(localizer_offset);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
+ }
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ // Add a goal, this should be ignored with auto tracking
+ auto shooter_goal = CreateShooterGoal(*builder.fbb(), 400.0, 500.0);
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kHoodRange().lower);
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter(shooter_goal);
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_shooter_tracking(true);
+ goal_builder.add_hood_tracking(true);
+
+ builder.Send(goal_builder.Finish());
+ }
+ RunFor(chrono::seconds(10));
+
+ superstructure_status_fetcher_.Fetch();
+
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->accelerator_left()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->accelerator_right()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->shooter()
+ ->finisher()
+ ->angular_velocity_goal(),
+ 100.0);
+ EXPECT_GE(superstructure_status_fetcher_->hood()->position(),
+ constants::Values::kHoodRange().lower);
+}
+
INSTANTIATE_TEST_CASE_P(ShootAnyAlliance, SuperstructureAllianceTest,
::testing::Values(aos::Alliance::kRed,
aos::Alliance::kBlue,