Fixed constants and moved them out of control_loop.py
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 51ce0ff..bda178e 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -10,7 +10,7 @@
self.formatToType['%f'] = "double";
self.formatToType['%d'] = "int";
def __str__ (self):
- return str("\nstatic const %s %s = "+ self.formatt +";\n") % \
+ return str("\nstatic constexpr %s %s = "+ self.formatt +";\n") % \
(self.formatToType[self.formatt], self.name, self.value)
@@ -39,9 +39,14 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
self._constant_list = []
- if (write_constants):
- self._constant_list.append(Constant("kMaxExtension", "%f", 0.32385));
- self._constant_list.append(Constant("kSpringConstant", "%f", 0.28));
+
+ def AddConstant(self, constant):
+ """Adds a constant to write.
+
+ Args:
+ constant: Constant, the constant to add to the header.
+ """
+ self._constant_list.append(constant)
def _TopDirectory(self):
return self._namespaces[0]
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index 6b8c224..69f2599 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -30,6 +30,9 @@
self.Kt = self.stall_torque / self.stall_current
# Spring constant for the springs, N/m
self.Ks = 2800.0
+ # Maximum extension distance (Distance from the 0 force point on the
+ # spring to the latch position.)
+ self.max_extension = 0.32385
# Gear ratio multiplied by radius of final sprocket.
self.G = 10.0 / 40.0 * 20.0 / 54.0 * 24.0 / 54.0 * 20.0 / 84.0 * 16.0 * (3.0 / 8.0) / (2.0 * numpy.pi) * 0.0254
@@ -236,8 +239,12 @@
sprung_shooter = SprungShooterDeltaU()
shooter = ShooterDeltaU()
loop_writer = control_loop.ControlLoopWriter("Shooter", [sprung_shooter,
- shooter],
- write_constants=True)
+ shooter])
+
+ loop_writer.AddConstant(control_loop.Constant("kMaxExtension", "%f",
+ sprung_shooter.max_extension))
+ loop_writer.AddConstant(control_loop.Constant("kSpringConstant", "%f",
+ sprung_shooter.Ks))
if argv[1][-3:] == '.cc':
loop_writer.Write(argv[2], argv[1])
else:
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();