Refactor szsdps to support the catapult controller taking over
We want to return the catapult back to the starting point using a motion
profile. We've got a nice class for this already, it just needs some
hooks to give up control. Add another interface which lets us separate
the kalman filter from the profile.
Change-Id: I3e57284394fa905e186392ed2adaec430113a172
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 86e608c..5a450ec 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -7,10 +7,9 @@
#include <utility>
#include "Eigen/Dense"
-
-#include "frc971/control_loops/control_loop.h"
#include "aos/util/trapezoid_profile.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
@@ -154,16 +153,23 @@
// Updates our estimator with the latest position.
void Correct(const typename ZeroingEstimator::Position &position);
- // Runs the controller and profile generator for a cycle.
- void Update(bool disabled);
+ // Runs the controller and profile generator for a cycle. This is equivilent
+ // to calling UpdateObserver(UpdateController()) with the rest of the syntax
+ // actually right.
+ double Update(bool disabled);
+ // Just computes the controller and pushes the feed forwards forwards 1 step.
+ double UpdateController(bool disabled);
+ // Updates the observer with the computed U.
+ // Note: if this is the only method called, ForceGoal should also be called to
+ // move the state to match.
+ void UpdateObserver(double voltage);
// Fills out the ProfiledJointStatus structure with the current state.
template <class StatusTypeBuilder>
- StatusTypeBuilder BuildStatus(
- flatbuffers::FlatBufferBuilder *fbb);
+ StatusTypeBuilder BuildStatus(flatbuffers::FlatBufferBuilder *fbb);
// Forces the current goal to the provided goal, bypassing the profiler.
- void ForceGoal(double goal);
+ void ForceGoal(double goal, double goal_velocity = 0.0);
// Sets whether to use the trapezoidal profiler or whether to just bypass it
// and pass the unprofiled goal through directly.
void set_enable_profile(bool enable) { enable_profile_ = enable; }
@@ -183,7 +189,7 @@
// Returns the requested voltage.
double voltage() const { return this->loop_->U(0, 0); }
- // Returns the current position.
+ // Returns the current position or velocity.
double position() const { return this->Y_(0, 0); }
// For testing:
@@ -285,10 +291,10 @@
builder.add_estimator_state(estimator_state);
Eigen::Matrix<double, 3, 1> error = this->controller().error();
- builder.add_position_power(
- this->controller().controller().K(0, 0) * error(0, 0));
- builder.add_velocity_power(
- this->controller().controller().K(0, 1) * error(1, 0));
+ builder.add_position_power(this->controller().controller().K(0, 0) *
+ error(0, 0));
+ builder.add_velocity_power(this->controller().controller().K(0, 1) *
+ error(1, 0));
return builder;
}
@@ -341,8 +347,9 @@
}
template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(double goal) {
- set_unprofiled_goal(goal, 0.0, false);
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(
+ double goal, double goal_velocity) {
+ set_unprofiled_goal(goal, goal_velocity, false);
this->loop_->mutable_R() = this->unprofiled_goal_;
this->loop_->mutable_next_R() = this->loop_->R();
@@ -360,7 +367,8 @@
}
template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+double SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateController(
+ bool disable) {
// TODO(austin): What do we want to do with the profile on reset? Also, we
// should probably reset R, the offset, the profile, etc.
if (this->should_reset_) {
@@ -389,12 +397,28 @@
CapGoal("next R", &this->loop_->mutable_next_R());
}
- this->loop_->Update(disable);
+ this->loop_->UpdateController(disable);
if (!disable && this->loop_->U(0, 0) != this->loop_->U_uncapped(0, 0)) {
const ::Eigen::Matrix<double, 3, 1> &R = this->loop_->R();
profile_.MoveCurrentState(R.block<2, 1>(0, 0));
}
+
+ return this->loop_->U(0, 0);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateObserver(
+ double voltage) {
+ this->loop_->mutable_U(0, 0) = voltage;
+ this->loop_->UpdateObserver(this->loop_->U(), this->loop_->plant().dt());
+}
+
+template <class ZeroingEstimator>
+double SingleDOFProfiledSubsystem<ZeroingEstimator>::Update(bool disable) {
+ const double voltage = UpdateController(disable);
+ UpdateObserver(voltage);
+ return voltage;
}
template <class ZeroingEstimator>
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 0e4be89..53cd6a2 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -349,6 +349,10 @@
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &last_U() const {
+ return last_U_;
+ }
+
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index ea47580..04d93c4 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -76,17 +76,51 @@
// Resets constrained min/max position
void clear_min_position() { min_position_ = params_.range.lower_hard; }
-
void clear_max_position() { max_position_ = params_.range.upper_hard; }
+ // Sets the unprofiled goal which UpdateController will go to.
+ void set_unprofiled_goal(double position, double velocity);
+ // Changes the profile parameters for UpdateController to track.
+ void AdjustProfile(double velocity, double acceleration);
+
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
+ Eigen::Vector3d estimated_state() const {
+ return profiled_subsystem_.X_hat();
+ }
+ double estimated_position() const { return profiled_subsystem_.X_hat(0, 0); }
+ double estimated_velocity() const { return profiled_subsystem_.X_hat(1, 0); }
+
+ // Corrects the internal state, adjusts limits, and sets nominal goals.
+ // Returns true if the controller should run.
+ bool Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled);
+
+ // Computes the feedback and feed forward steps for the current iteration.
+ // disabled should be true if the controller is disabled from Correct or
+ // another source.
+ double UpdateController(bool disabled);
+
+ // Predicts the observer state with the applied voltage.
+ void UpdateObserver(double voltage);
+
+ // Returns the current status.
+ flatbuffers::Offset<ProfiledJointStatus> MakeStatus(
+ flatbuffers::FlatBufferBuilder *status_fbb);
+
+ // Iterates the controller with the provided goal.
flatbuffers::Offset<ProfiledJointStatus> Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
const typename ZeroingEstimator::Position *position, double *output,
flatbuffers::FlatBufferBuilder *status_fbb);
+ // Sets the current profile state to solve from. Useful for when an external
+ // controller gives back control and we want the trajectory generator to
+ // take over control again.
+ void ForceGoal(double goal, double goal_velocity);
+
// Resets the profiled subsystem and returns to uninitialized
void Reset();
@@ -109,6 +143,13 @@
State state() const { return state_; }
+ bool running() const { return state_ == State::RUNNING; }
+
+ // Returns the controller.
+ const StateFeedbackLoop<3, 1, 1> &controller() const {
+ return profiled_subsystem_.controller();
+ }
+
private:
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
@@ -151,15 +192,12 @@
template <typename ZeroingEstimator, typename ProfiledJointStatus,
typename SubsystemParams>
-flatbuffers::Offset<ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
- Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- flatbuffers::FlatBufferBuilder *status_fbb) {
+bool StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::
+ Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled) {
CHECK_NOTNULL(position);
- CHECK_NOTNULL(status_fbb);
- bool disabled = output == nullptr;
profiled_subsystem_.Correct(*position);
if (profiled_subsystem_.error()) {
@@ -213,31 +251,17 @@
if (goal) {
if (goal->profile_params()) {
- profiled_subsystem_.AdjustProfile(
- goal->profile_params()->max_velocity(),
- std::min(goal->profile_params()->max_acceleration(),
- max_acceleration_));
+ AdjustProfile(goal->profile_params()->max_velocity(),
+ goal->profile_params()->max_acceleration());
} else {
- profiled_subsystem_.AdjustProfile(
- profiled_subsystem_.default_velocity(),
- std::min(profiled_subsystem_.default_acceleration(),
- static_cast<double>(max_acceleration_)));
+ AdjustProfile(profiled_subsystem_.default_velocity(),
+ profiled_subsystem_.default_acceleration());
}
- double safe_goal = goal->unsafe_goal();
- if (safe_goal < min_position_) {
- VLOG(1) << "Limiting to " << min_position_ << " from " << safe_goal;
- safe_goal = min_position_;
- }
- if (safe_goal > max_position_) {
- VLOG(1) << "Limiting to " << max_position_ << " from " << safe_goal;
- safe_goal = max_position_;
- }
if (goal->has_ignore_profile()) {
profiled_subsystem_.set_enable_profile(!goal->ignore_profile());
}
- profiled_subsystem_.set_unprofiled_goal(safe_goal,
- goal->goal_velocity());
+ set_unprofiled_goal(goal->unsafe_goal(), goal->goal_velocity());
}
} break;
@@ -254,14 +278,89 @@
profiled_subsystem_.set_max_voltage({{max_voltage}});
+ return disabled;
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::set_unprofiled_goal(double goal, double goal_velocity) {
+ if (goal < min_position_) {
+ VLOG(1) << "Limiting to " << min_position_ << " from " << goal;
+ goal = min_position_;
+ }
+ if (goal > max_position_) {
+ VLOG(1) << "Limiting to " << max_position_ << " from " << goal;
+ goal = max_position_;
+ }
+ profiled_subsystem_.set_unprofiled_goal(goal, goal_velocity);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::AdjustProfile(double velocity, double acceleration) {
+ profiled_subsystem_.AdjustProfile(
+ velocity, std::min(acceleration, static_cast<double>(max_acceleration_)));
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::
+ Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position, double *output,
+ flatbuffers::FlatBufferBuilder *status_fbb) {
+ const bool disabled = Correct(goal, position, output == nullptr);
+
// Calculate the loops for a cycle.
- profiled_subsystem_.Update(disabled);
+ const double voltage = UpdateController(disabled);
+
+ UpdateObserver(voltage);
// Write out all the voltages.
if (output) {
- *output = profiled_subsystem_.voltage();
+ *output = voltage;
}
+ return MakeStatus(status_fbb);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+double StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::UpdateController(bool disabled) {
+ return profiled_subsystem_.UpdateController(disabled);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::UpdateObserver(double voltage) {
+ profiled_subsystem_.UpdateObserver(voltage);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+void StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::ForceGoal(double goal, double goal_velocity) {
+ profiled_subsystem_.ForceGoal(goal, goal_velocity);
+}
+
+template <typename ZeroingEstimator, typename ProfiledJointStatus,
+ typename SubsystemParams>
+flatbuffers::Offset<ProfiledJointStatus>
+StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus,
+ SubsystemParams>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
+ CHECK_NOTNULL(status_fbb);
+
typename ProfiledJointStatus::Builder status_builder =
profiled_subsystem_
.template BuildStatus<typename ProfiledJointStatus::Builder>(