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/constants.cc b/y2020/constants.cc
index f0610a0..7fe24bf 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -107,10 +107,10 @@
intake->zeroing_constants.measured_absolute_position =
1.42977866919024 - Values::kIntakeZero();
- turret->potentiometer_offset =
- 5.52519370141247 + 0.00853506822980376 + 0.0109413725126625;
+ turret->potentiometer_offset = 5.52519370141247 + 0.00853506822980376 +
+ 0.0109413725126625 - 0.223719825811759;
turret_params->zeroing_constants.measured_absolute_position =
- 0.251065633255048;
+ 0.547478339799516;
break;
case kPracticeTeamNumber:
diff --git a/y2020/control_loops/python/turret.py b/y2020/control_loops/python/turret.py
index e276457..5a27afb 100644
--- a/y2020/control_loops/python/turret.py
+++ b/y2020/control_loops/python/turret.py
@@ -22,12 +22,12 @@
name='Turret',
motor=control_loop.Vex775Pro(),
G=(6.0 / 60.0) * (26.0 / 150.0),
- J=0.11,
- q_pos=0.40,
- q_vel=7.0,
+ J=0.20,
+ q_pos=0.30,
+ q_vel=4.5,
kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=3.0,
+ kalman_q_vel=10.0,
+ kalman_q_voltage=12.0,
kalman_r_position=0.05)
def main(argv):
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