Factored out the profiled subsystem status.
Change-Id: I03df9788bbe98d12e80dd70db8cd09d9b3651724
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8ceeace..8f8b381 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -148,6 +148,16 @@
],
)
+queue_library(
+ name = 'profiled_subsystem_queue',
+ srcs = [
+ 'profiled_subsystem.q',
+ ],
+ deps = [
+ ':queues',
+ ],
+)
+
cc_library(
name = 'profiled_subsystem',
srcs = [
@@ -157,6 +167,7 @@
'profiled_subsystem.h',
],
deps = [
+ ':profiled_subsystem_queue',
':simple_capped_state_feedback_loop',
':state_feedback_loop',
'//aos/common/controls:control_loop',
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 9444b66..379cdc9 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -2,6 +2,7 @@
#define FRC971_CONTROL_LOOPS_PROFILED_SUBSYSTEM_H_
#include <array>
+#include <chrono>
#include <memory>
#include <utility>
@@ -9,6 +10,8 @@
#include "aos/common/controls/control_loop.h"
#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/profiled_subsystem.q.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
@@ -142,23 +145,27 @@
// Runs the controller and profile generator for a cycle.
void Update(bool disabled);
+ // Fills out the ProfiledJointStatus structure with the current state.
+ void PopulateStatus(ProfiledJointStatus *status);
+
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal);
// Sets the unprofiled goal. The profiler will generate a profile to go to
// this goal.
void set_unprofiled_goal(double unprofiled_goal);
// 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,
double max_angular_acceleration);
// Returns true if we have exceeded any hard limits.
bool CheckHardLimits();
- // Returns the requested intake voltage.
- double intake_voltage() const { return loop_->U(0, 0); }
+ // Returns the requested voltage.
+ double voltage() const { return loop_->U(0, 0); }
// Returns the current position.
- double angle() const { return Y_(0, 0); }
+ double position() const { return Y_(0, 0); }
// For testing:
// Triggers an estimator error.
@@ -182,6 +189,8 @@
const double default_velocity_;
const double default_acceleration_;
+
+ double last_position_ = 0;
};
namespace internal {
@@ -213,6 +222,7 @@
loop_->mutable_X_hat()(0, 0) += doffset;
Y_(0, 0) += doffset;
+ last_position_ += doffset;
loop_->mutable_R(0, 0) += doffset;
profile_.MoveGoal(doffset);
@@ -222,12 +232,39 @@
}
template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
+ ProfiledJointStatus *status) {
+ status->zeroed = zeroed();
+ // We don't know, so default to the bad case.
+ status->estopped = true;
+
+ status->position = X_hat(0, 0);
+ status->velocity = X_hat(1, 0);
+ status->goal_position = goal(0, 0);
+ status->goal_velocity = goal(1, 0);
+ status->unprofiled_goal_position = unprofiled_goal(0, 0);
+ status->unprofiled_goal_velocity = unprofiled_goal(1, 0);
+ status->voltage_error = X_hat(2, 0);
+ status->calculated_velocity =
+ (position() - last_position_) /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+
+ status->estimator_state = EstimatorState(0);
+
+ Eigen::Matrix<double, 3, 1> error = controller().error();
+ status->position_power = controller().K(0, 0) * error(0, 0);
+ status->velocity_power = controller().K(0, 1) * error(1, 0);
+}
+
+template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::Correct(
- typename ZeroingEstimator::Position position) {
- estimators_[0].UpdateEstimate(position);
+ typename ZeroingEstimator::Position new_position) {
+ estimators_[0].UpdateEstimate(new_position);
if (estimators_[0].error()) {
- LOG(ERROR, "zeroing error with intake_estimator\n");
+ LOG(ERROR, "zeroing error\n");
return;
}
@@ -243,7 +280,8 @@
set_zeroed(0, true);
}
- Y_ << position.encoder;
+ last_position_ = position();
+ Y_ << new_position.encoder;
Y_ += offset_;
loop_->Correct(Y_);
}
@@ -251,14 +289,14 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::CapGoal(
const char *name, Eigen::Matrix<double, 3, 1> *goal) {
- // Limit the goal to min/max allowable angles.
+ // Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
- LOG(WARNING, "Intake goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
range_.upper);
(*goal)(0, 0) = range_.upper;
}
if ((*goal)(0, 0) < range_.lower) {
- LOG(WARNING, "Intake goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
range_.lower);
(*goal)(0, 0) = range_.lower;
}
@@ -305,10 +343,10 @@
bool SingleDOFProfiledSubsystem<ZeroingEstimator>::CheckHardLimits() {
// Returns whether hard limits have been exceeded.
- if (angle() > range_.upper_hard || angle() < range_.lower_hard) {
+ if (position() > range_.upper_hard || position() < range_.lower_hard) {
LOG(ERROR,
"SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
- angle(), range_.lower_hard, range_.upper_hard);
+ position(), range_.lower_hard, range_.upper_hard);
return true;
}
@@ -317,6 +355,13 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
+ const ::frc971::ProfileParameters &profile_parameters) {
+ AdjustProfile(profile_parameters.max_velocity,
+ profile_parameters.max_acceleration);
+}
+
+template <class ZeroingEstimator>
+void SingleDOFProfiledSubsystem<ZeroingEstimator>::AdjustProfile(
double max_angular_velocity, double max_angular_acceleration) {
profile_.set_maximum_velocity(
internal::UseUnlessZero(max_angular_velocity, default_velocity_));