Update how beambreak is done
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Iafa2de9e15e317686b2b7edc57b66c8fc189b49b
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 658eb10..4a3fc8e 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -15,11 +15,16 @@
EndEffectorState EndEffector::RunIteration(
const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
- bool cone_beambreak, bool cube_beambreak, double *roller_voltage) {
- bool beambreak = cone_beambreak || cube_beambreak;
-
+ double falcon_current, double cone_position, bool beambreak,
+ double *roller_voltage) {
*roller_voltage = 0.0;
+ constexpr double kMinCurrent = 20.0;
+ constexpr double kMaxConePosition = 0.92;
+
+ bool beambreak_status = (beambreak || (falcon_current > kMinCurrent &&
+ cone_position < kMaxConePosition));
+
switch (state_) {
case EndEffectorState::IDLE:
// If idle and intake requested, intake
@@ -30,7 +35,7 @@
break;
case EndEffectorState::INTAKING:
// If intaking and beam break is not triggered, keep intaking
- if (beambreak) {
+ if (beambreak_status) {
// Beam has been broken, switch to loaded.
state_ = EndEffectorState::LOADED;
break;
@@ -45,7 +50,7 @@
break;
case EndEffectorState::LOADED:
// If loaded and beam break not triggered, intake
- if (!beambreak) {
+ if (!beambreak_status) {
state_ = EndEffectorState::INTAKING;
timer_ = timestamp;
}
@@ -58,7 +63,7 @@
// If spit requested, spit
*roller_voltage = -constants::Values::kRollerVoltage();
if (beambreak_) {
- if (!beambreak) {
+ if (!beambreak_status) {
timer_ = timestamp;
}
} else if (timestamp > timer_ + constants::Values::kExtraSpittingTime()) {
@@ -69,7 +74,7 @@
break;
}
- beambreak_ = beambreak;
+ beambreak_ = beambreak_status;
return state_;
}
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 8c9b37b..b174e16 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -17,8 +17,8 @@
EndEffector();
EndEffectorState RunIteration(
const ::aos::monotonic_clock::time_point timestamp,
- RollerGoal roller_goal, bool cone_beambreak, bool cube_beambreak,
- double *intake_roller_voltage);
+ RollerGoal roller_goal, double falcon_current, double cone_position,
+ bool beambreak, double *intake_roller_voltage);
EndEffectorState state() const { return state_; }
void Reset();
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 617464b..3bd9488 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -79,8 +79,11 @@
EndEffectorState end_effector_state = end_effector_.RunIteration(
timestamp,
unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
- false,
- position->end_effector_cube_beam_break(), &output_struct.roller_voltage);
+ position->has_roller_falcon()
+ ? position->roller_falcon()->torque_current()
+ : 0.0,
+ position->cone_position(), position->end_effector_cube_beam_break(),
+ &output_struct.roller_voltage);
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));