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/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 56e58cc..1168b88 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -2,6 +2,7 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
+#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
namespace y2022 {
@@ -48,7 +49,7 @@
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
- turret_goal_buffer;
+ turret_loading_goal_buffer;
aos::FlatbufferFixedAllocatorArray<CatapultGoal, 64> catapult_goal_buffer;
const aos::monotonic_clock::time_point timestamp =
@@ -72,6 +73,7 @@
double transfer_roller_speed_front = 0.0;
double transfer_roller_speed_back = 0.0;
double flipper_arms_voltage = 0.0;
+ bool have_active_intake_request = false;
if (unsafe_goal != nullptr) {
roller_speed_compensated_front =
@@ -112,6 +114,10 @@
catapult_goal_buffer.Finish(builder.Finish());
catapult_goal = &catapult_goal_buffer.message();
}
+
+ if (unsafe_goal->has_turret_intake()) {
+ have_active_intake_request = true;
+ }
}
// Superstructure state machine:
@@ -157,103 +163,94 @@
}
}
- const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
- transfer_roller_speed_front < 0) ||
- (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
- transfer_roller_speed_back < 0));
-
- // Intake handling should happen regardless of the turret state
- if (position->intake_beambreak_front() || position->intake_beambreak_back()) {
- if (intake_state_ == IntakeState::NO_BALL) {
- if (position->intake_beambreak_front()) {
- intake_state_ = IntakeState::INTAKE_FRONT_BALL;
- } else if (position->intake_beambreak_back()) {
- intake_state_ = IntakeState::INTAKE_BACK_BALL;
- }
- }
-
- intake_beambreak_timer_ = timestamp;
+ if (position->intake_beambreak_front()) {
+ front_intake_has_ball_ = true;
+ front_intake_beambreak_timer_ = timestamp;
}
- if (timestamp >
- intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
- intake_state_ = IntakeState::NO_BALL;
+ if (position->intake_beambreak_back()) {
+ back_intake_has_ball_ = true;
+ back_intake_beambreak_timer_ = timestamp;
}
- if (intake_state_ != IntakeState::NO_BALL) {
+ // Check if we're either spitting of have lost the ball.
+ if (transfer_roller_speed_front < 0.0 ||
+ timestamp >
+ front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ front_intake_has_ball_ = false;
+ }
+
+ if (transfer_roller_speed_back < 0.0 ||
+ timestamp >
+ back_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ back_intake_has_ball_ = false;
+ }
+
+ // Wiggle transfer rollers: send the ball back and forth while waiting
+ // for the turret or waiting for another shot to be completed
+ const double wiggle_voltage =
+ constants::Values::kTransferRollerWiggleVoltage();
+ if (front_intake_has_ball_) {
roller_speed_compensated_front = 0.0;
- roller_speed_compensated_back = 0.0;
-
- const double wiggle_voltage =
- constants::Values::kTransferRollerWiggleVoltage();
- // Wiggle transfer rollers: send the ball back and forth while waiting
- // for the turret or waiting for another shot to be completed
- if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
- if (position->intake_beambreak_front()) {
- transfer_roller_speed_front = -wiggle_voltage;
- transfer_roller_speed_back = wiggle_voltage;
- } else {
- transfer_roller_speed_front = wiggle_voltage;
- transfer_roller_speed_back = -wiggle_voltage;
- }
+ if (position->intake_beambreak_front()) {
+ transfer_roller_speed_front = -wiggle_voltage;
} else {
- if (position->intake_beambreak_back()) {
- transfer_roller_speed_back = -wiggle_voltage;
- transfer_roller_speed_front = wiggle_voltage;
- } else {
- transfer_roller_speed_back = wiggle_voltage;
- transfer_roller_speed_front = -wiggle_voltage;
- }
+ transfer_roller_speed_front = wiggle_voltage;
}
}
- // Send the turret to the loading position if we have a ball in the intake, or
- // are trying to intake one
- double turret_loading_position = std::numeric_limits<double>::quiet_NaN();
- if (state_ == SuperstructureState::TRANSFERRING &&
- intake_state_ != IntakeState::NO_BALL) {
- turret_loading_position = (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? constants::Values::kTurretFrontIntakePos()
- : constants::Values::kTurretBackIntakePos());
- } else if (state_ == SuperstructureState::IDLE && unsafe_goal != nullptr) {
- if (unsafe_goal->roller_speed_front() > 0) {
- turret_loading_position = constants::Values::kTurretFrontIntakePos();
- } else if (unsafe_goal->roller_speed_back() > 0) {
- turret_loading_position = constants::Values::kTurretBackIntakePos();
+ if (back_intake_has_ball_) {
+ roller_speed_compensated_back = 0.0;
+ if (position->intake_beambreak_back()) {
+ transfer_roller_speed_back = -wiggle_voltage;
+ } else {
+ transfer_roller_speed_back = wiggle_voltage;
}
}
- if (!std::isnan(turret_loading_position)) {
- // Turn to the loading position as close to the current position as
- // possible
- // Strategy is copied from frc971/control_loops/aiming/aiming.cc
+ // Create the goal message for putting the turret in its loading position.
+ // This will then get used in the state machine for the states (IDLE and
+ // TRANSFERRING) that use it.
+ double turret_loading_position =
+ (turret_intake_state_ == RequestedIntake::kFront
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+ // Turn to the loading position as close to the current position as
+ // possible.
+ turret_loading_position =
+ turret_.estimated_position() +
+ aos::math::NormalizeAngle(turret_loading_position -
+ turret_.estimated_position());
+ // if out of range, reset back to within +/- pi of zero.
+ if (turret_loading_position > constants::Values::kTurretRange().upper ||
+ turret_loading_position < constants::Values::kTurretRange().lower) {
turret_loading_position =
- turret_.estimated_position() +
- aos::math::NormalizeAngle(turret_loading_position -
- turret_.estimated_position());
- // if out of range, reset back to within +/- pi of zero.
- if (turret_loading_position > constants::Values::kTurretRange().upper ||
- turret_loading_position < constants::Values::kTurretRange().lower) {
- turret_loading_position =
- aos::math::NormalizeAngle(turret_loading_position);
- }
-
- turret_goal_buffer.Finish(
- frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *turret_goal_buffer.fbb(), turret_loading_position));
- turret_goal = &turret_goal_buffer.message();
+ frc971::zeroing::Wrap(constants::Values::kTurretRange().middle(),
+ turret_loading_position, 2.0 * M_PI);
}
+ turret_loading_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *turret_loading_goal_buffer.fbb(), turret_loading_position));
+
switch (state_) {
case SuperstructureState::IDLE: {
- if (is_spitting) {
- intake_state_ = IntakeState::NO_BALL;
+ // Only change the turret's goal loading position when idle, to prevent us
+ // spinning the turret around when TRANSFERRING...
+ if (front_intake_has_ball_ != back_intake_has_ball_) {
+ if (have_active_intake_request) {
+ turret_intake_state_ = unsafe_goal->turret_intake();
+ }
+ turret_intake_state_ = front_intake_has_ball_ ? RequestedIntake::kFront
+ : RequestedIntake::kBack;
+ }
+ // When IDLE with no specific intake button pressed, allow the goal
+ // message to override the intaking stuff.
+ if (have_active_intake_request || turret_goal == nullptr) {
+ turret_goal = &turret_loading_goal_buffer.message();
}
- if (intake_state_ == IntakeState::NO_BALL ||
- !(position->intake_beambreak_front() ||
- position->intake_beambreak_back())) {
+ if (!front_intake_has_ball_ && !back_intake_has_ball_) {
break;
}
@@ -263,12 +260,17 @@
break;
}
case SuperstructureState::TRANSFERRING: {
- // If we've been transferring for too long, the ball probably got lost
- if (intake_state_ == IntakeState::NO_BALL) {
+ // If we've been transferring for too long, the ball probably got lost.
+ if ((turret_intake_state_ == RequestedIntake::kFront &&
+ !front_intake_has_ball_) ||
+ (turret_intake_state_ == RequestedIntake::kBack &&
+ !back_intake_has_ball_)) {
state_ = SuperstructureState::IDLE;
break;
}
+ turret_goal = &turret_loading_goal_buffer.message();
+
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
kTurretGoalThreshold;
@@ -277,22 +279,22 @@
}
// Transfer rollers and flipper arm belt on
- if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
+ if (turret_intake_state_ == RequestedIntake::kFront) {
transfer_roller_speed_front =
constants::Values::kTransferRollerVoltage();
- transfer_roller_speed_back =
- -constants::Values::kTransferRollerVoltage();
} else {
transfer_roller_speed_back =
constants::Values::kTransferRollerVoltage();
- transfer_roller_speed_front =
- -constants::Values::kTransferRollerVoltage();
}
flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
// Ball is in catapult
if (position->turret_beambreak()) {
- intake_state_ = IntakeState::NO_BALL;
+ if (turret_intake_state_ == RequestedIntake::kFront) {
+ front_intake_has_ball_ = false;
+ } else {
+ back_intake_has_ball_ = false;
+ }
state_ = SuperstructureState::LOADING;
loading_timer_ = timestamp;
}
@@ -496,7 +498,20 @@
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
status_builder.add_state(state_);
- status_builder.add_intake_state(intake_state_);
+ if (!front_intake_has_ball_ && !back_intake_has_ball_) {
+ status_builder.add_intake_state(IntakeState::NO_BALL);
+ } else if (front_intake_has_ball_ && back_intake_has_ball_) {
+ status_builder.add_intake_state(turret_intake_state_ ==
+ RequestedIntake::kFront
+ ? IntakeState::INTAKE_FRONT_BALL
+ : IntakeState::INTAKE_BACK_BALL);
+ } else {
+ status_builder.add_intake_state(front_intake_has_ball_
+ ? IntakeState::INTAKE_FRONT_BALL
+ : IntakeState::INTAKE_BACK_BALL);
+ }
+ status_builder.add_front_intake_has_ball(front_intake_has_ball_);
+ status_builder.add_back_intake_has_ball(back_intake_has_ball_);
status_builder.add_aimer(aimer_offset);
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index e9afcc1..11cd38f 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -81,7 +81,9 @@
bool reseating_in_catapult_ = false;
bool fire_ = false;
- aos::monotonic_clock::time_point intake_beambreak_timer_ =
+ aos::monotonic_clock::time_point front_intake_beambreak_timer_ =
+ aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point back_intake_beambreak_timer_ =
aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point transferring_timer_ =
aos::monotonic_clock::min_time;
@@ -90,7 +92,9 @@
aos::monotonic_clock::time_point flipper_opening_start_time_ =
aos::monotonic_clock::min_time;
SuperstructureState state_ = SuperstructureState::IDLE;
- IntakeState intake_state_ = IntakeState::NO_BALL;
+ bool front_intake_has_ball_ = false;
+ bool back_intake_has_ball_ = false;
+ RequestedIntake turret_intake_state_ = RequestedIntake::kFront;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 7816563..7227dc2 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,12 @@
namespace y2022.control_loops.superstructure;
+// Which intake to transfer to the turret from.
+enum RequestedIntake : ubyte {
+ kFront = 0,
+ kBack = 1,
+}
+
table CatapultGoal {
// Old fire flag, only kept for backwards-compatability with logs.
// Use the fire flag in the root Goal instead
@@ -54,6 +60,9 @@
// If true, we started with the ball loaded and should proceed to that state.
preloaded:bool (id: 13);
+
+ // Specifies which intake the turret should move to to intake.
+ turret_intake:RequestedIntake (id: 14);
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index e6ff227..389c01a 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -766,6 +766,7 @@
goal_builder.add_roller_speed_back(12.0);
goal_builder.add_roller_speed_compensation(0.0);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_turret_intake(RequestedIntake::kFront);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
RunFor(std::chrono::seconds(2));
@@ -784,8 +785,8 @@
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
- // Since we spun the front rollers, the turret should be trying to intake from
- // there
+ // Since we requested the front intake, the turret should be trying to intake
+ // from there.
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -796,7 +797,7 @@
// Make sure that the turret goal is set to be loading from the front intake
// and the supersturcture is transferring from the front intake, since that
- // beambreak was trigerred. Also, the outside rollers should be stopped
+ // beambreak was trigerred. Also, the front outside rollers should be stopped.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->state(),
@@ -804,7 +805,7 @@
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_FRONT_BALL);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
RunFor(chrono::seconds(2));
@@ -814,7 +815,7 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -824,7 +825,7 @@
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
constants::Values::kTransferRollerVoltage());
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- -constants::Values::kTransferRollerVoltage());
+ 0.0);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -905,20 +906,22 @@
// move the turret.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_front() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_front() <=
+ superstructure_output_fetcher_->transfer_roller_voltage_back() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_front() >=
+ superstructure_output_fetcher_->transfer_roller_voltage_back() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- -superstructure_output_fetcher_->transfer_roller_voltage_front());
+ EXPECT_EQ(0.0,
+ superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
+ EXPECT_FALSE(superstructure_status_fetcher_->front_intake_has_ball());
+ EXPECT_TRUE(superstructure_status_fetcher_->back_intake_has_ball());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
@@ -938,7 +941,7 @@
auto catapult_offset = catapult_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_speed_front(12.0);
+ goal_builder.add_roller_speed_front(0.0);
goal_builder.add_roller_speed_back(12.0);
goal_builder.add_roller_speed_compensation(0.0);
goal_builder.add_catapult(catapult_offset);
@@ -955,7 +958,7 @@
// Now that we were asked to fire and the flippers are open,
// we should be shooting the ball and holding the flippers open.
// The turret should still be at its goal, and we should still be wiggling the
- // transfer rollers to keep the ball in the back intake
+ // transfer rollers to keep the ball in the back intake.
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
@@ -966,8 +969,8 @@
constants::Values::kTransferRollerWiggleVoltage() &&
superstructure_output_fetcher_->transfer_roller_voltage_back() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- -superstructure_output_fetcher_->transfer_roller_voltage_back());
+ EXPECT_EQ(0.0,
+ superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::SHOOTING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -986,17 +989,18 @@
superstructure_plant_.set_intake_beambreak_back(false);
RunFor(std::chrono::seconds(2));
- // After a bit, we should have completed the shot and be idle.
+ // After a bit, we should have completed the shot and be transferring.
// Since the beambreak was triggered a bit ago, it should still think a ball
- // is there
+ // is there.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
- EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
// Since the intake beambreak hasn't triggered in a while, it should realize
- // the ball was lost
+ // the ball was lost.
RunFor(std::chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
@@ -1199,7 +1203,15 @@
EXPECT_NEAR(superstructure_status_fetcher_->catapult()->position(),
constants::Values::kCatapultRange().lower, 1e-3);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
- EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ // Will still be in TRANSFERRING because we left the front beambreak on after
+ // we entered loaded, so we queued up another ball.
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
+
+ RunFor(chrono::milliseconds(2000));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::IDLE);
}
// Test that we are able to signal that the ball was preloaded
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index d802f08..92f3dbd 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -48,6 +48,9 @@
state:SuperstructureState (id: 10);
// Intaking state
intake_state:IntakeState (id: 11);
+ // Whether the front/rear intakes currently are holding balls.
+ front_intake_has_ball:bool (id: 18);
+ back_intake_has_ball:bool (id: 19);
// Whether the flippers are open for shooting
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
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) {