Tune turret for real
Add in friction compensation and tune the controller a bit. Fix a poor
velocity estimate.
Change-Id: I153b659bf92d5f34f0e8f59fda34e3c693e41d6a
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 9a6c3cd..b9ba172 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,6 +64,17 @@
climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
+ const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+ GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+ if (output != nullptr) {
+ // Friction is a pain and putting a really high burden on the integrator.
+ double velocity_sign = turret_status->velocity() * kTurretFrictionGain;
+ output_struct.turret_voltage +=
+ std::clamp(velocity_sign, -kTurretFrictionVoltageLimit,
+ kTurretFrictionVoltageLimit);
+ }
+
bool zeroed;
bool estopped;
@@ -74,9 +85,6 @@
const AbsoluteEncoderProfiledJointStatus *const intake_status =
GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
- const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
- GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
-
zeroed = hood_status->zeroed() && intake_status->zeroed() &&
turret_status->zeroed();
estopped = hood_status->estopped() || intake_status->estopped() ||
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index d8ce917..d5f4235 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -21,6 +21,11 @@
explicit Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name = "/superstructure");
+ // Terms to control the velocity gain for the friction compensation, and the
+ // voltage cap.
+ static constexpr double kTurretFrictionGain = 10.0;
+ static constexpr double kTurretFrictionVoltageLimit = 1.5;
+
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
diff --git a/y2020/control_loops/superstructure/superstructure_goal.fbs b/y2020/control_loops/superstructure/superstructure_goal.fbs
index f066ab0..c7fe643 100644
--- a/y2020/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2020/control_loops/superstructure/superstructure_goal.fbs
@@ -27,9 +27,7 @@
// Positive is rollers intaking to Washing Machine.
roller_voltage:float;
- // 0 = facing the front of the robot. Positive rotates counterclockwise.
- // TODO(Kai): Define which wrap of the shooter is 0 if it can rotate past
- // forward more than once.
+ // 0 = facing the back of the robot. Positive rotates counterclockwise.
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal;
// Only applies if shooter_tracking = false.
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index f68db08..f74b111 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -243,9 +243,14 @@
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
intake_plant_->voltage_offset();
+ const double turret_velocity_sign =
+ turret_plant_->X(1) * Superstructure::kTurretFrictionGain;
::Eigen::Matrix<double, 1, 1> turret_U;
turret_U << superstructure_output_fetcher_->turret_voltage() +
- turret_plant_->voltage_offset();
+ turret_plant_->voltage_offset() -
+ std::clamp(turret_velocity_sign,
+ -Superstructure::kTurretFrictionVoltageLimit,
+ Superstructure::kTurretFrictionVoltageLimit);
::Eigen::Matrix<double, 1, 1> accelerator_left_U;
accelerator_left_U