Added getter for profiled_subsystem and missing status message.
Change-Id: I9a333adbf2e72232891552092d5a555c5fc379d6
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 379cdc9..7dbd1a8 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -15,6 +15,7 @@
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
+#include "frc971/constants.h"
namespace frc971 {
namespace control_loops {
@@ -171,6 +172,8 @@
// Triggers an estimator error.
void TriggerEstimatorError() { estimators_[0].TriggerError(); }
+ const ::frc971::constants::Range &range() const { return range_; }
+
private:
// Limits the provided goal to the soft limits. Prints "name" when it fails
// to aid debugging.
@@ -235,6 +238,7 @@
void SingleDOFProfiledSubsystem<ZeroingEstimator>::PopulateStatus(
ProfiledJointStatus *status) {
status->zeroed = zeroed();
+ status->state = -1;
// We don't know, so default to the bad case.
status->estopped = true;
diff --git a/frc971/control_loops/profiled_subsystem.q b/frc971/control_loops/profiled_subsystem.q
new file mode 100644
index 0000000..11d641d
--- /dev/null
+++ b/frc971/control_loops/profiled_subsystem.q
@@ -0,0 +1,41 @@
+package frc971.control_loops;
+
+import "frc971/control_loops/control_loops.q";
+
+struct ProfiledJointStatus {
+ // Is the turret zeroed?
+ bool zeroed;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ int32_t state;
+
+ // If true, we have aborted.
+ bool estopped;
+
+ // Position of the joint.
+ float position;
+ // Velocity of the joint in units/second.
+ float velocity;
+ // Profiled goal position of the joint.
+ float goal_position;
+ // Profiled goal velocity of the joint in units/second.
+ float goal_velocity;
+ // Unprofiled goal position from absoulte zero of the joint.
+ float unprofiled_goal_position;
+ // Unprofiled goal velocity of the joint in units/second.
+ float unprofiled_goal_velocity;
+
+ // The estimated voltage error.
+ float voltage_error;
+
+ // The calculated velocity with delta x/delta t
+ float calculated_velocity;
+
+ // Components of the control loop output
+ float position_power;
+ float velocity_power;
+ float feedforwards_power;
+
+ // State of the estimator.
+ .frc971.EstimatorState estimator_state;
+};