Added a PositionToPower function in the shooter.
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index ce64c53..a7f29c5 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -144,6 +144,11 @@
return new_pos;
}
+double ShooterMotor::PositionToPower(double position) {
+ double power = kSpringConstant * position * (kMaxExtension - position / 2.0);
+ return power;
+}
+
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
const control_loops::ShooterGroup::Goal *goal,
@@ -570,6 +575,8 @@
if (output) output->voltage = 0.0;
}
+ status->hard_stop_power = PowerToPosition(shooter_.absolute_position());
+
if (output) {
output->latch_piston = latch_piston_;
output->brake_piston = brake_piston_;