Add dither to y2020 turret
Basically just duplicate what Austin did with the hood a while back.
Change-Id: I7df9a45056de1dfca878402013facd3349453825
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index ce816a0..98de31d 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -164,11 +164,16 @@
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
+ // TODO(james): I'm not sure how helpful this gain is.
const double turret_velocity_sign =
turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
+ const double time_sec =
+ aos::time::DurationInSeconds(position_timestamp.time_since_epoch());
+ output_struct.turret_voltage +=
+ kTurretDitherGain * std::sin(2.0 * M_PI * time_sec * 30.0);
output_struct.turret_voltage =
std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
turret_.operating_voltage());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index c03f7a7..e4a9cb6 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -28,6 +28,7 @@
// voltage cap.
static constexpr double kTurretFrictionGain = 10.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
+ static constexpr double kTurretDitherGain = 0.4;
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 22dd40e..112780a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -742,7 +742,7 @@
superstructure_plant_.set_peak_intake_acceleration(0.2);
superstructure_plant_.set_peak_turret_velocity(23.0);
- superstructure_plant_.set_peak_turret_acceleration(0.2);
+ superstructure_plant_.set_peak_turret_acceleration(6.0);
// Intake needs over 9 seconds to reach the goal
RunFor(chrono::seconds(10));
@@ -971,7 +971,7 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(kShotAngle, superstructure_status_fetcher_->turret()->position(),
- 1e-4);
+ 5e-4);
EXPECT_FLOAT_EQ(kShotAngle,
superstructure_status_fetcher_->aimer()->turret_position());
EXPECT_FLOAT_EQ(0,