Fixed constants and moved them out of control_loop.py
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 195894b..db40f43 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -97,7 +97,7 @@
private:
// The offset between what is '0' (0 rate on the spring) and the 0 (all the
// way cocked).
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
// The accumulated voltage to apply to the motor.
double voltage_;
double last_voltage_;
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index 4fc5e23..c171912 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -50,7 +50,7 @@
// The difference between the position with 0 at the back, and the position
// with 0 measured where the spring has 0 force.
- constexpr static double kPositionOffset = 0.305054 + 0.0254;
+ constexpr static double kPositionOffset = kMaxExtension;
void Reinitialize(double initial_position) {
LOG(INFO, "Reinitializing to {pos: %f}\n", initial_position);
@@ -353,20 +353,21 @@
TEST_F(ShooterTest, PowerConversion) {
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);
+ EXPECT_NEAR(0.254001, shooter_motor_.PowerToPosition(140.0), 0.00001);
+ EXPECT_NEAR(0.00058, shooter_motor_.PowerToPosition(0.53), 0.00001);
+ EXPECT_NEAR(0.095251238129837101, shooter_motor_.PowerToPosition(73.67),
+ 0.00001);
// value too large should get max
- EXPECT_EQ(shooter_motor_.PowerToPosition(values.shooter.upper_limit),
- shooter_motor_.PowerToPosition(505050.99));
+ EXPECT_NEAR(values.shooter.upper_limit,
+ shooter_motor_.PowerToPosition(505050.99), 0.00001);
// negative values should zero
EXPECT_NEAR(0, shooter_motor_.PowerToPosition(-123.4), 0.00001);
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, GoesToValue) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -375,13 +376,15 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Fire) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 120; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -390,7 +393,7 @@
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
shooter_queue_group_.goal.MakeWithBuilder()
- .shot_power(0.0035)
+ .shot_power(35.0)
.shot_requested(true)
.Send();
@@ -407,7 +410,7 @@
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
shooter_queue_group_.goal.MakeWithBuilder()
- .shot_power(0.0017)
+ .shot_power(17.0)
.shot_requested(false)
.Send();
}
@@ -416,7 +419,9 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_preparefire);
EXPECT_TRUE(hit_fire);
@@ -424,7 +429,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, FireLong) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -464,7 +469,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, MoveGoal) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -472,7 +477,7 @@
SendDSPacket(true);
}
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.0014).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(14.0).Send();
for (int i = 0; i < 100; ++i) {
shooter_motor_plant_.SendPositionMessage();
@@ -482,14 +487,16 @@
}
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, Unload) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -515,7 +522,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupNegative) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -555,7 +562,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, UnloadWindupPositive) {
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 150; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -600,7 +607,7 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ShooterTest, StartsOnDistal) {
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -609,7 +616,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -618,7 +627,7 @@
TEST_F(ShooterTest, StartsOnProximal) {
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.007).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(70.0).Send();
for (int i = 0; i < 300; ++i) {
shooter_motor_plant_.SendPositionMessage();
shooter_motor_.Iterate();
@@ -627,7 +636,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power), pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -645,7 +656,7 @@
// latch, brake, plunger_back, start_pos);
bool initialized = false;
Reinitialize(start_pos);
- shooter_queue_group_.goal.MakeWithBuilder().shot_power(0.25).Send();
+ shooter_queue_group_.goal.MakeWithBuilder().shot_power(120.0).Send();
for (int i = 0; i < 200; ++i) {
shooter_motor_plant_.SendPositionMessage(!initialized, plunger_back, latch, brake);
initialized = true;
@@ -655,7 +666,9 @@
}
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
- ASSERT_NEAR(shooter_queue_group_.goal->shot_power, pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_queue_group_.goal->shot_power),
+ pos, 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
diff --git a/frc971/control_loops/shooter/shooter_motor_plant.h b/frc971/control_loops/shooter/shooter_motor_plant.h
index 40a2f25..606395a 100644
--- a/frc971/control_loops/shooter/shooter_motor_plant.h
+++ b/frc971/control_loops/shooter/shooter_motor_plant.h
@@ -5,9 +5,9 @@
namespace frc971 {
namespace control_loops {
-static const double kMaxExtension = 0.323850;
+static constexpr double kMaxExtension = 0.323850;
-static const double kSpringConstant = 0.280000;
+static constexpr double kSpringConstant = 2800.000000;
StateFeedbackPlantCoefficients<3, 1, 1> MakeSprungShooterPlantCoefficients();