Add climber code without tests
This runs a loop to follow the request.
Signed-off-by: Siddhant Kanwar <kanwarsiddhant@gmail.com>
Change-Id: Iad516390561175f74eb33e0cda6a505376a372ed
diff --git a/frc971/control_loops/profiled_subsystem.fbs b/frc971/control_loops/profiled_subsystem.fbs
index 7ca61b7..049b52f 100644
--- a/frc971/control_loops/profiled_subsystem.fbs
+++ b/frc971/control_loops/profiled_subsystem.fbs
@@ -237,39 +237,42 @@
}
table RelativeEncoderProfiledJointStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool (id: 0);
+
// The state of the subsystem, if applicable. -1 otherwise.
// TODO(alex): replace with enum.
- state:int (id: 0);
+ state:int (id: 1);
// If true, we have aborted.
- estopped:bool (id: 1);
+ estopped:bool (id: 2);
// Position of the joint.
- position:float (id: 2);
+ position:float (id: 3);
// Velocity of the joint in units/second.
- velocity:float (id: 3);
+ velocity:float (id: 4);
// Profiled goal position of the joint.
- goal_position:float (id: 4);
+ goal_position:float (id: 5);
// Profiled goal velocity of the joint in units/second.
- goal_velocity:float (id: 5);
+ goal_velocity:float (id: 6);
// Unprofiled goal position from absoulte zero of the joint.
- unprofiled_goal_position:float (id: 6);
+ unprofiled_goal_position:float (id: 7);
// Unprofiled goal velocity of the joint in units/second.
- unprofiled_goal_velocity:float (id: 7);
+ unprofiled_goal_velocity:float (id: 8);
// The estimated voltage error.
- voltage_error:float (id: 8);
+ voltage_error:float (id: 9);
// The calculated velocity with delta x/delta t
- calculated_velocity:float (id: 9);
+ calculated_velocity:float (id: 10);
// Components of the control loop output
- position_power:float (id: 10);
- velocity_power:float (id: 11);
- feedforwards_power:float (id: 12);
+ position_power:float (id: 11);
+ velocity_power:float (id: 12);
+ feedforwards_power:float (id: 13);
// State of the estimator.
- estimator_state:frc971.RelativeEncoderEstimatorState (id: 13);
+ estimator_state:frc971.RelativeEncoderEstimatorState (id: 14);
}
table StaticZeroingSingleDOFProfiledSubsystemGoal {
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 c1bfe8c..ea47580 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -104,6 +104,9 @@
ESTOP,
};
+ bool zeroed() const { return profiled_subsystem_.zeroed(); }
+ bool estopped() const { return state() == State::ESTOP; }
+
State state() const { return state_; }
private:
@@ -264,7 +267,7 @@
.template BuildStatus<typename ProfiledJointStatus::Builder>(
status_fbb);
- status_builder.add_estopped(state_ == State::ESTOP);
+ status_builder.add_estopped(estopped());
status_builder.add_state(static_cast<int32_t>(state_));
return status_builder.Finish();
}