Add superstructure state machine
Managing balls from the intake into the catapult
Change-Id: I88535ee82a876d63fe49e3607c6986690d704daf
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index e4d76d3..adc13c7 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -23,7 +23,7 @@
instance.constraint_matrix =
Eigen::SparseMatrix<double, Eigen::ColMajor, osqp::c_int>(horizon,
- horizon);
+ horizon);
instance.constraint_matrix.setIdentity();
instance.lower_bounds =
@@ -313,7 +313,7 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage,
+ double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb) {
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
@@ -326,13 +326,12 @@
if (catapult_disabled) {
catapult_state_ = CatapultState::PROFILE;
} else if (catapult_.running() && unsafe_goal &&
- unsafe_goal->has_catapult() && unsafe_goal->catapult()->fire() &&
- !last_firing_) {
+ unsafe_goal->has_catapult() && fire && !last_firing_) {
catapult_state_ = CatapultState::FIRING;
}
if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
- last_firing_ = unsafe_goal->catapult()->fire();
+ last_firing_ = fire;
}
use_profile_ = true;
@@ -379,8 +378,7 @@
use_profile_ = false;
}
} else {
- if (unsafe_goal && unsafe_goal->has_catapult() &&
- !unsafe_goal->catapult()->fire()) {
+ if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
// Eh, didn't manage to solve before it was time to fire. Give up.
catapult_state_ = CatapultState::PROFILE;
}
@@ -417,7 +415,8 @@
}
}
- catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage : 0.0);
+ catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
+ : 0.0);
return catapult_.MakeStatus(fbb);
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index a8141a0..ccd4b06 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -192,6 +192,8 @@
// Resets all state for when WPILib restarts.
void Reset() { catapult_.Reset(); }
+ void Estop() { catapult_.Estop(); }
+
bool zeroed() const { return catapult_.zeroed(); }
bool estopped() const { return catapult_.estopped(); }
double solve_time() const { return catapult_mpc_.solve_time(); }
@@ -201,12 +203,16 @@
// Returns the number of shots taken.
int shot_count() const { return shot_count_; }
+ // Returns the estimated position
+ double estimated_position() const { return catapult_.estimated_position(); }
+
// Runs either the MPC or the profiled subsystem depending on if we are
// shooting or not. Returns the status.
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, flatbuffers::FlatBufferBuilder *fbb);
+ double *catapult_voltage, bool fire,
+ flatbuffers::FlatBufferBuilder *fbb);
private:
// TODO(austin): Prototype is just an encoder. Catapult has both an encoder