Import shooter from 2013
Taken from commit a085044
I liberally delete some things in the Python code to get
things compiling. I also added TODOs where I am unsure whether
my changes are appropriate.
Change-Id: I4f3ae9b69ca9c2b1917e73136c766275a44d3665
diff --git a/y2016/control_loops/python/BUILD b/y2016/control_loops/python/BUILD
index b424625..0134379 100644
--- a/y2016/control_loops/python/BUILD
+++ b/y2016/control_loops/python/BUILD
@@ -37,3 +37,15 @@
'//frc971/control_loops/python:controls',
],
)
+
+py_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ]
+)
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
new file mode 100644
index 0000000..f34a1f1
--- /dev/null
+++ b/y2016/control_loops/python/shooter.py
@@ -0,0 +1,72 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+import numpy
+import sys
+from matplotlib import pylab
+
+class Shooter(control_loop.ControlLoop):
+ def __init__(self):
+ super(Shooter, self).__init__("Shooter")
+ # Stall Torque in N m
+ self.stall_torque = 0.49819248
+ # Stall Current in Amps
+ self.stall_current = 85
+ # Free Speed in RPM
+ self.free_speed = 19300.0 - 1500.0
+ # Free Current in Amps
+ self.free_current = 1.4
+ # Moment of inertia of the shooter wheel in kg m^2
+ self.J = 0.0032
+ # 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) /
+ (12.0 - self.R * self.free_current))
+ # Torque constant
+ self.Kt = self.stall_torque / self.stall_current
+ # Gear ratio
+ self.G = 11.0 / 34.0
+ # Control loop time step
+ self.dt = 0.005
+
+ # State feedback matrices
+ 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(
+ [[0],
+ [self.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1, 0]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.6, .981])
+
+ self.rpl = .45
+ self.ipl = 0.07
+ self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+
+def main(argv):
+ if len(argv) != 3:
+ print "Expected .h file name and .cc file name"
+ else:
+ namespaces = ['y2016', 'control_loops', 'shooter']
+ shooter = Shooter()
+ loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+ namespaces=namespaces)
+ if argv[1][-3:] == '.cc':
+ loop_writer.Write(argv[2], argv[1])
+ else:
+ loop_writer.Write(argv[1], argv[2])
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
new file mode 100644
index 0000000..6c15f15
--- /dev/null
+++ b/y2016/control_loops/shooter/BUILD
@@ -0,0 +1,86 @@
+package(default_visibility = ['//visibility:public'])
+
+load('/aos/build/queues', 'queue_library')
+
+queue_library(
+ name = 'shooter_queue',
+ srcs = [
+ 'shooter.q',
+ ],
+ deps = [
+ '//aos/common/controls:control_loop_queues',
+ '//frc971/control_loops:queues',
+ ],
+)
+
+genrule(
+ name = 'genrule_shooter',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //y2016/control_loops/python:shooter) $(OUTS)',
+ tools = [
+ '//y2016/control_loops/python:shooter',
+ ],
+ outs = [
+ 'shooter_plant.h',
+ 'shooter_plant.cc',
+ ],
+)
+
+cc_library(
+ name = 'shooter_plants',
+ srcs = [
+ 'shooter_plant.cc',
+ ],
+ hdrs = [
+ 'shooter_plant.h',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
+
+cc_library(
+ name = 'shooter_lib',
+ srcs = [
+ 'shooter.cc',
+ ],
+ hdrs = [
+ 'shooter.h',
+ ],
+ deps = [
+ ':shooter_queue',
+ ':shooter_plants',
+ '//aos/common/controls:control_loop',
+ '//aos/common/util:log_interval',
+ '//aos/common/logging:queue_logging',
+ '//aos/common/logging:matrix_logging',
+ ],
+)
+
+cc_test(
+ name = 'shooter_lib_test',
+ srcs = [
+ 'shooter_lib_test.cc',
+ ],
+ deps = [
+ ':shooter_queue',
+ ':shooter_lib',
+ '//aos/testing:googletest',
+ '//aos/common:queues',
+ '//aos/common/controls:control_loop_test',
+ '//frc971/control_loops:state_feedback_loop',
+ '//frc971/control_loops:team_number_test_environment',
+ ],
+)
+
+cc_binary(
+ name = 'shooter',
+ srcs = [
+ 'shooter_main.cc',
+ ],
+ deps = [
+ '//aos/linux_code:init',
+ ':shooter_lib',
+ ':shooter_queue',
+ ],
+)
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
new file mode 100644
index 0000000..41e6f81
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -0,0 +1,138 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2016/control_loops/shooter/shooter_plant.h"
+
+namespace y2016 {
+namespace control_loops {
+
+Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter),
+ loop_(new StateFeedbackLoop<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterLoop())),
+ history_position_(0),
+ position_goal_(0.0),
+ last_position_(0.0),
+ last_velocity_goal_(0) {
+ memset(history_, 0, sizeof(history_));
+}
+
+/*static*/ const double Shooter::dt = 0.005;
+/*static*/ const double Shooter::kMaxSpeed =
+ 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
+
+void Shooter::RunIteration(
+ const control_loops::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterQueue::Status *status) {
+ double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ const double current_position =
+ (position == NULL ? loop_->X_hat(0, 0) : position->position);
+ double output_voltage = 0.0;
+
+ // TODO(phil): Set a queue to trigger a shot. For now, disable the fetch from
+ // the queue.
+ if (false) {
+ // if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ // if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ && velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }
+ last_velocity_goal_ = velocity_goal;
+
+ // Track the current position if the velocity goal is small.
+ if (velocity_goal <= 1.0) {
+ position_goal_ = current_position;
+ }
+
+ // TODO(phil): Does this change make sense?
+ // loop_->Y << current_position;
+ Eigen::Matrix<double, 1, 1> Y;
+ Y << current_position;
+ loop_->Correct(Y);
+
+ // 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_->U_max(0, 0) -
+ kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
+ loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+ const double min_reference =
+ (loop_->U_min(0, 0) -
+ kVelocityWeightScalar * (velocity_goal - loop_->X_hat(1, 0)) *
+ loop_->K(0, 1)) /
+ loop_->K(0, 0) +
+ loop_->X_hat(0, 0);
+
+ position_goal_ =
+ ::std::max(::std::min(position_goal_, max_reference), min_reference);
+ // TODO(phil): Does this change make sense?
+ // loop_->R << position_goal_, velocity_goal;
+ loop_->mutable_R(0, 0) = position_goal_;
+ loop_->mutable_R(1, 0) = velocity_goal;
+ position_goal_ += velocity_goal * dt;
+
+ // TODO(phil): Does this change make sense?
+ // loop_->Update(position, output == NULL);
+ loop_->Update(output == NULL);
+
+ // Kill power at low velocity goals.
+ if (velocity_goal < 1.0) {
+ loop_->mutable_U(0, 0) = 0.0;
+ } else {
+ output_voltage = loop_->U(0, 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, current_position, current_position / (2 * M_PI),
+ (current_position - last_position_) / dt,
+ // TODO(phil): Does this change make sense?
+ // loop_->X_hat[0], loop_->X_hat[1], loop_->R[0], loop_->R[1]);
+ loop_->X_hat(0, 0), loop_->X_hat(1, 0), loop_->R(0, 0), loop_->R(1, 0));
+
+ // 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_]) / dt /
+ (double)(kHistoryLength - 1);
+
+ status->average_velocity = average_velocity_;
+
+ // 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: ");
+ status->ready = true;
+ } else {
+ LOG(DEBUG, "Not ready: ");
+ status->ready = false;
+ }
+ LOG(DEBUG, "avg = %f goal = %f\n", average_velocity_, velocity_goal);
+
+ last_position_ = current_position;
+
+ if (output) {
+ output->voltage = output_voltage;
+ }
+}
+
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
new file mode 100644
index 0000000..766845d
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.h
@@ -0,0 +1,58 @@
+#ifndef Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
+
+namespace y2016 {
+namespace control_loops {
+
+class Shooter
+ : public ::aos::controls::ControlLoop<control_loops::ShooterQueue> {
+ public:
+ explicit Shooter(
+ control_loops::ShooterQueue *my_shooter = &control_loops::shooter_queue);
+
+ // 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::ShooterQueue::Goal *goal,
+ const control_loops::ShooterQueue::Position *position,
+ ::aos::control_loops::Output *output,
+ control_loops::ShooterQueue::Status *status);
+
+ private:
+ // 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 = 5;
+ double history_[kHistoryLength];
+ ptrdiff_t history_position_;
+ double average_velocity_;
+
+ double position_goal_;
+ double last_position_;
+
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace control_loops
+} // namespace y2016
+
+#endif // Y2016_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
new file mode 100644
index 0000000..57d54f9
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter.q
@@ -0,0 +1,32 @@
+package y2016.control_loops;
+
+import "aos/common/controls/control_loops.q";
+import "frc971/control_loops/control_loops.q";
+
+queue_group ShooterQueue {
+ 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;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue aos.control_loops.Output output;
+ queue Status status;
+};
+
+queue_group ShooterQueue shooter_queue;
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
new file mode 100644
index 0000000..cf5a810
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -0,0 +1,193 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "aos/common/queue.h"
+#include "aos/common/controls/control_loop_test.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+
+using ::aos::time::Time;
+using ::frc971::control_loops::testing::kTeamNumber;
+
+namespace y2016 {
+namespace control_loops {
+namespace testing {
+
+// Class which simulates the shooter and sends out queue messages with the
+// position.
+class ShooterSimulation {
+ public:
+ // Constructs a shooter simulation. I'm not sure how the construction of
+ // the queue (shooter_queue_) actually works (same format as wrist).
+ ShooterSimulation()
+ : shooter_plant_(new StateFeedbackPlant<2, 1, 1>(
+ ::y2016::control_loops::shooter::MakeShooterPlant())),
+ shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
+ ".y2016.control_loops.shooter.goal",
+ ".y2016.control_loops.shooter.position",
+ ".y2016.control_loops.shooter.output",
+ ".y2016.control_loops.shooter.status") {}
+
+ // Sends a queue message with the position of the shooter.
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<control_loops::ShooterQueue::Position> position =
+ shooter_queue_.position.MakeMessage();
+
+ position->position = shooter_plant_->Y(0, 0);
+ position.Send();
+ }
+
+ // Simulates shooter for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+
+ shooter_plant_->mutable_U(0, 0) = shooter_queue_.output->voltage;
+ shooter_plant_->Update();
+ }
+
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_;
+
+ private:
+ ShooterQueue shooter_queue_;
+};
+
+class ShooterTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ ShooterTest()
+ : shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
+ ".y2016.control_loops.shooter.goal",
+ ".y2016.control_loops.shooter.position",
+ ".y2016.control_loops.shooter.output",
+ ".y2016.control_loops.shooter.status"),
+ shooter_(&shooter_queue_),
+ shooter_plant_() {
+ // Flush the robot state queue so we can use clean shared memory for this
+ // test.
+ set_team_id(kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ EXPECT_NEAR(shooter_queue_.goal->velocity,
+ shooter_queue_.status->average_velocity, 10.0);
+ }
+
+ // Runs one iteration of the whole simulation and checks that separation
+ // remains reasonable.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ shooter_plant_.SendPositionMessage();
+ shooter_.Iterate();
+ shooter_plant_.Simulate();
+
+ TickTime();
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const Time &run_for, bool enabled = true) {
+ const auto start_time = Time::Now();
+ while (Time::Now() < start_time + run_for) {
+ RunIteration(enabled);
+ }
+ }
+
+ // 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.
+ ShooterQueue shooter_queue_;
+
+ // Create a control loop and simulation.
+ Shooter shooter_;
+ ShooterSimulation shooter_plant_;
+};
+
+// Tests that the shooter does nothing when the goal is zero.
+TEST_F(ShooterTest, DoesNothing) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(0.0).Send());
+ RunIteration();
+
+ VerifyNearGoal();
+
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(shooter_queue_.output->voltage, 0.0);
+}
+
+// Tests that the shooter spins up to speed and that it then spins down
+// without applying any power.
+TEST_F(ShooterTest, SpinUpAndDown) {
+ // Spin up.
+ EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(450.0).Send());
+ RunForTime(Time::InSeconds(10));
+ VerifyNearGoal();
+ EXPECT_TRUE(shooter_queue_.status->ready);
+ EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(0.0).Send());
+
+ // Make sure we don't apply voltage on spin-down.
+ RunIteration();
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage);
+
+ // Continue to stop.
+ RunForTime(Time::InSeconds(5));
+ EXPECT_TRUE(shooter_queue_.output.FetchLatest());
+ EXPECT_EQ(0.0, shooter_queue_.output->voltage);
+}
+
+// Test to make sure that it does not exceed the encoder's rated speed.
+TEST_F(ShooterTest, RateLimitTest) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(1000.0).Send());
+
+ RunForTime(Time::InSeconds(10));
+ EXPECT_TRUE(shooter_queue_.status.FetchLatest());
+ EXPECT_TRUE(shooter_queue_.status->ready);
+
+ shooter_queue_.goal.FetchLatest();
+ shooter_queue_.status.FetchLatest();
+
+ EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
+
+ EXPECT_GT(shooter_.kMaxSpeed, shooter_queue_.status->average_velocity);
+}
+
+// Tests that the shooter can spin up nicely while missing position packets.
+TEST_F(ShooterTest, MissingPositionMessages) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(200.0).Send());
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_plant_.SendPositionMessage();
+ }
+
+ RunIteration();
+ }
+
+ VerifyNearGoal();
+}
+
+// Tests that the shooter can spin up nicely while disabled for a while.
+TEST_F(ShooterTest, Disabled) {
+ ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder().velocity(200.0).Send());
+ for (int i = 0; i < 100; ++i) {
+ if (i % 7) {
+ shooter_plant_.SendPositionMessage();
+ }
+
+ RunIteration();
+ }
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace control_loops
+} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
new file mode 100644
index 0000000..911ab29
--- /dev/null
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -0,0 +1,11 @@
+#include "y2016/control_loops/shooter/shooter.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2016::control_loops::Shooter shooter;
+ shooter.Run();
+ ::aos::Cleanup();
+ return 0;
+}