Add shooter superstructure code
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I559d79e1308c8ec6d434235e16f0273b4ce09f0d
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 507a914..c084047 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -149,6 +149,10 @@
// Sets the unprofiled goal which UpdateController will go to.
void set_unprofiled_goal(double position, double velocity);
+ double unprofiled_goal(int row, int col) const {
+ return profiled_subsystem_.unprofiled_goal(row, col);
+ }
+
// Returns the current position
double position() const { return profiled_subsystem_.position(); }
diff --git a/frc971/control_loops/subsystem_simulator.h b/frc971/control_loops/subsystem_simulator.h
index 0a23a38..3dc44f7 100644
--- a/frc971/control_loops/subsystem_simulator.h
+++ b/frc971/control_loops/subsystem_simulator.h
@@ -79,6 +79,9 @@
PositionSensorSimulator *encoder() { return &encoder_; }
+ double position() const { return plant_->X(0, 0); }
+ double velocity() const { return plant_->X(1, 0); }
+
private:
std::unique_ptr<CappedTestPlant> plant_;
PositionSensorSimulator encoder_;