Refactor handling of dual intaking
* Add flag to goal message to explicitly indicate which intake we are
requesting, rather than attempting to infer it from the exact settings
of the intake roller speeds.
* While idle, latch the turret to one of the two intaking locations,
chosen based on whichever intake was either last requested or which
intake currently has a ball (if only one has a ball).
* Clean up some of the logic to operate the front/back intakes
independently since we actually have separate transfer rollers for them.
Change-Id: Ib97052a011fe01784077a7b66fccbcc02aa49952
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 8423f47..2820114 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -163,11 +163,13 @@
double intake_back_pos = 1.47;
double transfer_roller_front_speed = 0.0;
double transfer_roller_back_speed = 0.0;
+ std::optional<control_loops::superstructure::RequestedIntake>
+ requested_intake;
double roller_front_speed = 0.0;
double roller_back_speed = 0.0;
- double turret_pos = 0.0;
+ std::optional<double> turret_pos = 0.0;
double catapult_pos = 0.03;
double catapult_speed = 18.0;
@@ -191,8 +193,6 @@
} else {
turret_pos = -1.5;
}
- } else {
- turret_pos = 0.0;
}
if (setpoint_fetcher_.get()) {
@@ -217,15 +217,15 @@
if (data.IsPressed(kIntakeFrontOut)) {
intake_front_pos = kIntakePosition;
transfer_roller_front_speed = kTransferRollerSpeed;
- transfer_roller_back_speed = -kTransferRollerSpeed;
intake_front_counter_ = kIntakeCounterIterations;
+ intake_back_counter_ = 0;
} else if (data.IsPressed(kIntakeBackOut)) {
intake_back_pos = kIntakePosition;
transfer_roller_back_speed = kTransferRollerSpeed;
- transfer_roller_front_speed = -kTransferRollerSpeed;
intake_back_counter_ = kIntakeCounterIterations;
+ intake_front_counter_ = 0;
} else if (data.IsPressed(kSpit)) {
transfer_roller_front_speed = -kTransferRollerSpeed;
transfer_roller_back_speed = -kTransferRollerSpeed;
@@ -238,10 +238,12 @@
if (intake_front_counter_ > 0) {
intake_front_counter_--;
roller_front_speed = kRollerSpeed;
+ requested_intake = control_loops::superstructure::RequestedIntake::kFront;
}
if (intake_back_counter_ > 0) {
intake_back_counter_--;
roller_back_speed = kRollerSpeed;
+ requested_intake = control_loops::superstructure::RequestedIntake::kBack;
}
if (data.IsPressed(kFire)) {
@@ -263,9 +265,12 @@
CreateProfileParameters(*builder.fbb(), 8.0, 40.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
- turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), turret_pos,
- CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
+ turret_offset;
+ if (turret_pos.has_value()) {
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), turret_pos.value(),
+ CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
+ }
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
catapult_return_offset =
@@ -296,8 +301,10 @@
transfer_roller_front_speed);
superstructure_goal_builder.add_transfer_roller_speed_back(
transfer_roller_back_speed);
- superstructure_goal_builder.add_auto_aim(
- data.IsPressed(kAutoAim));
+ superstructure_goal_builder.add_auto_aim(data.IsPressed(kAutoAim));
+ if (requested_intake.has_value()) {
+ superstructure_goal_builder.add_turret_intake(requested_intake.value());
+ }
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {