Update limit checks and unit testing for PowerToPosition in shooter
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 9cf563e..cc354fa 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -116,10 +116,31 @@
cycles_not_moved_(0) {}
double ShooterMotor::PowerToPosition(double power) {
- // LOG(WARNING, "power to position not correctly implemented\n");
const frc971::constants::Values &values = constants::GetValues();
- double new_pos = kMaxExtension - sqrt((power + power) / kSpringConstant);
- new_pos = ::std::min(::std::max(power, values.shooter.lower_limit),
+ double maxpower = 0.5 * kSpringConstant *
+ (kMaxExtension * kMaxExtension -
+ (kMaxExtension - values.shooter.upper_limit) *
+ (kMaxExtension - values.shooter.upper_limit));
+ if (power < 0) {
+ LOG(ERROR, "Power too low giving minimum (%f) (%f).\n", power,
+ 0.0);
+ power = 0;
+ } else if (power > maxpower) {
+ LOG(ERROR, "Power too high giving maximum (%f) (%f).\n", power,
+ maxpower);
+ power = maxpower;
+ }
+
+ double mp = kMaxExtension * kMaxExtension - (power + power) / kSpringConstant;
+ double new_pos = 0.10;
+ if (mp < 0) {
+ LOG(ERROR,
+ "Power calculation has negative number before square root (%f).\n", mp);
+ } else {
+ new_pos = kMaxExtension - ::std::sqrt(mp);
+ }
+
+ new_pos = ::std::min(::std::max(new_pos, values.shooter.lower_limit),
values.shooter.upper_limit);
return new_pos;
}
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index af523a9..59ef4f2 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -351,16 +351,17 @@
};
TEST_F(ShooterTest, PowerConversion) {
- // test a couple of values return the right thing
- EXPECT_EQ(0.021, shooter_motor_.PowerToPosition(0.021));
- EXPECT_EQ(0.175, shooter_motor_.PowerToPosition(0.175));
-
const frc971::constants::Values &values = constants::GetValues();
+ // test a couple of values return the right thing
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(0.014), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.000053),0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(0.007367),0.00001);
+
// value too large should get max
- EXPECT_EQ(values.shooter.upper_limit,
+ EXPECT_EQ(shooter_motor_.PowerToPosition(values.shooter.upper_limit),
shooter_motor_.PowerToPosition(505050.99));
// negative values should zero
- EXPECT_EQ(values.shooter.lower_limit, shooter_motor_.PowerToPosition(-123.4));
+ EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
}
// Tests that the wrist zeros correctly and goes to a position.