Merge changes I1f059673,I9ade9a74
* changes:
Sequence the indexer
Tune flywheels
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d769541..f033a89 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -142,7 +142,7 @@
A_.setZero();
B_.setZero();
DelayedU_.setZero();
- UpdateAB(::std::chrono::milliseconds(5));
+ UpdateAB(::std::chrono::microseconds(5050));
}
// Assert that U is within the hardware range.
@@ -158,6 +158,8 @@
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt, Scalar voltage_battery) {
+ CHECK_NE(dt, std::chrono::nanoseconds(0));
+
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
@@ -178,7 +180,11 @@
// or the unit delay... Might want to do that if we care about performance
// again.
UpdateAB(dt);
- return A() * X + B() * U;
+ const Eigen::Matrix<Scalar, number_of_states, 1> new_X =
+ A() * X + B() * DelayedU_;
+ DelayedU_ = U;
+
+ return new_X;
}
protected:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 27a659a..9c36c90 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -284,9 +284,9 @@
def InitializeState(self):
"""Sets X, Y, and X_hat to zero defaults."""
- self.X = numpy.zeros((self.A.shape[0], 1))
+ self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
- self.X_hat = numpy.zeros((self.A.shape[0], 1))
+ self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d63bca7..168df43 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -108,6 +108,8 @@
}
Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
+ const std::chrono::nanoseconds dt() const { return coefficients().dt; }
+
const Eigen::Matrix<Scalar, number_of_states, 1> &X() const { return X_; }
Scalar X(int i, int j = 0) const { return X()(i, j); }
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y() const { return Y_; }
@@ -503,9 +505,7 @@
return controller().Kff() * (next_R() - plant().A() * R());
}
- // stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors,
- ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
+ void UpdateController(bool stop_motors) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -514,10 +514,15 @@
U_ = U_uncapped_ = ControllerOutput();
CapU();
}
+ UpdateFFReference();
+ }
+
+ // stop_motors is whether or not to output all 0s.
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(0)) {
+ UpdateController(stop_motors);
UpdateObserver(U_, dt);
-
- UpdateFFReference();
}
// Updates R() after any CapU operations happen on U().
diff --git a/y2020/control_loops/python/accelerator.py b/y2020/control_loops/python/accelerator.py
index 1746589..48c9098 100644
--- a/y2020/control_loops/python/accelerator.py
+++ b/y2020/control_loops/python/accelerator.py
@@ -28,20 +28,22 @@
name='Accelerator',
motor=control_loop.Falcon(),
G=G,
- J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ J=J + 0.0015,
+ q_pos=0.01,
+ q_vel=40.0,
+ q_voltage=2.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.86])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(kAccelerator, goal=R, iterations=200)
+ flywheel.PlotSpinup(kAccelerator, goal=R, iterations=400)
return 0
+ glog.debug("J is %f" % J)
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -53,4 +55,5 @@
if __name__ == '__main__':
argv = FLAGS(sys.argv)
+ glog.init()
sys.exit(main(argv))
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 5067a16..9381630 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -14,7 +14,7 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
# Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0
+J_wheel = 0.000319 * 2.0 * 6.0
# Gear ratio to the final wheel.
# 40 tooth on the flywheel
# 48 for the falcon.
@@ -29,17 +29,17 @@
motor=control_loop.Falcon(),
G=G,
J=J,
- q_pos=0.08,
- q_vel=4.00,
- q_voltage=0.4,
+ q_pos=0.01,
+ q_vel=100.0,
+ q_voltage=6.0,
r_pos=0.05,
- controller_poles=[.84])
+ controller_poles=[.92])
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[0.0], [500.0], [0.0]])
- flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=200)
+ flywheel.PlotSpinup(params=kFinisher, goal=R, iterations=400)
return 0
if len(argv) != 5:
diff --git a/y2020/control_loops/python/flywheel.py b/y2020/control_loops/python/flywheel.py
index 630d223..451788a 100755
--- a/y2020/control_loops/python/flywheel.py
+++ b/y2020/control_loops/python/flywheel.py
@@ -30,7 +30,7 @@
self.controller_poles = controller_poles
-class VelocityFlywheel(control_loop.ControlLoop):
+class VelocityFlywheel(control_loop.HybridControlLoop):
def __init__(self, params, name="Flywheel"):
super(VelocityFlywheel, self).__init__(name=name)
self.params = params
@@ -121,26 +121,44 @@
self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
# states
# [position, velocity, voltage_error]
self.C_unaugmented = self.C
self.C = numpy.matrix(numpy.zeros((1, 3)))
self.C[0:1, 0:2] = self.C_unaugmented
+ glog.debug('A_continuous %s' % str(self.A_continuous))
+ glog.debug('B_continuous %s' % str(self.B_continuous))
+ glog.debug('C %s' % str(self.C))
+
self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
self.B_continuous, self.dt)
+ glog.debug('A %s' % str(self.A))
+ glog.debug('B %s' % str(self.B))
+
q_pos = self.params.q_pos
q_vel = self.params.q_vel
q_voltage = self.params.q_voltage
- self.Q = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
+ 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)]])
r_pos = self.params.r_pos
- self.R = numpy.matrix([[(r_pos**2.0)]])
+ self.R_continuous = numpy.matrix([[(r_pos**2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous,
+ B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous,
+ R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ glog.debug('Q_discrete %s' % (str(self.Q)))
+ glog.debug('R_discrete %s' % (str(self.R)))
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
@@ -155,7 +173,7 @@
self.InitializeState()
-def PlotSpinup(params, goal, iterations=200):
+def PlotSpinup(params, goal, iterations=400):
"""Runs the flywheel plant with an initial condition and goal.
Args:
@@ -210,21 +228,20 @@
if observer_flywheel is not None:
observer_flywheel.Y = flywheel.Y
- observer_flywheel.CorrectObserver(U)
+ observer_flywheel.CorrectHybridObserver(U)
offset.append(observer_flywheel.X_hat[2, 0])
applied_U = U.copy()
- if i > 100:
+ if i > 200:
applied_U += 2
flywheel.Update(applied_U)
if observer_flywheel is not None:
- observer_flywheel.PredictObserver(U)
+ observer_flywheel.PredictHybridObserver(U, flywheel.dt)
t.append(initial_t + i * flywheel.dt)
u.append(U[0, 0])
- glog.debug('Time: %f', t[-1])
pylab.subplot(3, 1, 1)
pylab.plot(t, v, label='x')
pylab.plot(t, x_hat, label='x_hat')
@@ -281,5 +298,9 @@
loop_writer.Write(plant_files[0], plant_files[1])
integral_loop_writer = control_loop.ControlLoopWriter(
- 'Integral' + name, integral_flywheels, namespaces=namespace)
+ 'Integral' + name,
+ integral_flywheels,
+ namespaces=namespace,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index 98ddfe4..7e75dce 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -47,6 +47,8 @@
angular_system.PlotKick(kHood, R)
angular_system.PlotMotion(kHood, R)
+ glog.info("Radians per turn: %f\n", radians_per_turn)
+
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
index 77fb769..07c3d99 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.cc
@@ -11,8 +11,12 @@
namespace superstructure {
namespace shooter {
-FlywheelController::FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop)
- : loop_(new StateFeedbackLoop<3, 1, 1>(std::move(loop))) {
+FlywheelController::FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop)
+ : loop_(new StateFeedbackLoop<3, 1, 1, double,
+ StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(std::move(loop))) {
history_.fill(std::pair<double, ::aos::monotonic_clock::time_point>(
0, ::aos::monotonic_clock::epoch()));
Y_.setZero();
@@ -26,6 +30,18 @@
void FlywheelController::set_position(
double current_position,
const aos::monotonic_clock::time_point position_timestamp) {
+ // Project time forwards.
+ const int newest_history_position =
+ ((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
+
+ if (!first_) {
+ loop_->UpdateObserver(
+ loop_->U(),
+ position_timestamp - std::get<1>(history_[newest_history_position]));
+ } else {
+ first_ = false;
+ }
+
// Update position in the model.
Y_ << current_position;
@@ -34,6 +50,8 @@
std::pair<double, ::aos::monotonic_clock::time_point>(current_position,
position_timestamp);
history_position_ = (history_position_ + 1) % kHistoryLength;
+
+ loop_->Correct(Y_);
}
double FlywheelController::voltage() const { return loop_->U(0, 0); }
@@ -45,22 +63,22 @@
disabled = true;
}
- loop_->Correct(Y_);
- loop_->Update(disabled);
+ loop_->UpdateController(disabled);
}
flatbuffers::Offset<FlywheelControllerStatus> FlywheelController::SetStatus(
flatbuffers::FlatBufferBuilder *fbb) {
// Compute the oldest point in the history.
- const int oldest_history_position =
+ const int oldest_history_position = history_position_;
+ const int newest_history_position =
((history_position_ == 0) ? kHistoryLength : history_position_) - 1;
const double total_loop_time = ::aos::time::DurationInSeconds(
- std::get<1>(history_[history_position_]) -
+ std::get<1>(history_[newest_history_position]) -
std::get<1>(history_[oldest_history_position]));
const double distance_traveled =
- std::get<0>(history_[history_position_]) -
+ std::get<0>(history_[newest_history_position]) -
std::get<0>(history_[oldest_history_position]);
// Compute the distance moved over that time period.
@@ -70,6 +88,7 @@
builder.add_avg_angular_velocity(avg_angular_velocity_);
builder.add_angular_velocity(loop_->X_hat(1, 0));
+ builder.add_voltage_error(loop_->X_hat(2, 0));
builder.add_angular_velocity_goal(last_goal_);
return builder.Finish();
}
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index a3e9bdb..e130389 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -18,7 +18,9 @@
// Handles the velocity control of each flywheel.
class FlywheelController {
public:
- FlywheelController(StateFeedbackLoop<3, 1, 1> &&loop);
+ FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop);
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
@@ -45,7 +47,10 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 10;
@@ -59,6 +64,8 @@
double last_goal_ = 0;
+ bool first_ = true;
+
DISALLOW_COPY_AND_ASSIGN(FlywheelController);
};
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 25f6a6a..a9f1c4c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -12,7 +12,7 @@
namespace shooter {
namespace {
-const double kVelocityTolerance = 0.01;
+const double kVelocityTolerance = 20.0;
} // namespace
Shooter::Shooter()
@@ -46,16 +46,18 @@
accelerator_right_.set_position(position->theta_accelerator_right(),
position_timestamp);
- finisher_.Update(output == nullptr);
- accelerator_left_.Update(output == nullptr);
- accelerator_right_.Update(output == nullptr);
-
// Update goal.
if (goal) {
finisher_.set_goal(goal->velocity_finisher());
accelerator_left_.set_goal(goal->velocity_accelerator());
accelerator_right_.set_goal(goal->velocity_accelerator());
+ }
+ finisher_.Update(output == nullptr);
+ accelerator_left_.Update(output == nullptr);
+ accelerator_right_.Update(output == nullptr);
+
+ if (goal) {
if (UpToSpeed(goal) && goal->velocity_finisher() > kVelocityTolerance &&
goal->velocity_accelerator() > kVelocityTolerance) {
ready_ = true;
diff --git a/y2020/control_loops/superstructure/shooter_plot.pb b/y2020/control_loops/superstructure/shooter_plot.pb
index 01a1e20..16ab5f4 100644
--- a/y2020/control_loops/superstructure/shooter_plot.pb
+++ b/y2020/control_loops/superstructure/shooter_plot.pb
@@ -24,19 +24,87 @@
line {
y_signal {
channel: "Status"
- field: "hood.position"
+ field: "shooter.accelerator_left.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_left.angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.angular_velocity"
}
}
line {
y_signal {
channel: "Goal"
- field: "hood.unsafe_goal"
+ field: "shooter.velocity_accelerator"
}
}
line {
y_signal {
- channel: "Position"
- field: "hood.encoder"
+ channel: "Status"
+ field: "shooter.finisher.avg_angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.finisher.angular_velocity"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Goal"
+ field: "shooter.velocity_finisher"
+ }
+ }
+ }
+ axes {
+ line {
+ y_signal {
+ channel: "Output"
+ field: "accelerator_left_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Output"
+ field: "accelerator_right_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_left.voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.accelerator_right.voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Output"
+ field: "finisher_voltage"
+ }
+ }
+ line {
+ y_signal {
+ channel: "Status"
+ field: "shooter.finisher.voltage_error"
}
}
ylabel: "hood position"
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index d16c692..5dfd2d1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -42,6 +42,30 @@
output != nullptr ? &(output_struct.hood_voltage) : nullptr,
status->fbb());
+ if (unsafe_goal != nullptr) {
+ if (unsafe_goal->shooting() &&
+ shooting_start_time_ == aos::monotonic_clock::min_time) {
+ shooting_start_time_ = position_timestamp;
+ }
+
+ if (unsafe_goal->shooting()) {
+ constexpr std::chrono::milliseconds kPeriod =
+ std::chrono::milliseconds(250);
+ if ((position_timestamp - shooting_start_time_) % (kPeriod * 2) <
+ kPeriod) {
+ intake_joint_.set_min_position(-0.25);
+ } else {
+ intake_joint_.set_min_position(-0.75);
+ }
+ } else {
+ intake_joint_.clear_min_position();
+ }
+
+ if (!unsafe_goal->shooting()) {
+ shooting_start_time_ = aos::monotonic_clock::min_time;
+ }
+ }
+
flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
intake_joint_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
@@ -115,13 +139,21 @@
if (output != nullptr) {
if (unsafe_goal) {
- output_struct.washing_machine_spinner_voltage = 6.0;
+ output_struct.washing_machine_spinner_voltage = 0.0;
if (unsafe_goal->shooting()) {
- output_struct.feeder_voltage = 6.0;
+ if (shooter_.ready() &&
+ unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
+ unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+ output_struct.feeder_voltage = 9.0;
+ } else {
+ output_struct.feeder_voltage = 0.0;
+ }
+ output_struct.washing_machine_spinner_voltage = 5.0;
+ output_struct.intake_roller_voltage = 3.0;
} else {
output_struct.feeder_voltage = 0.0;
+ output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
}
- output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
} else {
output_struct.intake_roller_voltage = 0.0;
}
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index 8f7cd8c..1526610 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -56,6 +56,9 @@
Climber climber_;
+ aos::monotonic_clock::time_point shooting_start_time_ =
+ aos::monotonic_clock::min_time;
+
double time_ = 0.0;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 5932d2d..6e4e08b 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -763,7 +763,7 @@
}
// Give it a lot of time to get there.
- RunFor(chrono::seconds(9));
+ RunFor(chrono::seconds(15));
VerifyNearGoal();
}
diff --git a/y2020/control_loops/superstructure/superstructure_status.fbs b/y2020/control_loops/superstructure/superstructure_status.fbs
index e8f7637..0b12d33 100644
--- a/y2020/control_loops/superstructure/superstructure_status.fbs
+++ b/y2020/control_loops/superstructure/superstructure_status.fbs
@@ -14,6 +14,9 @@
// The target speed selected by the lookup table or from manual override
// Can be compared to velocity to determine if ready.
angular_velocity_goal:double;
+
+ // Voltage error.
+ voltage_error:double;
}
table ShooterStatus {
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 75a4b26..026a50b 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -96,7 +96,13 @@
}
if (data.IsPressed(kIntakeExtend)) {
- intake_pos = 1.0;
+ intake_pos = 1.2;
+ roller_speed = 9.0f;
+ }
+
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->intake()->position() > -0.5) {
+ roller_speed = std::max(roller_speed, 6.0f);
}
if (data.IsPressed(kFeed)) {
@@ -119,7 +125,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), hood_pos,
- CreateProfileParameters(*builder.fbb(), 0.5, 1.0));
+ CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(