Designed a shooter loop using voltage error estimation.
This gets rid of all the corner cases with the moving position style
loop from before, and doesn't have overshoot issues like standard
integral loops have.
Change-Id: I4e4eb1767038563cf851040ce8218e73ca60904a
diff --git a/y2016/control_loops/python/shooter.py b/y2016/control_loops/python/shooter.py
index c81d597..3bb22a9 100755
--- a/y2016/control_loops/python/shooter.py
+++ b/y2016/control_loops/python/shooter.py
@@ -1,13 +1,21 @@
#!/usr/bin/python
from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
import numpy
import sys
from matplotlib import pylab
-class Shooter(control_loop.ControlLoop):
- def __init__(self):
- super(Shooter, self).__init__("Shooter")
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+class VelocityShooter(control_loop.ControlLoop):
+ def __init__(self, name='VelocityShooter'):
+ super(VelocityShooter, self).__init__(name)
# Stall Torque in N m
self.stall_torque = 0.71
# Stall Current in Amps
@@ -31,43 +39,243 @@
self.dt = 0.005
# State feedback matrices
- # [position, angular velocity]
+ # [angular velocity]
self.A_continuous = numpy.matrix(
- [[0, 1],
- [0, -self.Kt / self.Kv / (self.J * self.G * self.G * self.R)]])
+ [[-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.Kt / (self.J * self.G * self.R)]])
+ self.C = numpy.matrix([[1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.87])
+
+ self.PlaceObserverPoles([0.3])
+
+ self.U_max = numpy.matrix([[12.0]])
+ self.U_min = numpy.matrix([[-12.0]])
+
+ qff_vel = 8.0
+ self.Qff = numpy.matrix([[1.0 / (qff_vel ** 2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+
+
+class Shooter(VelocityShooter):
+ def __init__(self, name='Shooter'):
+ super(Shooter, self).__init__(name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
+ self.A_continuous[0, 1] = 1
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+
+ # State feedback matrices
+ # [position, angular velocity]
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]])
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 2)))
+ self.K[0, 1:2] = self.K_unaugmented
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 2)))
+ self.Kff[0, 1:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class IntegralShooter(Shooter):
+ def __init__(self, name="IntegralShooter"):
+ super(IntegralShooter, self).__init__(name=name)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[0:2, 2] = self.B_continuous_unaugmented
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ q_pos = 0.08
+ q_vel = 4.00
+ q_voltage = 0.2
+ self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
+
+ r_pos = 0.05
+ self.R = numpy.matrix([[(r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1
+ self.Kff_unaugmented = self.Kff
+ self.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 0:2] = self.Kff_unaugmented
+
+ self.InitializeState()
+
+
+class ScenarioPlotter(object):
+ def __init__(self):
+ # Various lists for graphing things.
+ self.t = []
+ self.x = []
+ self.v = []
+ self.a = []
+ self.x_hat = []
+ self.u = []
+ self.offset = []
+
+ def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
+ observer_shooter=None):
+ """Runs the shooter plant with an initial condition and goal.
+
+ Test for whether the goal has been reached and whether the separation
+ goes outside of the initial and goal values by more than
+ max_separation_error.
+
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ shooter: shooter object to use.
+ goal: goal state.
+ iterations: Number of timesteps to run the model for.
+ controller_shooter: Wrist object to get K from, or None if we should
+ use shooter.
+ observer_shooter: Wrist object to use for the observer, or None if we should
+ use the actual state.
+ """
+
+ if controller_shooter is None:
+ controller_shooter = shooter
+
+ vbat = 12.0
+
+ if self.t:
+ initial_t = self.t[-1] + shooter.dt
+ else:
+ initial_t = 0
+
+ for i in xrange(iterations):
+ X_hat = shooter.X
+
+ if observer_shooter is not None:
+ X_hat = observer_shooter.X_hat
+ self.x_hat.append(observer_shooter.X_hat[1, 0])
+
+ ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
+
+ U = controller_shooter.K * (goal - X_hat) + ff_U
+ U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
+ self.x.append(shooter.X[0, 0])
+
+
+ if self.v:
+ last_v = self.v[-1]
+ else:
+ last_v = 0
+
+ self.v.append(shooter.X[1, 0])
+ self.a.append((self.v[-1] - last_v) / shooter.dt)
+
+ if observer_shooter is not None:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
+ self.offset.append(observer_shooter.X_hat[2, 0])
+
+ applied_U = U.copy()
+ if i > 30:
+ applied_U += 2
+ shooter.Update(applied_U)
+
+ if observer_shooter is not None:
+ observer_shooter.PredictObserver(U)
+
+ self.t.append(initial_t + i * shooter.dt)
+ self.u.append(U[0, 0])
+
+ glog.debug('Time: %f', self.t[-1])
+
+ def Plot(self):
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.t, self.v, label='x')
+ pylab.plot(self.t, self.x_hat, label='x_hat')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.t, self.u, label='u')
+ pylab.plot(self.t, self.offset, label='voltage_offset')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.t, self.a, label='a')
+ pylab.legend()
+
+ pylab.show()
def main(argv):
- if len(argv) != 3:
- print "Expected .h file name and .cc file name"
+ scenario_plotter = ScenarioPlotter()
+
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
+
+ # Test moving the shooter with constant separation.
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter, iterations=200)
+
+ if FLAGS.plot:
+ scenario_plotter.Plot()
+
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name')
else:
namespaces = ['y2016', 'control_loops', 'shooter']
- shooter = Shooter()
- loop_writer = control_loop.ControlLoopWriter("Shooter", [shooter],
+ 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])
+ loop_writer.Write(argv[1], argv[2])
+
+ integral_shooter = IntegralShooter('IntegralShooter')
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/python/wrist.py b/y2016/control_loops/python/wrist.py
index 2ad1d07..4d4f5f1 100755
--- a/y2016/control_loops/python/wrist.py
+++ b/y2016/control_loops/python/wrist.py
@@ -238,8 +238,6 @@
def main(argv):
- argv = FLAGS(argv)
-
scenario_plotter = ScenarioPlotter()
wrist = Wrist()
@@ -260,7 +258,7 @@
glog.fatal('Expected .h file name and .cc file name for the wrist and integral wrist.')
else:
namespaces = ['y2016', 'control_loops', 'superstructure']
- wrist = Wrist("Wrist")
+ wrist = Wrist('Wrist')
loop_writer = control_loop.ControlLoopWriter(
'Wrist', [wrist], namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
@@ -271,4 +269,5 @@
integral_loop_writer.Write(argv[3], argv[4])
if __name__ == '__main__':
- sys.exit(main(sys.argv))
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/y2016/control_loops/shooter/BUILD b/y2016/control_loops/shooter/BUILD
index cb3c0bc..c641a7d 100644
--- a/y2016/control_loops/shooter/BUILD
+++ b/y2016/control_loops/shooter/BUILD
@@ -23,6 +23,8 @@
outs = [
'shooter_plant.h',
'shooter_plant.cc',
+ 'shooter_integral_plant.h',
+ 'shooter_integral_plant.cc',
],
)
@@ -30,9 +32,11 @@
name = 'shooter_plants',
srcs = [
'shooter_plant.cc',
+ 'shooter_integral_plant.cc',
],
hdrs = [
'shooter_plant.h',
+ 'shooter_integral_plant.h',
],
deps = [
'//frc971/control_loops:state_feedback_loop',
diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 9a46906..6550fc2 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -9,129 +9,92 @@
namespace y2016 {
namespace control_loops {
+namespace shooter {
+
+// TODO(austin): Pseudo current limit?
ShooterSide::ShooterSide()
- : loop_(new StateFeedbackLoop<2, 1, 1>(
- ::y2016::control_loops::shooter::MakeShooterLoop())) {
- memset(history_, 0, sizeof(history_));
+ : loop_(new StateFeedbackLoop<3, 1, 1>(MakeIntegralShooterLoop())) {
+ history_.fill(0);
+ Y_.setZero();
}
-void ShooterSide::SetGoal(double angular_velocity_goal_uncapped) {
- angular_velocity_goal_ = std::min(angular_velocity_goal_uncapped,
- kMaxSpeed);
+void ShooterSide::set_goal(double angular_velocity_goal) {
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
}
-void ShooterSide::EstimatePositionTimestep() {
- // NULL position, so look at the loop.
- SetPosition(loop_->X_hat(0, 0));
-}
-
-void ShooterSide::SetPosition(double current_position) {
- current_position_ = current_position;
-
- // Track the current position if the velocity goal is small.
- if (angular_velocity_goal_ <= 1.0) position_goal_ = current_position_;
-
+void ShooterSide::set_position(double current_position) {
// Update position in the model.
- Eigen::Matrix<double, 1, 1> Y;
- Y << current_position_;
- loop_->Correct(Y);
-
- // Prevents integral windup by limiting the position error such that the
- // error can't produce much more than full power.
- const double max_reference =
- (loop_->U_max(0, 0) -
- kAngularVelocityWeightScalar *
- (angular_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) -
- kAngularVelocityWeightScalar *
- (angular_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);
-
- loop_->mutable_R() << position_goal_, angular_velocity_goal_;
- position_goal_ +=
- angular_velocity_goal_ * ::aos::controls::kLoopFrequency.ToSeconds();
+ Y_ << current_position;
// Add the position to the history.
- history_[history_position_] = current_position_;
+ history_[history_position_] = current_position;
history_position_ = (history_position_ + 1) % kHistoryLength;
}
-const ShooterStatus ShooterSide::GetStatus() {
- // Calculate average over dt * kHistoryLength.
- int old_history_position =
- ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
- double avg_angular_velocity =
- (history_[old_history_position] - history_[history_position_]) /
- ::aos::controls::kLoopFrequency.ToSeconds() /
- static_cast<double>(kHistoryLength - 1);
-
- // Ready if average angular velocity is close to the goal.
- bool ready = (std::abs(angular_velocity_goal_ - avg_angular_velocity) <
- kTolerance &&
- angular_velocity_goal_ != 0.0);
-
- return {avg_angular_velocity, ready};
-}
-
-double ShooterSide::GetOutput() {
- if (angular_velocity_goal_ < 1.0) {
- // Kill power at low angular velocities.
- return 0.0;
- }
-
+double ShooterSide::voltage() const {
return loop_->U(0, 0);
}
-void ShooterSide::UpdateLoop(bool output_is_null) {
- loop_->Update(output_is_null);
+void ShooterSide::Update(bool disabled) {
+ loop_->mutable_R() = loop_->next_R();
+ if (loop_->R(1, 0) < 1.0) {
+ // Kill power at low angular velocities.
+ disabled = true;
+ }
+
+ loop_->Correct(Y_);
+ loop_->Update(disabled);
}
-Shooter::Shooter(control_loops::ShooterQueue *my_shooter)
- : aos::controls::ControlLoop<control_loops::ShooterQueue>(my_shooter) {}
+void ShooterSide::SetStatus(ShooterSideStatus *status) {
+ // Compute the oldest point in the history.
+ const int oldest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
-void Shooter::RunIteration(
- const control_loops::ShooterQueue::Goal *goal,
- const control_loops::ShooterQueue::Position *position,
- control_loops::ShooterQueue::Output *output,
- control_loops::ShooterQueue::Status *status) {
+ // Compute the distance moved over that time period.
+ status->avg_angular_velocity =
+ (history_[oldest_history_position] - history_[history_position_]) /
+ (::aos::controls::kLoopFrequency.ToSeconds() *
+ static_cast<double>(kHistoryLength - 1));
+
+ status->angular_velocity = loop_->X_hat(1, 0);
+
+ // Ready if average angular velocity is close to the goal.
+ status->ready = (std::abs(loop_->next_R(1, 0) -
+ status->avg_angular_velocity) < kTolerance &&
+ loop_->next_R(1, 0) > 1.0);
+}
+
+Shooter::Shooter(ShooterQueue *my_shooter)
+ : aos::controls::ControlLoop<ShooterQueue>(my_shooter) {}
+
+void Shooter::RunIteration(const ShooterQueue::Goal *goal,
+ const ShooterQueue::Position *position,
+ ShooterQueue::Output *output,
+ ShooterQueue::Status *status) {
if (goal) {
// Update position/goal for our two shooter sides.
- left_.SetGoal(goal->angular_velocity_left);
- right_.SetGoal(goal->angular_velocity_right);
-
- if (position == nullptr) {
- left_.EstimatePositionTimestep();
- right_.EstimatePositionTimestep();
- } else {
- left_.SetPosition(position->theta_left);
- right_.SetPosition(position->theta_right);
- }
+ left_.set_goal(goal->angular_velocity);
+ right_.set_goal(goal->angular_velocity);
}
- ShooterStatus status_left = left_.GetStatus();
- ShooterStatus status_right = right_.GetStatus();
- status->avg_angular_velocity_left = status_left.avg_angular_velocity;
- status->avg_angular_velocity_right = status_right.avg_angular_velocity;
+ left_.set_position(position->theta_left);
+ right_.set_position(position->theta_right);
- status->ready_left = status_left.ready;
- status->ready_right = status_right.ready;
- status->ready_both = (status_left.ready && status_right.ready);
+ left_.Update(output == nullptr);
+ right_.Update(output == nullptr);
- left_.UpdateLoop(output == nullptr);
- right_.UpdateLoop(output == nullptr);
+ left_.SetStatus(&status->left);
+ right_.SetStatus(&status->right);
+ status->ready = (status->left.ready && status->right.ready);
if (output) {
- output->voltage_left = left_.GetOutput();
- output->voltage_right = right_.GetOutput();
+ output->voltage_left = left_.voltage();
+ output->voltage_right = right_.voltage();
}
}
+} // namespace shooter
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 6a3fa4d..0354bb5 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -6,61 +6,59 @@
#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_integral_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
namespace y2016 {
namespace control_loops {
+namespace shooter {
namespace {
-// TODO(constants): Update.
-const double kTolerance = 10.0;
-const double kMaxSpeed = 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
-const double kAngularVelocityWeightScalar = 0.35;
+constexpr double kTolerance = 10.0;
} // namespace
-struct ShooterStatus {
- double avg_angular_velocity;
- bool ready;
-};
-
class ShooterSide {
public:
ShooterSide();
- void SetGoal(double angular_velocity_goal);
- void EstimatePositionTimestep();
- void SetPosition(double current_position);
- const ShooterStatus GetStatus();
- double GetOutput();
- void UpdateLoop(bool output_is_null);
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ // Sets the current encoder position in radians
+ void set_position(double current_position);
+
+ // Populates the status structure.
+ void SetStatus(ShooterSideStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
private:
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
-
- double current_position_ = 0.0;
- double position_goal_ = 0.0;
- double angular_velocity_goal_ = 0.0;
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
// History array for calculating a filtered angular velocity.
- static const int kHistoryLength = 5;
- double history_[kHistoryLength];
+ static constexpr int kHistoryLength = 10;
+ ::std::array<double, kHistoryLength> history_;
ptrdiff_t history_position_;
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
-class Shooter
- : public ::aos::controls::ControlLoop<control_loops::ShooterQueue> {
+class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
public:
- explicit Shooter(control_loops::ShooterQueue *shooter_queue =
- &control_loops::shooter_queue);
+ explicit Shooter(
+ ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
protected:
- void RunIteration(const control_loops::ShooterQueue::Goal *goal,
- const control_loops::ShooterQueue::Position *position,
- control_loops::ShooterQueue::Output *output,
- control_loops::ShooterQueue::Status *status) override;
+ void RunIteration(const ShooterQueue::Goal *goal,
+ const ShooterQueue::Position *position,
+ ShooterQueue::Output *output,
+ ShooterQueue::Status *status) override;
private:
ShooterSide left_, right_;
@@ -68,6 +66,7 @@
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
+} // namespace shooter
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter.q b/y2016/control_loops/shooter/shooter.q
index 49575fb..af3f064 100644
--- a/y2016/control_loops/shooter/shooter.q
+++ b/y2016/control_loops/shooter/shooter.q
@@ -1,38 +1,47 @@
-package y2016.control_loops;
+package y2016.control_loops.shooter;
import "aos/common/controls/control_loops.q";
import "frc971/control_loops/control_loops.q";
+struct ShooterSideStatus {
+ // True if the shooter side is up to speed and stable.
+ bool ready;
+ // The current average velocity in radians/second.
+ double avg_angular_velocity;
+ // The current instantaneous filtered velocity in radians/second.
+ double angular_velocity;
+};
+
queue_group ShooterQueue {
implements aos.control_loops.ControlLoop;
- // All angles are in radians, and angular velocities are in rad/sec. For all
- // angular velocities, positive is shooting the ball out of the robot.
+ // All angles are in radians, and angular velocities are in radians/second.
+ // For all angular velocities, positive is shooting the ball out of the robot.
message Goal {
- // Angular velocity goals in rad/sec.
- double angular_velocity_left;
- double angular_velocity_right;
+ // Angular velocity goals in radians/second.
+ double angular_velocity;
};
message Position {
- // Wheel angle in rad/sec.
+ // Wheel angle in radians/second.
double theta_left;
double theta_right;
};
message Status {
- bool ready_left;
- bool ready_right;
- // Is the shooter ready to shoot?
- bool ready_both;
+ // Left side status.
+ ShooterSideStatus left;
+ // Right side status.
+ ShooterSideStatus right;
- // Average angular velocities over dt * kHistoryLength.
- double avg_angular_velocity_left;
- double avg_angular_velocity_right;
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ bool ready;
};
message Output {
+ // Voltage in volts of the left and right shooter motors.
double voltage_left;
double voltage_right;
};
diff --git a/y2016/control_loops/shooter/shooter_lib_test.cc b/y2016/control_loops/shooter/shooter_lib_test.cc
index 9ec0334..8d0ae39 100644
--- a/y2016/control_loops/shooter/shooter_lib_test.cc
+++ b/y2016/control_loops/shooter/shooter_lib_test.cc
@@ -10,23 +10,45 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/shooter/shooter.h"
+#include "y2016/control_loops/shooter/shooter_plant.h"
using ::aos::time::Time;
using ::frc971::control_loops::testing::kTeamNumber;
namespace y2016 {
namespace control_loops {
+namespace shooter {
namespace testing {
+class ShooterPlant : public StateFeedbackPlant<2, 1, 1> {
+ public:
+ explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
+ : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+
+ void CheckU() override {
+ assert(U(0, 0) <= U_max(0, 0) + 0.00001 + voltage_offset_);
+ assert(U(0, 0) >= U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
+
// Class which simulates the shooter and sends out queue messages with the
// position.
class ShooterSimulation {
public:
// Constructs a shooter simulation.
ShooterSimulation()
- : shooter_plant_left_(new StateFeedbackPlant<2, 1, 1>(
+ : shooter_plant_left_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
- shooter_plant_right_(new StateFeedbackPlant<2, 1, 1>(
+ shooter_plant_right_(new ShooterPlant(
::y2016::control_loops::shooter::MakeShooterPlant())),
shooter_queue_(".y2016.control_loops.shooter", 0x78d8e372,
".y2016.control_loops.shooter.goal",
@@ -36,7 +58,7 @@
// Sends a queue message with the position of the shooter.
void SendPositionMessage() {
- ::aos::ScopedMessagePtr<control_loops::ShooterQueue::Position> position =
+ ::aos::ScopedMessagePtr<ShooterQueue::Position> position =
shooter_queue_.position.MakeMessage();
position->theta_left = shooter_plant_left_->Y(0, 0);
@@ -44,20 +66,30 @@
position.Send();
}
+ void set_left_voltage_offset(double voltage_offset) {
+ shooter_plant_left_->set_voltage_offset(voltage_offset);
+ }
+
+ void set_right_voltage_offset(double voltage_offset) {
+ shooter_plant_right_->set_voltage_offset(voltage_offset);
+ }
+
// Simulates shooter for a single timestep.
void Simulate() {
EXPECT_TRUE(shooter_queue_.output.FetchLatest());
- shooter_plant_left_->mutable_U(0, 0) = shooter_queue_.output->voltage_left;
+ shooter_plant_left_->mutable_U(0, 0) =
+ shooter_queue_.output->voltage_left +
+ shooter_plant_left_->voltage_offset();
shooter_plant_right_->mutable_U(0, 0) =
- shooter_queue_.output->voltage_right;
+ shooter_queue_.output->voltage_right +
+ shooter_plant_right_->voltage_offset();
shooter_plant_left_->Update();
shooter_plant_right_->Update();
}
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> shooter_plant_left_,
- shooter_plant_right_;
+ ::std::unique_ptr<ShooterPlant> shooter_plant_left_, shooter_plant_right_;
private:
ShooterQueue shooter_queue_;
@@ -83,10 +115,15 @@
EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity_left,
- shooter_queue_.status->avg_angular_velocity_left, 10.0);
- EXPECT_NEAR(shooter_queue_.goal->angular_velocity_right,
- shooter_queue_.status->avg_angular_velocity_right, 10.0);
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+ shooter_queue_.status->left.angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+ shooter_queue_.status->right.angular_velocity, 10.0);
+
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+ shooter_queue_.status->left.avg_angular_velocity, 10.0);
+ EXPECT_NEAR(shooter_queue_.goal->angular_velocity,
+ shooter_queue_.status->right.avg_angular_velocity, 10.0);
}
// Runs one iteration of the whole simulation.
@@ -121,8 +158,7 @@
// Tests that the shooter does nothing when the goal is zero.
TEST_F(ShooterTest, DoesNothing) {
ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(0.0)
- .angular_velocity_right(0.0)
+ .angular_velocity(0.0)
.Send());
RunIteration();
@@ -137,17 +173,13 @@
// without applying any power.
TEST_F(ShooterTest, SpinUpAndDown) {
// Spin up.
- EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(450.0)
- .angular_velocity_right(450.0)
- .Send());
- RunForTime(Time::InSeconds(10));
+ EXPECT_TRUE(
+ shooter_queue_.goal.MakeWithBuilder().angular_velocity(450.0).Send());
+ RunForTime(Time::InSeconds(1));
VerifyNearGoal();
- EXPECT_TRUE(shooter_queue_.status->ready_both);
- EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(0.0)
- .angular_velocity_right(0.0)
- .Send());
+ EXPECT_TRUE(shooter_queue_.status->ready);
+ EXPECT_TRUE(
+ shooter_queue_.goal.MakeWithBuilder().angular_velocity(0.0).Send());
// Make sure we don't apply voltage on spin-down.
RunIteration();
@@ -167,10 +199,10 @@
// mistakenly say that we are ready. This test should look at both extremes.
TEST_F(ShooterTest, SideLagTest) {
// Spin up.
- EXPECT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(20.0)
- .angular_velocity_right(450.0)
- .Send());
+ EXPECT_TRUE(
+ shooter_queue_.goal.MakeWithBuilder().angular_velocity(20.0).Send());
+ // Cause problems by adding a voltage error on one side.
+ shooter_plant_.set_right_voltage_offset(-4.0);
RunForTime(Time::InSeconds(0.1));
shooter_queue_.goal.FetchLatest();
@@ -180,9 +212,9 @@
EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
// Left should be up to speed, right shouldn't.
- EXPECT_TRUE(shooter_queue_.status->ready_left);
- EXPECT_FALSE(shooter_queue_.status->ready_right);
- EXPECT_FALSE(shooter_queue_.status->ready_both);
+ EXPECT_TRUE(shooter_queue_.status->left.ready);
+ EXPECT_FALSE(shooter_queue_.status->right.ready);
+ EXPECT_FALSE(shooter_queue_.status->ready);
RunForTime(Time::InSeconds(5));
@@ -193,71 +225,25 @@
EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
// Both should be up to speed.
- EXPECT_TRUE(shooter_queue_.status->ready_left);
- EXPECT_TRUE(shooter_queue_.status->ready_right);
- EXPECT_TRUE(shooter_queue_.status->ready_both);
+ EXPECT_TRUE(shooter_queue_.status->left.ready);
+ EXPECT_TRUE(shooter_queue_.status->right.ready);
+ EXPECT_TRUE(shooter_queue_.status->ready);
}
-// Test to make sure that it does not exceed the encoder's rated speed.
-TEST_F(ShooterTest, RateLimitTest) {
- ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(1000.0)
- .angular_velocity_right(1000.0)
- .Send());
-
- RunForTime(Time::InSeconds(10));
- EXPECT_TRUE(shooter_queue_.status.FetchLatest());
- EXPECT_TRUE(shooter_queue_.status->ready_both);
-
- shooter_queue_.goal.FetchLatest();
- shooter_queue_.status.FetchLatest();
-
- EXPECT_TRUE(shooter_queue_.goal.get() != nullptr);
- EXPECT_TRUE(shooter_queue_.status.get() != nullptr);
-
- EXPECT_GT(::y2016::control_loops::kMaxSpeed,
- shooter_queue_.status->avg_angular_velocity_left);
-
- EXPECT_GT(::y2016::control_loops::kMaxSpeed,
- shooter_queue_.status->avg_angular_velocity_right);
-}
-
-// Tests that the shooter can spin up nicely while missing position packets.
-TEST_F(ShooterTest, MissingPositionMessages) {
- ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(200.0)
- .angular_velocity_right(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.
-// TODO(comran): Update this test, since it's the same as
-// MissingPositionMessages.
+// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(ShooterTest, Disabled) {
ASSERT_TRUE(shooter_queue_.goal.MakeWithBuilder()
- .angular_velocity_left(200.0)
- .angular_velocity_right(200.0)
+ .angular_velocity(200.0)
.Send());
- for (int i = 0; i < 100; ++i) {
- if (i % 7) {
- shooter_plant_.SendPositionMessage();
- }
+ RunForTime(Time::InSeconds(5), false);
+ EXPECT_EQ(nullptr, shooter_queue_.output.get());
- RunIteration();
- }
+ RunForTime(Time::InSeconds(5));
VerifyNearGoal();
}
} // namespace testing
+} // namespace shooter
} // namespace control_loops
} // namespace y2016
diff --git a/y2016/control_loops/shooter/shooter_main.cc b/y2016/control_loops/shooter/shooter_main.cc
index 911ab29..4036c5f 100644
--- a/y2016/control_loops/shooter/shooter_main.cc
+++ b/y2016/control_loops/shooter/shooter_main.cc
@@ -4,7 +4,7 @@
int main() {
::aos::Init();
- ::y2016::control_loops::Shooter shooter;
+ ::y2016::control_loops::shooter::Shooter shooter;
shooter.Run();
::aos::Cleanup();
return 0;