Make profile type a template parameter
We'd like to be able to swap in more complicated profiles for the
catapult. To do that, make it a template parameter.
AdjustProfile went away because the more complicated profiles don't have
to have the same concept of what the parameters are.
Change-Id: Ie0fc08c6b3454b3e80c7608c39d4c9515bc118bf
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/catapult/catapult.cc b/frc971/control_loops/catapult/catapult.cc
index 2e1867c..a827dc9 100644
--- a/frc971/control_loops/catapult/catapult.cc
+++ b/frc971/control_loops/catapult/catapult.cc
@@ -102,9 +102,11 @@
case CatapultState::RESETTING:
if (catapult_.controller().R(1, 0) > 7.0) {
- catapult_.AdjustProfile(7.0, 2000.0);
+ catapult_.mutable_profile()->set_maximum_velocity(7.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(2000.0);
} else if (catapult_.controller().R(1, 0) > 0.0) {
- catapult_.AdjustProfile(7.0, 1000.0);
+ catapult_.mutable_profile()->set_maximum_velocity(7.0);
+ catapult_.mutable_profile()->set_maximum_acceleration(1000.0);
} else {
catapult_state_ = CatapultState::PROFILE;
}
@@ -135,4 +137,4 @@
return catapult_.MakeStatus(fbb);
}
-} // namespace frc971::control_loops::catapult
\ No newline at end of file
+} // namespace frc971::control_loops::catapult
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 7a0b7fa..599c4da 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -148,7 +148,8 @@
};
template <typename ZeroingEstimator =
- ::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
+ ::frc971::zeroing::PotAndIndexPulseZeroingEstimator,
+ class Profile = aos::util::TrapezoidProfile>
class SingleDOFProfiledSubsystem
: public ::frc971::control_loops::ProfiledSubsystem<3, 1,
ZeroingEstimator> {
@@ -209,6 +210,9 @@
double default_velocity() const { return default_velocity_; }
double default_acceleration() const { return default_acceleration_; }
+ // Returns a pointer to the profile in use.
+ Profile *mutable_profile() { return &profile_; }
+
protected:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -218,7 +222,7 @@
private:
void UpdateOffset(double offset);
- aos::util::TrapezoidProfile profile_;
+ Profile profile_;
bool enable_profile_ = true;
// Current measurement.
@@ -240,12 +244,13 @@
} // namespace internal
-template <class ZeroingEstimator>
-SingleDOFProfiledSubsystem<ZeroingEstimator>::SingleDOFProfiledSubsystem(
- ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
- const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
- const ::frc971::constants::Range &range, double default_velocity,
- double default_acceleration)
+template <class ZeroingEstimator, class Profile>
+SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::
+ SingleDOFProfiledSubsystem(
+ ::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
+ const typename ZeroingEstimator::ZeroingConstants &zeroing_constants,
+ const ::frc971::constants::Range &range, double default_velocity,
+ double default_acceleration)
: ProfiledSubsystem<3, 1, ZeroingEstimator>(
::std::move(loop), {{ZeroingEstimator(zeroing_constants)}}),
profile_(this->loop_->plant().coefficients().dt),
@@ -254,11 +259,11 @@
default_acceleration_(default_acceleration) {
Y_.setZero();
offset_.setZero();
- AdjustProfile(0.0, 0.0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::UpdateOffset(
+ double offset) {
const double doffset = offset - offset_(0, 0);
AOS_LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
@@ -273,9 +278,10 @@
CapGoal("R", &this->loop_->mutable_R());
}
-template <class ZeroingEstimator>
+template <class ZeroingEstimator, class Profile>
template <class StatusTypeBuilder>
-StatusTypeBuilder SingleDOFProfiledSubsystem<ZeroingEstimator>::BuildStatus(
+StatusTypeBuilder
+SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::BuildStatus(
flatbuffers::FlatBufferBuilder *fbb) {
flatbuffers::Offset<typename ZeroingEstimator::State> estimator_state =
this->EstimatorState(fbb, 0);
@@ -306,8 +312,8 @@
return builder;
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::Correct(
const typename ZeroingEstimator::Position &new_position) {
this->estimators_[0].UpdateEstimate(new_position);
@@ -336,8 +342,8 @@
this->X_hat_ = this->loop_->X_hat();
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::CapGoal(
const char *name, Eigen::Matrix<double, 3, 1> *goal, bool print) {
// Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
@@ -356,8 +362,8 @@
}
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::ForceGoal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::ForceGoal(
double goal, double goal_velocity) {
set_unprofiled_goal(goal, goal_velocity, false);
this->loop_->mutable_R() = this->unprofiled_goal_;
@@ -367,8 +373,8 @@
this->profile_.MoveCurrentState(R.block<2, 1>(0, 0));
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::set_unprofiled_goal(
double unprofiled_goal, double unprofiled_goal_velocity, bool print) {
this->unprofiled_goal_(0, 0) = unprofiled_goal;
this->unprofiled_goal_(1, 0) = unprofiled_goal_velocity;
@@ -376,8 +382,8 @@
CapGoal("unprofiled R", &this->unprofiled_goal_, print);
}
-template <class ZeroingEstimator>
-double SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateController(
+template <class ZeroingEstimator, class Profile>
+double SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::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.
@@ -418,22 +424,23 @@
return this->loop_->U(0, 0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateObserver(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::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) {
+template <class ZeroingEstimator, class Profile>
+double SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::Update(
+ bool disable) {
const double voltage = UpdateController(disable);
UpdateObserver(voltage);
return voltage;
}
-template <class ZeroingEstimator>
-bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
+template <class ZeroingEstimator, class Profile>
+bool SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
if (position() > range_.upper_hard || position() < range_.lower_hard) {
@@ -447,8 +454,8 @@
return false;
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::AdjustProfile(
const ::frc971::ProfileParameters *profile_parameters) {
AdjustProfile(
profile_parameters != nullptr ? profile_parameters->max_velocity() : 0.0,
@@ -456,8 +463,8 @@
: 0.0);
}
-template <class ZeroingEstimator>
-void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+template <class ZeroingEstimator, class Profile>
+void SingleDOFProfiledSubsystem<ZeroingEstimator, Profile>::AdjustProfile(
double max_angular_velocity, double max_angular_acceleration) {
profile_.set_maximum_velocity(
internal::UseUnlessZero(max_angular_velocity, default_velocity_));
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 069914e..507a914 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -96,7 +96,8 @@
// Class for controlling and motion profiling a single degree of freedom
// subsystem with a zeroing strategy of not moving.
template <typename TZeroingEstimator, typename TProfiledJointStatus,
- typename TSubsystemParams = TZeroingEstimator>
+ typename TSubsystemParams = TZeroingEstimator,
+ typename TProfile = aos::util::TrapezoidProfile>
class StaticZeroingSingleDOFProfiledSubsystem {
public:
// Constructs the subsystem from flatbuffer types (appropriate when using the
@@ -147,8 +148,6 @@
// 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(); }
@@ -178,6 +177,12 @@
flatbuffers::Offset<ProfiledJointStatus> MakeStatus(
flatbuffers::FlatBufferBuilder *status_fbb);
+ // 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) {
+ profiled_subsystem_.set_enable_profile(enable);
+ }
+
// Iterates the controller with the provided goal.
flatbuffers::Offset<ProfiledJointStatus> Iterate(
const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
@@ -220,6 +225,9 @@
return profiled_subsystem_.controller();
}
+ // Returns a pointer to the profile in use.
+ TProfile *mutable_profile() { return profiled_subsystem_.mutable_profile(); }
+
private:
State state_ = State::UNINITIALIZED;
double min_position_, max_position_;
@@ -227,14 +235,15 @@
const StaticZeroingSingleDOFProfiledSubsystemParams<TSubsystemParams> params_;
- ::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator>
+ ::frc971::control_loops::SingleDOFProfiledSubsystem<ZeroingEstimator,
+ TProfile>
profiled_subsystem_;
};
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
+ SubsystemParams, Profile>::
StaticZeroingSingleDOFProfiledSubsystem(
const StaticZeroingSingleDOFProfiledSubsystemParams<SubsystemParams>
¶ms)
@@ -251,9 +260,9 @@
};
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::Reset() {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams, Profile>::Reset() {
state_ = State::UNINITIALIZED;
clear_min_position();
clear_max_position();
@@ -261,12 +270,12 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
bool StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus, SubsystemParams>::
- Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position,
- bool disabled) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::Correct(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
+ const typename ZeroingEstimator::Position *position,
+ bool disabled) {
CHECK_NOTNULL(position);
profiled_subsystem_.Correct(*position);
@@ -296,8 +305,9 @@
// jump.
profiled_subsystem_.ForceGoal(profiled_subsystem_.position());
// Set up the profile to be the zeroing profile.
- profiled_subsystem_.AdjustProfile(
- params_.zeroing_profile_params.max_velocity,
+ mutable_profile()->set_maximum_velocity(
+ params_.zeroing_profile_params.max_velocity);
+ mutable_profile()->set_maximum_acceleration(
params_.zeroing_profile_params.max_acceleration);
// We are not ready to start doing anything yet.
@@ -321,11 +331,20 @@
if (goal) {
if (goal->profile_params()) {
- AdjustProfile(goal->profile_params()->max_velocity(),
- goal->profile_params()->max_acceleration());
+ mutable_profile()->set_maximum_velocity(
+ internal::UseUnlessZero(goal->profile_params()->max_velocity(),
+ profiled_subsystem_.default_velocity()));
+ mutable_profile()->set_maximum_acceleration(
+ std::min(static_cast<double>(max_acceleration_),
+ internal::UseUnlessZero(
+ goal->profile_params()->max_acceleration(),
+ profiled_subsystem_.default_acceleration())));
} else {
- AdjustProfile(profiled_subsystem_.default_velocity(),
- profiled_subsystem_.default_acceleration());
+ mutable_profile()->set_maximum_velocity(
+ profiled_subsystem_.default_velocity());
+ mutable_profile()->set_maximum_acceleration(
+ std::min(static_cast<double>(max_acceleration_),
+ profiled_subsystem_.default_acceleration()));
}
if (goal->has_ignore_profile()) {
@@ -352,10 +371,10 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::set_unprofiled_goal(double goal, double goal_velocity) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::set_unprofiled_goal(double goal, double goal_velocity) {
if (goal < min_position_) {
VLOG(1) << "Limiting to " << min_position_ << " from " << goal;
goal = min_position_;
@@ -368,22 +387,14 @@
}
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>
+ typename SubsystemParams, typename Profile>
flatbuffers::Offset<ProfiledJointStatus>
-StaticZeroingSingleDOFProfiledSubsystem<ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::
- Iterate(const StaticZeroingSingleDOFProfiledSubsystemGoal *goal,
- const typename ZeroingEstimator::Position *position, double *output,
- flatbuffers::FlatBufferBuilder *status_fbb) {
+StaticZeroingSingleDOFProfiledSubsystem<
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::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.
@@ -400,35 +411,35 @@
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
double StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::UpdateController(bool disabled) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::UpdateController(bool disabled) {
return profiled_subsystem_.UpdateController(disabled);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::UpdateObserver(double voltage) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::UpdateObserver(double voltage) {
profiled_subsystem_.UpdateObserver(voltage);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
void StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::ForceGoal(double goal, double goal_velocity) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::ForceGoal(double goal, double goal_velocity) {
profiled_subsystem_.ForceGoal(goal, goal_velocity);
}
template <typename ZeroingEstimator, typename ProfiledJointStatus,
- typename SubsystemParams>
+ typename SubsystemParams, typename Profile>
flatbuffers::Offset<ProfiledJointStatus>
StaticZeroingSingleDOFProfiledSubsystem<
- ZeroingEstimator, ProfiledJointStatus,
- SubsystemParams>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
+ ZeroingEstimator, ProfiledJointStatus, SubsystemParams,
+ Profile>::MakeStatus(flatbuffers::FlatBufferBuilder *status_fbb) {
CHECK_NOTNULL(status_fbb);
typename ProfiledJointStatus::Builder status_builder =