Updated the shooter loop to simplify it and add more tests. Fixed the poles to be close to last year.
diff --git a/frc971/control_loops/python/shooter.py b/frc971/control_loops/python/shooter.py
index ab7b163..663a6b2 100755
--- a/frc971/control_loops/python/shooter.py
+++ b/frc971/control_loops/python/shooter.py
@@ -9,11 +9,11 @@
def __init__(self):
super(Shooter, self).__init__("Shooter")
# Stall Torque in N m
- self.stall_torque = 0.49819248
+ self.stall_torque = 0.49819248
# Stall Current in Amps
- self.stall_current = 85
+ self.stall_current = 85
# Free Speed in RPM
- self.free_speed = 19300.0
+ self.free_speed = 19300.0
# Free Current in Amps
self.free_current = 1.4
# Moment of inertia of the shooter wheel in kg m^2
@@ -21,7 +21,7 @@
# Resistance of the motor, divided by 2 to account for the 2 motors
self.R = 12.0 / self.stall_current / 2
# Motor velocity constant
- self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
+ self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(12.0 - self.R * self.free_current))
# Torque constant
self.Kt = self.stall_torque / self.stall_current
@@ -31,19 +31,19 @@
self.dt = 0.01
# State feedback matrices
- self.A_continuous = numpy.matrix(
+ self.A_continuous = numpy.matrix(
[[0, 1],
[0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
- self.B_continuous = numpy.matrix(
+ self.B_continuous = numpy.matrix(
[[0],
[self.Kt / (self.J * self.G * self.R)]])
- self.C = numpy.matrix([[1, 0]])
- self.D = numpy.matrix([[0]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
- self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
+ self.ContinuousToDiscrete(self.A_continuous, self.B_continuous,
self.dt, self.C)
- self.PlaceControllerPoles([.2, .15])
+ self.PlaceControllerPoles([.6, .981])
self.rpl = .45
self.ipl = 0.07
@@ -69,50 +69,57 @@
shooter = Shooter()
close_loop_x = []
close_loop_U = []
- velocity_goal = 1050.0
+ velocity_goal = 300
R = numpy.matrix([[0.0], [velocity_goal]])
for _ in pylab.linspace(0,1.99,200):
# Iterate the position up.
R = numpy.matrix([[R[0, 0] + 10.5], [velocity_goal]])
# Prevents the position goal from going beyond what is necessary.
velocity_weight_scalar = 0.35
- max_reference = ((shooter.U_max[0, 0] - velocity_weight_scalar *
- (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) / shooter.K[0, 0]
- + shooter.X_hat[0, 0])
- min_reference = ((shooter.U_min[0, 0] - velocity_weight_scalar *
- (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) / shooter.K[0, 0]
- + shooter.X_hat[0, 0])
- R[0, 0] = max(min(R[0, 0], max_reference), min_reference)
- U = numpy.clip(shooter.K * (R - shooter.X_hat), shooter.U_min, shooter.U_max)
+ max_reference = (
+ (shooter.U_max[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ min_reference = (
+ (shooter.U_min[0, 0] - velocity_weight_scalar *
+ (velocity_goal - shooter.X_hat[1, 0]) * shooter.K[0, 1]) /
+ shooter.K[0, 0] +
+ shooter.X_hat[0, 0])
+ R[0, 0] = numpy.clip(R[0, 0], min_reference, max_reference)
+ U = numpy.clip(shooter.K * (R - shooter.X_hat),
+ shooter.U_min, shooter.U_max)
shooter.UpdateObserver(U)
shooter.Update(U)
close_loop_x.append(shooter.X[1, 0])
close_loop_U.append(U[0, 0])
-# pylab.plotfile("shooter.csv", (0,1))
-# pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
-# pylab.plotfile("shooter.csv", (0,2))
+ #pylab.plotfile("shooter.csv", (0,1))
+ #pylab.plot(pylab.linspace(0,1.99,200), close_loop_U, 'ro')
+ #pylab.plotfile("shooter.csv", (0,2))
pylab.plot(pylab.linspace(0,1.99,200), close_loop_x, 'ro')
pylab.show()
# Simulate spin down.
spin_down_x = [];
- R = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[50.0], [0.0]])
for _ in xrange(150):
U = 0
shooter.UpdateObserver(U)
shooter.Update(U)
spin_down_x.append(shooter.X[1, 0])
-# pylab.plot(range(150), spin_down_x)
-# pylab.show()
+ #pylab.plot(range(150), spin_down_x)
+ #pylab.show()
-
- if len(argv) != 3:
- print "Expected .cc file name and .h file name"
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
else:
- shooter.DumpHeaderFile(argv[1])
- shooter.DumpCppFile(argv[2], argv[1])
+ if argv[1][-3:] == '.cc':
+ print '.cc file is second'
+ else:
+ shooter.DumpHeaderFile(argv[1])
+ shooter.DumpCppFile(argv[2], argv[1])
if __name__ == '__main__':
diff --git a/frc971/control_loops/shooter.cc b/frc971/control_loops/shooter.cc
index 102032e..3cb2415 100644
--- a/frc971/control_loops/shooter.cc
+++ b/frc971/control_loops/shooter.cc
@@ -10,96 +10,84 @@
namespace frc971 {
namespace control_loops {
-namespace {
-// Motor controllers have a range of PWM values where they don't trigger,
-// and we use these values to avoid that.
-// TODO: find these values for the Talons.
-static const double positive_deadband_power = 0.0;
-static const double negative_deadband_power = 0.0;
-}
-
ShooterMotor::ShooterMotor(control_loops::ShooterLoop *my_shooter)
: aos::control_loops::ControlLoop<control_loops::ShooterLoop>(my_shooter),
loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
history_position_(0),
position_goal_(0.0),
- time_(0.0) {
- memset(history, 0, sizeof(history));
- // Creates the header for the data file. Overwrites anything already there.
- loop_->StartDataFile("shooter.csv");
+ last_position_(0.0) {
+ memset(history_, 0, sizeof(history_));
}
+/*static*/ const double ShooterMotor::dt = 0.01;
+/*static*/ const double ShooterMotor::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
void ShooterMotor::RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
-
- bool bad_pos = false;
- if (position == NULL) {
- LOG(WARNING, "no position given\n");
- bad_pos = true;
- }
-
- const double velocity_goal = std::min(goal->velocity, max_speed);
- static double last_position = 0.0;
+ const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat[0] : position->position);
double output_voltage = 0.0;
- if (!bad_pos) {
- static bool first_time = true;
- if (first_time) {
- position_goal_ = position->position;
- first_time = false;
- }
-
- // Add position to the history.
- history[history_position_] = position->position;
- history_position_ = (history_position_ + 1) % kHistoryLength;
-
- loop_->Y << position->position;
- // Prevents the position from going out of the bounds
- // where the control loop will run the motor at full power.
- const double velocity_weight_scalar = 0.35;
- const double max_reference = (loop_->plant.U_max[0] - velocity_weight_scalar *
- (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
- / loop_->K[0] + loop_->X_hat[0];
- const double min_reference = (loop_->plant.U_min[0] - velocity_weight_scalar *
- (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
- / loop_->K[0] + loop_->X_hat[0];
- position_goal_ = std::max(std::min(position_goal_, max_reference), min_reference);
- loop_->R << position_goal_, velocity_goal;
- position_goal_ += velocity_goal * dt;
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
}
+
+ loop_->Y << current_position;
+
+ // Add the position to the history.
+ history_[history_position_] = current_position;
+ history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ // Prevents integral windup by limiting the position error such that the
+ // error can't produce much more than full power.
+ const double kVelocityWeightScalar = 0.35;
+ const double max_reference =
+ (loop_->plant.U_max[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+ const double min_reference =
+ (loop_->plant.U_min[0] - kVelocityWeightScalar *
+ (velocity_goal - loop_->X_hat[1]) * loop_->K[1])
+ / loop_->K[0] + loop_->X_hat[0];
+
+ position_goal_ = ::std::max(::std::min(position_goal_, max_reference),
+ min_reference);
+ loop_->R << position_goal_, velocity_goal;
+ position_goal_ += velocity_goal * dt;
- loop_->Update(!bad_pos, bad_pos || (output == NULL));
- // There is no need to make the motors actively assist the spin-down.
+ loop_->Update(position, output == NULL);
+
+ // Kill power at low velocity goals.
if (velocity_goal < 1.0) {
- output_voltage = 0.0;
- // Also, reset the position incrementer to avoid accumulating error.
- position_goal_ = position->position;
+ loop_->U[0] = 0.0;
} else {
- output_voltage = loop_->U[0] / 12.0;
+ output_voltage = loop_->U[0];
}
+
LOG(DEBUG,
"PWM: %f, raw_pos: %f rotations: %f "
"junk velocity: %f, xhat[0]: %f xhat[1]: %f, R[0]: %f R[1]: %f\n",
- output_voltage, position->position,
- position->position / (2 * M_PI),
- (position->position - last_position) / dt,
+ output_voltage, current_position,
+ current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
-// aos::DriverStationDisplay::Send(2, "RPS%3.0f(%3.0f) RPM%4.0f",
-// (position->position - last_position) * 100.0, goal->goal,
-// (position->position - last_position) / (2.0 * M_PI) * 6000.0);
-
- // Calculates the velocity over the last kHistoryLength*.001 seconds
+ // Calculates the velocity over the last kHistoryLength * .01 seconds
// by taking the difference between the current and next history positions.
int old_history_position = ((history_position_ == 0) ?
kHistoryLength : history_position_) - 1;
- average_velocity_ = (history[old_history_position]
- - history[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+ average_velocity_ = (history_[old_history_position] -
+ history_[history_position_]) * 100.0 / (double)(kHistoryLength - 1);
+
status->average_velocity = average_velocity_;
- // Determines if the velocity is close enough to the goal to be ready.
+
+ // Determine if the velocity is close enough to the goal to be ready.
if (std::abs(velocity_goal - average_velocity_) < 10.0 &&
velocity_goal != 0.0) {
LOG(DEBUG, "Steady: ");
@@ -110,34 +98,12 @@
}
LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
- // Deal with motor controller deadbands.
- if (output_voltage > 0) {
- output_voltage += positive_deadband_power;
- }
- if (output_voltage < 0) {
- output_voltage -= negative_deadband_power;
- }
+ last_position_ = current_position;
- if (bad_pos) {
- last_position = position->position;
- } else {
- // use the predicted position
- last_position = loop_->X_hat[0];
- }
if (output) {
output->voltage = output_voltage;
}
-
- // If anything is happening, record it. Otherwise, reset the time.
- // This should be removed once we are done with testing.
- if (output_voltage != 0 && average_velocity_ != 0 && velocity_goal != 0) {
- loop_->RecordDatum("shooter.csv", time_);
- time_ += dt;
- }
- else {
- time_ = 0;
- }
-} // RunIteration
+}
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/shooter.h b/frc971/control_loops/shooter.h
index f26c42d..7358de2 100644
--- a/frc971/control_loops/shooter.h
+++ b/frc971/control_loops/shooter.h
@@ -17,34 +17,33 @@
explicit ShooterMotor(
control_loops::ShooterLoop *my_shooter = &control_loops::shooter);
+ // Control loop time step.
+ static const double dt;
+
+ // Maximum speed of the shooter wheel which the encoder is rated for in
+ // rad/sec.
+ static const double kMaxSpeed;
+
protected:
virtual void RunIteration(
const control_loops::ShooterLoop::Goal *goal,
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status);
+
private:
- // Timestep of the control loop (in seconds)
- static constexpr double dt = 0.01;
-
- // Maximum speed of the shooter wheel which the encoder is rated for.
- // 10000 rpm * (2 * M_PI radians / rotation) / (60 sec / min) * 15 / 34
- static constexpr double max_speed = 461.998919646;
-
// The state feedback control loop to talk to.
::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
// History array and stuff for determining average velocity and whether
// we are ready to shoot.
- static const int kHistoryLength = 10;
- double history[kHistoryLength];
+ static const int kHistoryLength = 5;
+ double history_[kHistoryLength];
ptrdiff_t history_position_;
double average_velocity_;
double position_goal_;
-
- // time_ is used for recording data so that we have a time.
- double time_;
+ double last_position_;
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/shooter_lib_test.cc b/frc971/control_loops/shooter_lib_test.cc
index 63afb80..1a19cbc 100644
--- a/frc971/control_loops/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter_lib_test.cc
@@ -15,7 +15,7 @@
namespace control_loops {
namespace testing {
-// Class which simulates the shooter and sends out queue messages with the
+// Class which simulates the shooter and sends out queue messages with the
// position.
class ShooterMotorSimulation {
public:
@@ -28,19 +28,11 @@
".frc971.control_loops.shooter.position",
".frc971.control_loops.shooter.output",
".frc971.control_loops.shooter.status") {
- Reinitialize();
- }
-
- // Resets the shooter simulation.
- void Reinitialize() {
- shooter_plant_->X(0, 0) = 0;
- shooter_plant_->X(1, 0) = 0;
- shooter_plant_->Y = shooter_plant_->C * shooter_plant_->X;
}
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
+ ::aos::ScopedMessagePtr<control_loops::ShooterLoop::Position> position =
my_shooter_loop_.position.MakeMessage();
position->position = shooter_plant_->Y(0, 0);
position.Send();
@@ -49,36 +41,24 @@
// Simulates shooter for a single timestep.
void Simulate() {
EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
- shooter_plant_->U << my_shooter_loop_.output->voltage * 12;
+ shooter_plant_->U << my_shooter_loop_.output->voltage;
shooter_plant_->Update();
}
::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
private:
ShooterLoop my_shooter_loop_;
};
class ShooterTest : public ::testing::Test {
protected:
- // I haven't the faintest idea exactly what this line does,
- // but without it things break.
- ::aos::common::testing::GlobalCoreInstance my_core;
-
- // Create a new instance of the test queue "so that it invalidates the queue
- // that it points to Otherwise, we will have a pointed to
- // shared memory that is no longer valid."
- ShooterLoop my_shooter_loop_;
-
- // Create a control loop and simulation.
- ShooterMotor shooter_motor_;
- ShooterMotorSimulation shooter_motor_plant_;
-
ShooterTest() : my_shooter_loop_(".frc971.control_loops.shooter",
- 0x78d8e372,
+ 0x78d8e372,
".frc971.control_loops.shooter.goal",
".frc971.control_loops.shooter.position",
".frc971.control_loops.shooter.output",
- ".frc971.control_loops.shooter.status"),
+ ".frc971.control_loops.shooter.status"),
shooter_motor_(&my_shooter_loop_),
shooter_motor_plant_() {
// Flush the robot state queue so we can use clean shared memory for this
@@ -87,6 +67,10 @@
SendDSPacket(true);
}
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ }
+
// Update the robot state. Without this, the Iteration of the control loop
// will stop all the motors and the shooter won't go anywhere.
void SendDSPacket(bool enabled) {
@@ -104,9 +88,17 @@
10.0);
}
- virtual ~ShooterTest() {
- ::aos::robot_state.Clear();
- }
+ // Bring up and down Core.
+ ::aos::common::testing::GlobalCoreInstance my_core;
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointed to
+ // shared memory that is no longer valid.
+ ShooterLoop my_shooter_loop_;
+
+ // Create a control loop and simulation.
+ ShooterMotor shooter_motor_;
+ ShooterMotorSimulation shooter_motor_plant_;
};
// Tests that the shooter does nothing when the goal is zero.
@@ -117,6 +109,8 @@
shooter_motor_.Iterate();
shooter_motor_plant_.Simulate();
VerifyNearGoal();
+ my_shooter_loop_.output.FetchLatest();
+ EXPECT_EQ(my_shooter_loop_.output->voltage, 0.0);
}
// Tests that the shooter spins up to speed and that it then spins down
@@ -133,13 +127,16 @@
is_done = my_shooter_loop_.status->ready;
}
VerifyNearGoal();
+
my_shooter_loop_.goal.MakeWithBuilder().velocity(0.0).Send();
- shooter_motor_plant_.SendPositionMessage();
- shooter_motor_.Iterate();
- shooter_motor_plant_.Simulate();
- SendDSPacket(true);
- EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
- EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ for (int i = 0; i < 100; ++i) {
+ shooter_motor_plant_.SendPositionMessage();
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ EXPECT_TRUE(my_shooter_loop_.output.FetchLatest());
+ EXPECT_EQ(0.0, my_shooter_loop_.output->voltage);
+ }
}
// Test to make sure that it does not exceed the encoder's rated speed.
@@ -154,10 +151,41 @@
EXPECT_TRUE(my_shooter_loop_.status.FetchLatest());
is_done = my_shooter_loop_.status->ready;
}
+
my_shooter_loop_.goal.FetchLatest();
my_shooter_loop_.status.FetchLatest();
- EXPECT_FALSE(std::abs(my_shooter_loop_.goal->velocity -
- my_shooter_loop_.status->average_velocity) < 10.0);
+ EXPECT_LT(shooter_motor_.kMaxSpeed,
+ shooter_motor_plant_.shooter_plant_->X(1, 0));
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(true);
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ my_shooter_loop_.goal.MakeWithBuilder().velocity(200.0).Send();
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_motor_plant_.SendPositionMessage();
+ }
+ shooter_motor_.Iterate();
+ shooter_motor_plant_.Simulate();
+ SendDSPacket(i > 50);
+ }
+
+ VerifyNearGoal();
}
} // namespace testing
diff --git a/frc971/control_loops/shooter_main.cc b/frc971/control_loops/shooter_main.cc
index 94b8d59..513424c 100644
--- a/frc971/control_loops/shooter_main.cc
+++ b/frc971/control_loops/shooter_main.cc
@@ -2,4 +2,10 @@
#include "aos/aos_core.h"
-AOS_RUN_LOOP(frc971::control_loops::ShooterMotor);
+int main() {
+ ::aos::Init();
+ frc971::control_loops::ShooterMotor shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/control_loops/shooter_motor.q b/frc971/control_loops/shooter_motor.q
index 29a40de..f2abf25 100644
--- a/frc971/control_loops/shooter_motor.q
+++ b/frc971/control_loops/shooter_motor.q
@@ -6,15 +6,19 @@
implements aos.control_loops.ControlLoop;
message Goal {
+ // Goal velocity in rad/sec
double velocity;
};
message Status {
+ // True if the shooter is up to speed.
bool ready;
+ // The average velocity over the last 0.1 seconds.
double average_velocity;
};
message Position {
+ // The angle of the shooter wheel measured in rad/sec.
double position;
};
diff --git a/frc971/control_loops/shooter_motor_plant.cc b/frc971/control_loops/shooter_motor_plant.cc
index db11e7f..7736075 100644
--- a/frc971/control_loops/shooter_motor_plant.cc
+++ b/frc971/control_loops/shooter_motor_plant.cc
@@ -26,7 +26,7 @@
Eigen::Matrix<double, 2, 1> L;
L << 1.07156131086, 28.0940195016;
Eigen::Matrix<double, 1, 2> K;
- K << 43.5200653183, 0.819154156845;
+ K << 0.486400730028, 0.247515916371;
return StateFeedbackLoop<2, 1, 1>(L, K, MakeShooterPlant());
}