Added another pole to the flywheel
Scott, Adam and Campbel did a bunch of testing and concluded
that there is a second pole in the flywheel response. This
commit adds that pole to our system. We need to tune it,
but we need a robot first.
Change-Id: I5ba67db077bb709460c70f6df0164b8c3c1f700d
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 917470a..1a4f910 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -61,12 +61,15 @@
self.C = numpy.matrix([[1]])
self.D = numpy.matrix([[0]])
+ # The states are [unfiltered_velocity]
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
self.PlaceControllerPoles([.75])
glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
glog.debug('Poles are %s',
repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
@@ -81,40 +84,85 @@
self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
self.InitializeState()
+class SecondOrderVelocityShooter(VelocityShooter):
+ def __init__(self, name='SecondOrderVelocityShooter'):
+ super(SecondOrderVelocityShooter, self).__init__(name)
-class Shooter(VelocityShooter):
+ 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[0:1, 0:1] = self.A_continuous_unaugmented
+ self.A_continuous[1, 0] = 105.0
+ self.A_continuous[1, 1] = -105.0
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[0:1, 0] = self.B_continuous_unaugmented
+
+ self.C = numpy.matrix([[0, 1]])
+ self.D = numpy.matrix([[0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.PlaceControllerPoles([.75, 0.75])
+
+ glog.debug('K %s', repr(self.K))
+ glog.debug('System poles are %s',
+ repr(numpy.linalg.eig(self.A_continuous)[0]))
+ glog.debug('Poles are %s',
+ repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+ self.PlaceObserverPoles([0.3, 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), 0.0],
+ [0.0, 1.0 / (qff_vel ** 2.0)]])
+
+ self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)
+ self.InitializeState()
+
+
+class Shooter(SecondOrderVelocityShooter):
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.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[1:3, 1:3] = self.A_continuous_unaugmented
+ self.A_continuous[0, 2] = 1
- self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
- self.B_continuous[1:2, 0] = self.B_continuous_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[1:3, 0] = self.B_continuous_unaugmented
# State feedback matrices
- # [position, angular velocity]
- self.C = numpy.matrix([[1, 0]])
+ # [position, unfiltered_velocity, angular velocity]
+ self.C = numpy.matrix([[1, 0, 0]])
self.D = numpy.matrix([[0]])
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
+ glog.debug(repr(self.A_continuous))
+ glog.debug(repr(self.B_continuous))
- self.rpl = .45
- self.ipl = 0.07
- self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl])
+ observeability = controls.ctrb(self.A.T, self.C.T)
+ glog.debug('Rank of augmented observability matrix. %d', numpy.linalg.matrix_rank(
+ observeability))
+
+
+ self.PlaceObserverPoles([0.9, 0.8, 0.7])
self.K_unaugmented = self.K
- self.K = numpy.matrix(numpy.zeros((1, 2)))
- self.K[0, 1:2] = self.K_unaugmented
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 1:3] = 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.Kff = numpy.matrix(numpy.zeros((1, 3)))
+ self.Kff[0, 1:3] = self.Kff_unaugmented
self.InitializeState()
@@ -126,17 +174,18 @@
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.A_continuous = numpy.matrix(numpy.zeros((4, 4)))
+ self.A_continuous[0:3, 0:3] = self.A_continuous_unaugmented
+ self.A_continuous[0:3, 3] = self.B_continuous_unaugmented
- self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
- self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+ self.B_continuous = numpy.matrix(numpy.zeros((4, 1)))
+ self.B_continuous[0:3, 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.C = numpy.matrix(numpy.zeros((1, 4)))
+ self.C[0:1, 0:3] = self.C_unaugmented
+ # The states are [position, unfiltered_velocity, velocity, torque_error]
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
@@ -148,9 +197,10 @@
q_pos = 0.01
q_vel = 2.0
q_voltage = 0.3
- self.Q_continuous = 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)]])
+ self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0, 0.0],
+ [0.0, 0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.0003
self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
@@ -165,12 +215,12 @@
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.K = numpy.matrix(numpy.zeros((1, 4)))
+ self.K[0, 0:3] = self.K_unaugmented
+ self.K[0, 3] = 1
self.Kff_unaugmented = self.Kff
- self.Kff = numpy.matrix(numpy.zeros((1, 3)))
- self.Kff[0, 0:2] = self.Kff_unaugmented
+ self.Kff = numpy.matrix(numpy.zeros((1, 4)))
+ self.Kff[0, 0:3] = self.Kff_unaugmented
self.InitializeState()
@@ -216,7 +266,7 @@
if observer_shooter is not None:
X_hat = observer_shooter.X_hat
- self.x_hat.append(observer_shooter.X_hat[1, 0])
+ self.x_hat.append(observer_shooter.X_hat[2, 0])
ff_U = controller_shooter.Kff * (goal - observer_shooter.A * goal)
@@ -224,21 +274,21 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(shooter.X[0, 0])
- self.diff.append(shooter.X[1, 0] - observer_shooter.X_hat[1, 0])
+ self.diff.append(shooter.X[2, 0] - observer_shooter.X_hat[2, 0])
if self.v:
last_v = self.v[-1]
else:
last_v = 0
- self.v.append(shooter.X[1, 0])
+ self.v.append(shooter.X[2, 0])
self.a.append((self.v[-1] - last_v) / shooter.dt)
if observer_shooter is not None:
if i != 0:
observer_shooter.Y = shooter.Y
observer_shooter.CorrectObserver(U)
- self.offset.append(observer_shooter.X_hat[2, 0])
+ self.offset.append(observer_shooter.X_hat[3, 0])
applied_U = U.copy()
if i > 30:
@@ -283,8 +333,8 @@
observer_shooter = IntegralShooter()
iterations = 200
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [100.0], [0.0]])
scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
observer_shooter=observer_shooter, iterations=iterations)
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 18fc795..c0b9d6e 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,15 +23,16 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>>(
+ : loop_(new StateFeedbackLoop<4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>,
+ HybridKalman<4, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
}
void ShooterController::set_goal(double angular_velocity_goal) {
- loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, angular_velocity_goal,
+ 0.0;
}
void ShooterController::set_position(double current_position) {
@@ -52,7 +53,7 @@
void ShooterController::Update(bool disabled, chrono::nanoseconds dt) {
loop_->mutable_R() = loop_->next_R();
- if (::std::abs(loop_->R(1, 0)) < 1.0) {
+ if (::std::abs(loop_->R(2, 0)) < 1.0) {
// Kill power at low angular velocities.
disabled = true;
}
@@ -72,17 +73,17 @@
static_cast<double>(kHistoryLength - 1));
// Ready if average angular velocity is close to the goal.
- error_ = average_angular_velocity_ - loop_->next_R(1, 0);
+ error_ = average_angular_velocity_ - loop_->next_R(2, 0);
- ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+ ready_ = std::abs(error_) < kTolerance && loop_->next_R(2, 0) > 1.0;
- if (last_ready_ && !ready_ && loop_->next_R(1, 0) > 1.0 && error_ < 0.0) {
+ if (last_ready_ && !ready_ && loop_->next_R(2, 0) > 1.0 && error_ < 0.0) {
needs_reset_ = true;
- min_velocity_ = loop_->X_hat(1, 0);
+ min_velocity_ = velocity();
}
if (needs_reset_) {
- min_velocity_ = ::std::min(min_velocity_, loop_->X_hat(1, 0));
- if (loop_->X_hat(1, 0) > min_velocity_ + 5.0) {
+ min_velocity_ = ::std::min(min_velocity_, velocity());
+ if (velocity() > min_velocity_ + 5.0) {
reset_ = true;
needs_reset_ = false;
}
@@ -109,10 +110,10 @@
void ShooterController::SetStatus(ShooterStatus *status) {
status->avg_angular_velocity = average_angular_velocity_;
- status->angular_velocity = X_hat_current_(1, 0);
+ status->angular_velocity = X_hat_current_(2, 0);
status->ready = ready_;
- status->voltage_error = X_hat_current_(2, 0);
+ status->voltage_error = X_hat_current_(3, 0);
status->position_error = position_error_;
status->instantaneous_velocity = dt_velocity_;
status->fixed_instantaneous_velocity = fixed_dt_velocity_;
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index d9b6f45..d13d94c 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -33,7 +33,8 @@
double voltage() const;
// Returns the instantaneous velocity.
- double velocity() const { return loop_->X_hat(1, 0); }
+ double velocity() const { return loop_->X_hat(2, 0); }
+ double voltage_error() const { return loop_->X_hat(3, 0); }
double dt_velocity() const { return dt_velocity_; }
@@ -50,7 +51,7 @@
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
::std::unique_ptr<StateFeedbackLoop<
- 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ 4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>, HybridKalman<4, 1, 1>>>
loop_;
// History array for calculating a filtered angular velocity.
@@ -67,7 +68,7 @@
double min_velocity_ = 0.0;
double position_error_ = 0.0;
- Eigen::Matrix<double, 3, 1> X_hat_current_;
+ Eigen::Matrix<double, 4, 1> X_hat_current_;
bool ready_ = false;
bool needs_reset_ = false;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 114b449..7906af4 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -29,10 +29,10 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-class ShooterPlant : public StateFeedbackPlant<2, 1, 1> {
+class ShooterPlant : public StateFeedbackPlant<3, 1, 1> {
public:
- explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+ explicit ShooterPlant(StateFeedbackPlant<3, 1, 1> &&other)
+ : StateFeedbackPlant<3, 1, 1>(::std::move(other)) {}
void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
@@ -224,7 +224,7 @@
double intake_position() const { return intake_plant_->X(0, 0); }
double intake_velocity() const { return intake_plant_->X(1, 0); }
- double shooter_velocity() const { return shooter_plant_->X(1, 0); }
+ double shooter_velocity() const { return shooter_plant_->X(2, 0); }
double indexer_velocity() const { return column_plant_->X(1, 0); }