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_;
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 742bb0c..9a130bc 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -125,6 +125,7 @@
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
+ double PositionToPower(double position);
typedef enum {
STATE_INITIALIZE = 0,
diff --git a/frc971/control_loops/shooter/shooter.q b/frc971/control_loops/shooter/shooter.q
index fd2ddd4..7475873 100755
--- a/frc971/control_loops/shooter/shooter.q
+++ b/frc971/control_loops/shooter/shooter.q
@@ -46,6 +46,9 @@
// How many times we've shot.
int32_t shots;
bool done;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ double hard_stop_power;
};
queue Goal goal;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 99ecb78..afc63a9 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -361,6 +361,22 @@
EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
}
+// Test that PowerToPosition and PositionToPower are inverses of each other.
+// Note that PowerToPosition will cap position whereas PositionToPower will not
+// cap power.
+TEST_F(ShooterTest, InversePowerConversion) {
+ // Test a few values.
+ double power = 140.0;
+ double position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = .53;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+ power = 71.971;
+ position = shooter_motor_.PowerToPosition(power);
+ EXPECT_NEAR(power, shooter_motor_.PositionToPower(position), 1e-5);
+}
+
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();