Add piping to SZSDPS for non-profiled goals
Change-Id: I5adeadf4f7dfc3455433e13e27635dcb479128e6
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index e4b2f25..185133b 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -237,4 +237,11 @@
unsafe_goal:double;
profile_params:frc971.ProfileParameters;
+
+ // Sets the goal velocity of the subsystem.
+ goal_velocity:double;
+
+ // If set to true, then we will ignore the profiling on this joint and pass
+ // the goal + goal velocity directly to the control loop.
+ ignore_profile:bool;
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 2752dcc..b9ec5c5 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -164,9 +164,13 @@
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
+ // 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; }
// Sets the unprofiled goal. The profiler will generate a profile to go to
// this goal.
- void set_unprofiled_goal(double unprofiled_goal);
+ void set_unprofiled_goal(double unprofiled_goal,
+ double unprofiled_goal_velocity = 0.0);
// Limits our profiles to a max velocity and acceleration for proper motion.
void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
@@ -196,6 +200,7 @@
void UpdateOffset(double offset);
aos::util::TrapezoidProfile profile_;
+ bool enable_profile_ = true;
// Current measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -338,9 +343,9 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::set_unprofiled_goal(
- double unprofiled_goal) {
+ double unprofiled_goal, double unprofiled_goal_velocity) {
this->unprofiled_goal_(0, 0) = unprofiled_goal;
- this->unprofiled_goal_(1, 0) = 0.0;
+ this->unprofiled_goal_(1, 0) = unprofiled_goal_velocity;
this->unprofiled_goal_(2, 0) = 0.0;
CapGoal("unprofiled R", &this->unprofiled_goal_);
}
@@ -357,12 +362,21 @@
}
if (!disable) {
- ::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
- this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));
+ if (enable_profile_) {
+ ::Eigen::Matrix<double, 2, 1> goal_state = profile_.Update(
+ this->unprofiled_goal_(0, 0), this->unprofiled_goal_(1, 0));
- this->loop_->mutable_next_R(0, 0) = goal_state(0, 0);
- this->loop_->mutable_next_R(1, 0) = goal_state(1, 0);
- this->loop_->mutable_next_R(2, 0) = 0.0;
+ this->loop_->mutable_next_R(0, 0) = goal_state(0, 0);
+ this->loop_->mutable_next_R(1, 0) = goal_state(1, 0);
+ this->loop_->mutable_next_R(2, 0) = 0.0;
+ } else {
+ this->loop_->mutable_R() = this->unprofiled_goal_;
+ this->loop_->mutable_next_R() = this->unprofiled_goal_;
+ this->loop_->mutable_next_R(0, 0) +=
+ this->unprofiled_goal_(1) *
+ aos::time::DurationInSeconds(this->loop_->plant().coefficients().dt);
+ CapGoal("R", &this->loop_->mutable_R());
+ }
CapGoal("next R", &this->loop_->mutable_next_R());
}
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 e115b0e..948ed5d 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -203,7 +203,11 @@
AOS_LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
safe_goal = max_position_;
}
- profiled_subsystem_.set_unprofiled_goal(safe_goal);
+ if (goal->has_ignore_profile()) {
+ profiled_subsystem_.set_enable_profile(!goal->ignore_profile());
+ }
+ profiled_subsystem_.set_unprofiled_goal(safe_goal,
+ goal->goal_velocity());
}
} break;
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index d515a07..026d372 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -288,6 +288,12 @@
const auto params_builder_offset = params_builder.Finish();
StaticZeroingSingleDOFProfiledSubsystemGoalBuilder goal_builder(fbb);
goal_builder.add_unsafe_goal(unsafe_goal->unsafe_goal());
+ if (unsafe_goal->has_goal_velocity()) {
+ goal_builder.add_goal_velocity(unsafe_goal->goal_velocity());
+ }
+ if (unsafe_goal->has_ignore_profile()) {
+ goal_builder.add_ignore_profile(unsafe_goal->ignore_profile());
+ }
goal_builder.add_profile_params(params_builder_offset);
fbb.Finish(goal_builder.Finish());
} else {
@@ -463,6 +469,65 @@
this->VerifyNearGoal();
}
+// Tests that the subsystem loop can reach a goal when the profile is disabled.
+TYPED_TEST_P(IntakeSystemTest, FunctionsWhenProfileDisabled) {
+ this->SetEnabled(true);
+ {
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ // By setting NaN for the profile, we would cause the entire system to fail
+ // or blow up if it is not ignoring the profile correctly.
+ profile_builder.add_max_velocity(std::numeric_limits<double>::quiet_NaN());
+ profile_builder.add_max_acceleration(
+ std::numeric_limits<double>::quiet_NaN());
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), 0.10, profile_builder.Finish(), 0.0, true)));
+ }
+
+ // Give it a lot of time to get there.
+ this->RunFor(chrono::seconds(8));
+
+ this->VerifyNearGoal();
+}
+
+// Tests that the subsystem loop can maintain a velocity when using the
+// goal_velocity setting.
+TYPED_TEST_P(IntakeSystemTest, MaintainConstantVelocityWithoutProfile) {
+ this->SetEnabled(true);
+
+ const double kStartingGoal = -0.10;
+ const double kVelocity = 0.05;
+ this->test_event_loop_->AddPhasedLoop(
+ [this, kStartingGoal, kVelocity](int) {
+ auto message = this->subsystem_goal_sender_.MakeBuilder();
+ auto profile_builder =
+ message.template MakeBuilder<frc971::ProfileParameters>();
+ profile_builder.add_max_velocity(0);
+ profile_builder.add_max_acceleration(0);
+ EXPECT_TRUE(message.Send(zeroing::testing::CreateSubsystemGoal(
+ *message.fbb(), kStartingGoal +
+ aos::time::DurationInSeconds(
+ this->monotonic_now().time_since_epoch()) *
+ kVelocity,
+ profile_builder.Finish(), kVelocity, true)));
+ },
+ this->dt());
+
+ const double kRunTimeSec = 4;
+ // Give time for the system to settle down--it should've been running at a
+ // constant velocity the whole time, once it converged.
+ this->RunFor(chrono::seconds(static_cast<int>(kRunTimeSec)));
+
+ EXPECT_TRUE(this->subsystem_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(kStartingGoal + kVelocity * kRunTimeSec,
+ this->subsystem_status_fetcher_->position(), 0.001);
+ EXPECT_NEAR(kStartingGoal + kVelocity * kRunTimeSec,
+ this->subsystem_plant_.subsystem_position(), 0.001);
+ EXPECT_NEAR(kVelocity, this->subsystem_status_fetcher_->velocity(), 0.001);
+}
+
// Makes sure that the voltage on a motor is properly pulled back after
// saturation such that we don't get weird or bad (e.g. oscillating) behaviour.
TYPED_TEST_P(IntakeSystemTest, SaturationTest) {
@@ -755,6 +820,8 @@
}
REGISTER_TYPED_TEST_CASE_P(IntakeSystemTest, DoesNothing, ReachesGoal,
+ FunctionsWhenProfileDisabled,
+ MaintainConstantVelocityWithoutProfile,
SaturationTest, RespectsRange, ZeroTest, ZeroNoGoal,
LowerHardstopStartup, UpperHardstopStartup,
ResetTest, DisabledGoalTest, DisabledZeroTest,
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
index e39272a..da6e7ae 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.fbs
@@ -6,6 +6,8 @@
table SubsystemGoal {
unsafe_goal:double;
profile_params:frc971.ProfileParameters;
+ goal_velocity:double;
+ ignore_profile:bool;
}
table SubsystemOutput {