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/BUILD b/y2022/control_loops/superstructure/BUILD
index ac91668..2efb322 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -77,6 +77,7 @@
],
deps = [
":collision_avoidance_lib",
+ ":superstructure_can_position_fbs",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
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
diff --git a/y2022/control_loops/superstructure/collision_avoidance.cc b/y2022/control_loops/superstructure/collision_avoidance.cc
index 5d0fe27..b39b48f 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance.cc
@@ -67,8 +67,10 @@
return false;
}
-void CollisionAvoidance::UpdateGoal(const CollisionAvoidance::Status &status,
- const Goal *unsafe_goal) {
+void CollisionAvoidance::UpdateGoal(
+ const CollisionAvoidance::Status &status,
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *unsafe_turret_goal) {
// Start with our constraints being wide open.
clear_max_turret_goal();
clear_min_turret_goal();
@@ -81,10 +83,9 @@
const double intake_back_position = status.intake_back_position;
const double turret_position = status.turret_position;
- const double turret_goal =
- (unsafe_goal != nullptr && unsafe_goal->turret() != nullptr
- ? unsafe_goal->turret()->unsafe_goal()
- : std::numeric_limits<double>::quiet_NaN());
+ const double turret_goal = (unsafe_turret_goal != nullptr
+ ? unsafe_turret_goal->unsafe_goal()
+ : std::numeric_limits<double>::quiet_NaN());
// Calculating the avoidance with either intake, and when the turret is
// wrapped.
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index 94b454c..7c25964 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -69,7 +69,10 @@
double min_turret_collision_position,
double max_turret_collision_position);
// Checks and alters goals to make sure they're safe.
- void UpdateGoal(const Status &status, const Goal *unsafe_goal);
+ void UpdateGoal(
+ const Status &status,
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *unsafe_turret_goal);
// Limits if goal is in collision spots.
void CalculateAvoidance(bool intake_front, double intake_position,
double turret_goal, double mix_turret_collision_goal,
diff --git a/y2022/control_loops/superstructure/collision_avoidance_test.cc b/y2022/control_loops/superstructure/collision_avoidance_test.cc
index 616a10b..43695d1 100644
--- a/y2022/control_loops/superstructure/collision_avoidance_test.cc
+++ b/y2022/control_loops/superstructure/collision_avoidance_test.cc
@@ -2,8 +2,8 @@
#include "aos/commonmath.h"
#include "aos/flatbuffers.h"
-#include "gtest/gtest.h"
#include "gmock/gmock.h"
+#include "gtest/gtest.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
@@ -84,22 +84,18 @@
status_({0.0, 0.0, 0.0}),
prev_status_({0.0, 0.0, 0.0}) {}
- int t = 0;
- int d = 0;
void Simulate() {
FlatbufferDetachedBuffer<Goal> safe_goal = MakeZeroGoal();
- t++;
// Don't simulate if already collided
if (avoidance_.IsCollided(status_)) {
- d++;
return;
}
bool moving = true;
while (moving) {
// Compute the safe goal
- avoidance_.UpdateGoal(status_, &unsafe_goal_.message());
+ avoidance_.UpdateGoal(status_, unsafe_goal_.message().turret());
// The system should never be collided
ASSERT_FALSE(avoidance_.IsCollided(status_));
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index bf94b08..aa5eee8 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -21,10 +21,12 @@
intake_front_(values_->intake_front.subsystem_params),
intake_back_(values_->intake_back.subsystem_params),
turret_(values_->turret.subsystem_params),
+ catapult_(*values_),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- catapult_(*values_) {
+ can_position_fetcher_(
+ event_loop->MakeFetcher<CANPosition>("/superstructure")) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -32,8 +34,6 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- OutputT output_struct;
-
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_front_.Reset();
@@ -43,27 +43,28 @@
catapult_.Reset();
}
- collision_avoidance_.UpdateGoal(
- {.intake_front_position = intake_front_.estimated_position(),
- .intake_back_position = intake_back_.estimated_position(),
- .turret_position = turret_.estimated_position()},
- unsafe_goal);
+ OutputT output_struct;
- turret_.set_min_position(collision_avoidance_.min_turret_goal());
- turret_.set_max_position(collision_avoidance_.max_turret_goal());
- intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
- intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
- intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
- intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+ turret_goal_buffer;
+
+ const aos::monotonic_clock::time_point timestamp =
+ event_loop()->context().monotonic_event_time;
drivetrain_status_fetcher_.Fetch();
const float velocity = robot_velocity();
- double roller_speed_compensated_front = 0;
- double roller_speed_compensated_back = 0;
- double transfer_roller_speed = 0;
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *turret_goal = nullptr;
+ double roller_speed_compensated_front = 0.0;
+ double roller_speed_compensated_back = 0.0;
+ double transfer_roller_speed = 0.0;
+ double flipper_arms_voltage = 0.0;
if (unsafe_goal != nullptr) {
+ turret_goal = unsafe_goal->turret();
+
roller_speed_compensated_front =
unsafe_goal->roller_speed_front() +
std::max(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
@@ -75,45 +76,303 @@
transfer_roller_speed = unsafe_goal->transfer_roller_speed();
}
+ // TODO: Aimer sets turret_goal here
+ // Supersturcture state machine:
+ // 1. IDLE: Wait until an intake beambreak is triggerred, meaning that a ball
+ // is being intaked. This means that the transfer rollers have a ball. If
+ // we've been waiting here for too long without any beambreak triggered, the
+ // ball got lost, so reset.
+ // 2. TRANSFERRING: Until the turret reaches the loading position where the
+ // ball can be transferred into the catapult, wiggle the ball in place.
+ // Once the turret reaches the loading position, send the ball forward with
+ // the transfer rollers until the turret beambreak is triggered.
+ // If we have been in this state for too long, the ball probably got lost so
+ // reset back to IDLE.
+ // 3. LOADING: To load the ball into the catapult, put the flippers at the
+ // feeding speed. Wait for a timeout, and then wait until the ball has gone
+ // past the turret beambreak and the flippers have stopped moving, meaning
+ // that the ball is fully loaded in the catapult.
+ // 4. LOADED: Wait until the user asks us to fire to transition to the
+ // shooting stage. If asked to cancel the shot, reset back to the IDLE state.
+ // 5. SHOOTING: Open the flippers to get ready for the shot. If they don't
+ // open quickly enough, try reseating the ball and going back to the LOADING
+ // stage, which moves the flippers in the opposite direction first.
+ // Now, hold the flippers open and wait until the turret has reached its
+ // aiming goal. Once the turret is ready, tell the catapult to fire.
+ // If the flippers move back for some reason now, it could damage the
+ // catapult, so estop it. Otherwise, wait until the catapult shoots a ball and
+ // goes back to its return position. We have now finished the shot, so return
+ // to IDLE.
+
+ const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
+ transfer_roller_speed < 0) ||
+ (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
+ transfer_roller_speed > 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 (intake_state_ != IntakeState::NO_BALL) {
+ // Block intaking in
+ roller_speed_compensated_front = 0.0;
+ roller_speed_compensated_back = 0.0;
+
+ const double wiggle_voltage =
+ (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTransferRollerFrontWiggleVoltage()
+ : constants::Values::kTransferRollerBackWiggleVoltage());
+ // 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 &&
+ position->intake_beambreak_front()) ||
+ (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
+ position->intake_beambreak_back())) {
+ transfer_roller_speed = -wiggle_voltage;
+ } else {
+ transfer_roller_speed = wiggle_voltage;
+ }
+ }
+
+ switch (state_) {
+ case SuperstructureState::IDLE: {
+ if (timestamp >
+ intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ intake_state_ = IntakeState::NO_BALL;
+ }
+
+ if (is_spitting) {
+ intake_state_ = IntakeState::NO_BALL;
+ }
+
+ if (intake_state_ == IntakeState::NO_BALL ||
+ !(position->intake_beambreak_front() ||
+ position->intake_beambreak_back())) {
+ break;
+ }
+
+ state_ = SuperstructureState::TRANSFERRING;
+ transferring_timer_ = timestamp;
+
+ // Save the side the ball is on for later
+
+ break;
+ }
+ case SuperstructureState::TRANSFERRING: {
+ // If we've been transferring for too long, the ball probably got lost
+ if (timestamp >
+ transferring_timer_ + constants::Values::kBallLostTime()) {
+ intake_state_ = IntakeState::NO_BALL;
+ break;
+ }
+
+ if (intake_state_ == IntakeState::NO_BALL) {
+ state_ = SuperstructureState::IDLE;
+ break;
+ }
+
+ double turret_loading_position =
+ (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+
+ turret_goal_buffer.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *turret_goal_buffer.fbb(), turret_loading_position));
+ turret_goal = &turret_goal_buffer.message();
+
+ const bool turret_near_goal =
+ std::abs(turret_.estimated_position() - turret_loading_position) <
+ kTurretGoalThreshold;
+ if (!turret_near_goal) {
+ break; // Wait for turret to reach the chosen intake
+ }
+
+ // Transfer rollers and flipper arm belt on
+ transfer_roller_speed =
+ (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTransferRollerFrontVoltage()
+ : constants::Values::kTransferRollerBackVoltage());
+ flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
+
+ // Ball is in catapult
+ if (position->turret_beambreak()) {
+ intake_state_ = IntakeState::NO_BALL;
+ state_ = SuperstructureState::LOADING;
+ loading_timer_ = timestamp;
+ }
+ break;
+ }
+ case SuperstructureState::LOADING: {
+ flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
+
+ // Keep feeding for kExtraLoadingTime
+
+ can_position_fetcher_.Fetch();
+ const bool flipper_arm_roller_is_stopped =
+ can_position_fetcher_.get() != nullptr &&
+ std::abs(
+ can_position_fetcher_->flipper_arm_integrated_sensor_velocity()) <
+ 0.01;
+
+ const bool reading_is_recent =
+ can_position_fetcher_.get() != nullptr &&
+ (timestamp < can_position_fetcher_.context().monotonic_event_time +
+ constants::Values::kFlipperVelocityValidTime());
+
+ // The ball should go past the turret beambreak to be loaded.
+ // If we got a CAN reading not too long ago, the flippers should have also
+ // stopped.
+ // TODO(milind): maybe it's better to update loading_timer_ as long as the
+ // turret beambreak is triggered.
+ if (timestamp > loading_timer_ + constants::Values::kExtraLoadingTime() &&
+ !position->turret_beambreak() &&
+ (flipper_arm_roller_is_stopped || !reading_is_recent)) {
+ state_ = SuperstructureState::LOADED;
+ reseating_in_catapult_ = false;
+ }
+ break;
+ }
+ case SuperstructureState::LOADED: {
+ if (unsafe_goal != nullptr) {
+ if (unsafe_goal->cancel_shot()) {
+ // Cancel the shot process
+ state_ = SuperstructureState::IDLE;
+ } else if (unsafe_goal->fire()) {
+ // Start if we were asked to and the turret is at goal
+ state_ = SuperstructureState::SHOOTING;
+ prev_shot_count_ = catapult_.shot_count();
+
+ // Reset opening timeout
+ flipper_opening_start_time_ = timestamp;
+ }
+ }
+ break;
+ }
+ case SuperstructureState::SHOOTING: {
+ // Opening flipper arms could fail, wait until they are open using their
+ // potentiometers (the member below is just named encoder).
+ // Be a little more lenient if the flippers were already open in case of
+ // noise or collisions.
+ const double flipper_open_position =
+ (flippers_open_ ? constants::Values::kReseatFlipperPosition()
+ : constants::Values::kFlipperOpenPosition());
+ flippers_open_ =
+ position->flipper_arm_left()->encoder() >= flipper_open_position &&
+ position->flipper_arm_right()->encoder() >= flipper_open_position;
+
+ if (flippers_open_) {
+ // Hold at kFlipperHoldVoltage
+ flipper_arms_voltage = constants::Values::kFlipperHoldVoltage();
+ } else {
+ // Open at kFlipperOpenVoltage
+ flipper_arms_voltage = constants::Values::kFlipperOpenVoltage();
+ }
+
+ if (!flippers_open_ &&
+ timestamp >
+ loading_timer_ + constants::Values::kFlipperOpeningTimeout()) {
+ // Reseat the ball and try again
+ state_ = SuperstructureState::LOADING;
+ loading_timer_ = timestamp;
+ reseating_in_catapult_ = true;
+ break;
+ }
+
+ const bool turret_near_goal =
+ turret_goal != nullptr &&
+ std::abs(turret_goal->unsafe_goal() - turret_.position()) <
+ kTurretGoalThreshold;
+
+ // If the turret reached the aiming goal, fire!
+ if (flippers_open_ && turret_near_goal) {
+ fire_ = true;
+ }
+
+ // If we started firing and the flippers closed a bit, estop to prevent
+ // damage
+ if (fire_ && !flippers_open_) {
+ catapult_.Estop();
+ }
+
+ const bool near_return_position =
+ (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+ unsafe_goal->catapult()->has_return_position() &&
+ std::abs(unsafe_goal->catapult()->return_position()->unsafe_goal() -
+ catapult_.estimated_position()) < kCatapultGoalThreshold);
+
+ // Once the shot is complete and the catapult is back to its return
+ // position, go back to IDLE
+ if (catapult_.shot_count() > prev_shot_count_ && near_return_position) {
+ prev_shot_count_ = catapult_.shot_count();
+ fire_ = false;
+ state_ = SuperstructureState::IDLE;
+ }
+
+ break;
+ }
+ }
+
+ collision_avoidance_.UpdateGoal(
+ {.intake_front_position = intake_front_.estimated_position(),
+ .intake_back_position = intake_back_.estimated_position(),
+ .turret_position = turret_.estimated_position()},
+ turret_goal);
+
+ turret_.set_min_position(collision_avoidance_.min_turret_goal());
+ turret_.set_max_position(collision_avoidance_.max_turret_goal());
+ intake_front_.set_min_position(collision_avoidance_.min_intake_front_goal());
+ intake_front_.set_max_position(collision_avoidance_.max_intake_front_goal());
+ intake_back_.set_min_position(collision_avoidance_.min_intake_back_goal());
+ intake_back_.set_max_position(collision_avoidance_.max_intake_back_goal());
+
+ // Disable the catapult if we want to restart to prevent damage with flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- catapult_status_offset = catapult_.Iterate(
- unsafe_goal, position,
- output != nullptr ? &(output_struct.catapult_voltage) : nullptr,
- status->fbb());
+ catapult_status_offset =
+ catapult_.Iterate(unsafe_goal, position,
+ output != nullptr && !catapult_.estopped()
+ ? &(output_struct.catapult_voltage)
+ : nullptr,
+ fire_, status->fbb());
- flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
+ const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
- position->climber(),
- output != nullptr ? &(output_struct.climber_voltage) : nullptr,
- status->fbb());
+ position->climber(), &output_struct.climber_voltage, status->fbb());
- flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
intake_status_offset_front = intake_front_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake_front() : nullptr,
- position->intake_front(),
- output != nullptr ? &(output_struct.intake_voltage_front) : nullptr,
+ position->intake_front(), &output_struct.intake_voltage_front,
status->fbb());
- flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
intake_status_offset_back = intake_back_.Iterate(
unsafe_goal != nullptr ? unsafe_goal->intake_back() : nullptr,
- position->intake_back(),
- output != nullptr ? &(output_struct.intake_voltage_back) : nullptr,
+ position->intake_back(), &output_struct.intake_voltage_back,
status->fbb());
- flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- turret_status_offset = turret_.Iterate(
- unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
- position->turret(),
- output != nullptr ? &(output_struct.turret_voltage) : nullptr,
- status->fbb());
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ turret_status_offset =
+ turret_.Iterate(turret_goal, position->turret(),
+ &output_struct.turret_voltage, status->fbb());
if (output != nullptr) {
output_struct.roller_voltage_front = roller_speed_compensated_front;
output_struct.roller_voltage_back = roller_speed_compensated_back;
output_struct.transfer_roller_voltage = transfer_roller_speed;
+ output_struct.flipper_arms_voltage = flipper_arms_voltage;
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -121,9 +380,11 @@
Status::Builder status_builder = status->MakeBuilder<Status>();
const bool zeroed = intake_front_.zeroed() && intake_back_.zeroed() &&
- turret_.zeroed() && climber_.zeroed() && catapult_.zeroed();
+ turret_.zeroed() && climber_.zeroed() &&
+ catapult_.zeroed();
const bool estopped = intake_front_.estopped() || intake_back_.estopped() ||
- turret_.estopped() || climber_.estopped() || catapult_.estopped();
+ turret_.estopped() || climber_.estopped() ||
+ catapult_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
@@ -132,10 +393,17 @@
status_builder.add_intake_back(intake_status_offset_back);
status_builder.add_turret(turret_status_offset);
status_builder.add_climber(climber_status_offset);
+
status_builder.add_catapult(catapult_status_offset);
status_builder.add_solve_time(catapult_.solve_time());
- status_builder.add_mpc_active(catapult_.mpc_active());
status_builder.add_shot_count(catapult_.shot_count());
+ status_builder.add_mpc_active(catapult_.mpc_active());
+
+ status_builder.add_flippers_open(flippers_open_);
+ 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_);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index dfd4265..13a790a 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -7,6 +7,7 @@
#include "y2022/constants.h"
#include "y2022/control_loops/superstructure/catapult/catapult.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
+#include "y2022/control_loops/superstructure/superstructure_can_position_generated.h"
#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
@@ -29,6 +30,11 @@
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+ static constexpr double kTurretGoalThreshold = 0.01;
+ static constexpr double kCatapultGoalThreshold = 0.01;
+ // potentiometer will be more noisy
+ static constexpr double kFlipperGoalThreshold = 0.05;
+
explicit Superstructure(::aos::EventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
@@ -58,15 +64,32 @@
PotAndAbsoluteEncoderSubsystem intake_front_;
PotAndAbsoluteEncoderSubsystem intake_back_;
PotAndAbsoluteEncoderSubsystem turret_;
+ catapult::Catapult catapult_;
CollisionAvoidance collision_avoidance_;
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
+ aos::Fetcher<CANPosition> can_position_fetcher_;
+
+ int prev_shot_count_ = 0;
+
+ bool flippers_open_ = false;
+ bool reseating_in_catapult_ = false;
+ bool fire_ = false;
+
+ aos::monotonic_clock::time_point intake_beambreak_timer_ =
+ aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point transferring_timer_ =
+ aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point loading_timer_ =
+ aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point flipper_opening_start_time_ =
+ aos::monotonic_clock::min_time;
+ SuperstructureState state_ = SuperstructureState::IDLE;
+ IntakeState intake_state_ = IntakeState::NO_BALL;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
-
- catapult::Catapult catapult_;
};
} // namespace superstructure
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index ada39a2..379f6ba 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,8 +3,9 @@
namespace y2022.control_loops.superstructure;
table CatapultGoal {
- // If true, fire! The robot will only fire when ready.
- fire:bool (id: 0);
+ // Old fire flag, only kept for backwards-compatability with logs.
+ // Use the fire flag in the root Goal instead
+ fire:bool (id: 0, deprecated);
// The target shot position and velocity. If these are provided before fire
// is called, the optimizer can pre-compute the trajectory.
@@ -37,6 +38,15 @@
// Catapult goal state.
catapult:CatapultGoal (id: 8);
+
+ // If true, fire! The robot will only fire when ready.
+ fire:bool (id: 9);
+
+ // Aborts the shooting process if the ball has been loaded into the catapult
+ // and the superstructure is in the LOADED state.
+ cancel_shot:bool (id: 10);
}
+
+
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 06dee7b..ebd0bd3 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -244,6 +244,18 @@
flatbuffers::Offset<frc971::RelativePosition> climber_offset =
climber_.encoder()->GetSensorValues(&climber_builder);
+ frc971::RelativePosition::Builder flipper_arm_left_builder =
+ builder.MakeBuilder<frc971::RelativePosition>();
+ flipper_arm_left_builder.add_encoder(flipper_arm_left_);
+ flatbuffers::Offset<frc971::RelativePosition> flipper_arm_left_offset =
+ flipper_arm_left_builder.Finish();
+
+ frc971::RelativePosition::Builder flipper_arm_right_builder =
+ builder.MakeBuilder<frc971::RelativePosition>();
+ flipper_arm_right_builder.add_encoder(flipper_arm_right_);
+ flatbuffers::Offset<frc971::RelativePosition> flipper_arm_right_offset =
+ flipper_arm_left_builder.Finish();
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_intake_front(intake_front_offset);
@@ -251,6 +263,11 @@
position_builder.add_turret(turret_offset);
position_builder.add_catapult(catapult_offset);
position_builder.add_climber(climber_offset);
+ position_builder.add_intake_beambreak_front(intake_beambreak_front_);
+ position_builder.add_intake_beambreak_back(intake_beambreak_back_);
+ position_builder.add_turret_beambreak(turret_beambreak_);
+ position_builder.add_flipper_arm_left(flipper_arm_left_offset);
+ position_builder.add_flipper_arm_right(flipper_arm_right_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
@@ -262,6 +279,19 @@
PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
RelativeEncoderSimulator *climber() { return &climber_; }
+ void set_intake_beambreak_front(bool triggered) {
+ intake_beambreak_front_ = triggered;
+ }
+
+ void set_intake_beambreak_back(bool triggered) {
+ intake_beambreak_back_ = triggered;
+ }
+
+ void set_turret_beambreak(bool triggered) { turret_beambreak_ = triggered; }
+
+ void set_flipper_arm_left(double pos) { flipper_arm_left_ = pos; }
+ void set_flipper_arm_right(double pos) { flipper_arm_right_ = pos; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -273,6 +303,11 @@
bool first_ = true;
+ bool intake_beambreak_front_ = false;
+ bool intake_beambreak_back_ = false;
+ bool turret_beambreak_ = false;
+ double flipper_arm_left_ = 0.0;
+ double flipper_arm_right_ = 0.0;
PotAndAbsoluteEncoderSimulator intake_front_;
PotAndAbsoluteEncoderSimulator intake_back_;
PotAndAbsoluteEncoderSimulator turret_;
@@ -343,11 +378,6 @@
0.001);
}
- if (superstructure_goal_fetcher_->has_turret()) {
- EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
- superstructure_status_fetcher_->turret()->position(), 0.001);
- }
-
if (superstructure_goal_fetcher_->has_catapult() &&
superstructure_goal_fetcher_->catapult()->has_return_position()) {
EXPECT_NEAR(superstructure_goal_fetcher_->catapult()
@@ -361,7 +391,23 @@
EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
superstructure_status_fetcher_->climber()->position(), 0.001);
}
- }
+
+ if (superstructure_status_fetcher_->intake_state() !=
+ IntakeState::NO_BALL) {
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+ }
+
+ EXPECT_NEAR(superstructure_goal_fetcher_->climber()->unsafe_goal(),
+ superstructure_status_fetcher_->climber()->position(), 0.001);
+
+ if (superstructure_goal_fetcher_->has_turret() &&
+ superstructure_status_fetcher_->state() !=
+ SuperstructureState::TRANSFERRING) {
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->unsafe_goal(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ }
+ } // namespace testing
void CheckIfZeroed() {
superstructure_status_fetcher_.Fetch();
@@ -392,7 +438,7 @@
}
void TestRollerFront(double roller_speed_front,
- double roller_speed_compensation) {
+ double roller_speed_compensation, double expected) {
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_roller_speed_front(roller_speed_front);
@@ -400,18 +446,11 @@
builder.CheckOk(builder.Send(goal_builder.Finish()));
RunFor(dt() * 2);
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
- roller_speed_front + std::max((superstructure_.robot_velocity() *
- roller_speed_compensation),
- 0.0));
- if (superstructure_.robot_velocity() <= 0) {
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(),
- roller_speed_front);
- }
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), expected);
}
void TestRollerBack(double roller_speed_back,
- double roller_speed_compensation) {
+ double roller_speed_compensation, double expected) {
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_roller_speed_back(roller_speed_back);
@@ -420,18 +459,12 @@
RunFor(dt() * 2);
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
- roller_speed_back - std::min(superstructure_.robot_velocity() *
- roller_speed_compensation,
- 0.0));
- if (superstructure_.robot_velocity() >= 0) {
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(),
- roller_speed_back);
- }
+
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), expected);
}
void TestTransferRoller(double transfer_roller_speed,
- double roller_speed_compensation) {
+ double roller_speed_compensation, double expected) {
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_transfer_roller_speed(transfer_roller_speed);
@@ -441,7 +474,7 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
- transfer_roller_speed);
+ expected);
}
std::shared_ptr<const constants::Values> values_;
@@ -466,7 +499,7 @@
std::unique_ptr<aos::EventLoop> logger_event_loop_;
std::unique_ptr<aos::logger::Logger> logger_;
-};
+}; // namespace testing
// Tests that the superstructure does nothing when the goal is to remain
// still.
@@ -699,28 +732,265 @@
WaitUntilZeroed();
SendRobotVelocity(3.0);
- TestRollerFront(-12.0, 1.5);
- TestRollerFront(12.0, 1.5);
- TestRollerFront(0.0, 1.5);
+ TestRollerFront(-12.0, 1.5, -7.5);
+ TestRollerFront(12.0, 1.5, 16.5);
+ TestRollerFront(0.0, 1.5, 4.5);
SendRobotVelocity(-3.0);
- TestRollerFront(-12.0, 1.5);
- TestRollerFront(12.0, 1.5);
- TestRollerFront(0.0, 1.5);
+ TestRollerFront(-12.0, 1.5, -12.0);
+ TestRollerFront(12.0, 1.5, 12.0);
+ TestRollerFront(0.0, 1.5, 0.0);
SendRobotVelocity(3.0);
- TestRollerBack(-12.0, 1.5);
- TestRollerBack(12.0, 1.5);
- TestRollerBack(0.0, 1.5);
+ TestRollerBack(-12.0, 1.5, -12.0);
+ TestRollerBack(12.0, 1.5, 12.0);
+ TestRollerBack(0.0, 1.5, 0.0);
SendRobotVelocity(-3.0);
- TestRollerBack(-12.0, 1.5);
- TestRollerBack(12.0, 1.5);
- TestRollerBack(0.0, 1.5);
+ TestRollerBack(-12.0, 1.5, -7.5);
+ TestRollerBack(12.0, 1.5, 16.5);
+ TestRollerBack(0.0, 1.5, 4.5);
- TestTransferRoller(-12.0, 1.5);
- TestTransferRoller(12.0, 1.5);
- TestTransferRoller(0.0, 1.5);
+ TestTransferRoller(-12.0, 1.5, -12.0);
+ TestTransferRoller(12.0, 1.5, 12.0);
+ TestTransferRoller(0.0, 1.5, 0.0);
+}
+
+// Tests the whole shooting statemachine - from loading to shooting
+TEST_F(SuperstructureTest, LoadingToShooting) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ SendRobotVelocity(3.0);
+
+ constexpr double kTurretGoal = 3.0;
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kTurretGoal);
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_roller_speed_front(12.0);
+ goal_builder.add_roller_speed_back(12.0);
+ goal_builder.add_roller_speed_compensation(0.0);
+ goal_builder.add_turret(turret_offset);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+ RunFor(std::chrono::seconds(2));
+
+ // Make sure that the rollers are spinning, but the superstructure hasn't
+ // transitioned away from idle because the beambreaks haven't been triggered.
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::NO_BALL);
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+ 0.001);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ superstructure_plant_.set_intake_beambreak_front(true);
+ superstructure_plant_.set_intake_beambreak_back(false);
+ RunFor(dt());
+
+ // 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
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
+ 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);
+
+ RunFor(chrono::seconds(1));
+
+ // Make sure that we are still transferring and the front transfer rollers
+ // still have a ball. The turret should now be at the loading position and the
+ // flippers should be feeding the ball.
+ 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_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::INTAKE_FRONT_BALL);
+ EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+ constants::Values::kFlipperFeedVoltage());
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ constants::Values::kTransferRollerFrontVoltage());
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretFrontIntakePos(), 0.001);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ superstructure_plant_.set_intake_beambreak_front(false);
+ superstructure_plant_.set_intake_beambreak_back(false);
+ superstructure_plant_.set_turret_beambreak(true);
+ RunFor(dt() * 2);
+
+ // Now that the turret beambreak has been triggered, we should be loading the
+ // ball. The outside rollers shouldn't be limited anymore, and the transfer
+ // rollers should be off. The flippers should still be feeding the ball, and
+ // the intake state should reflect that the ball has been transferred away
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADING);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::NO_BALL);
+ EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+ constants::Values::kFlipperFeedVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ superstructure_plant_.set_turret_beambreak(false);
+ RunFor(constants::Values::kExtraLoadingTime() + dt());
+
+ // Now that the ball has gone past the turret beambreak,
+ // it should be loaded in the catapult and ready for firing.
+ // The flippers should be off.
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::NO_BALL);
+ EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ RunFor(std::chrono::seconds(2));
+
+ // After a few seconds, the turret should be at it's aiming goal. The flippers
+ // should still be off and we should still be loaded and ready to fire.
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::NO_BALL);
+ EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(), 0.0);
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+ 0.001);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ superstructure_plant_.set_intake_beambreak_front(false);
+ superstructure_plant_.set_intake_beambreak_back(true);
+ RunFor(dt() * 2);
+
+ // A ball being intaked from the back should be held by wiggling the transfer
+ // rollers, but we shound't abort the shot from the front intake for it and
+ // 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_back(), 0.0);
+ LOG(INFO) << superstructure_output_fetcher_->transfer_roller_voltage();
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
+ 0.0 &&
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
+ constants::Values::kTransferRollerFrontWiggleVoltage() &&
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
+ -constants::Values::kTransferRollerFrontWiggleVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::INTAKE_BACK_BALL);
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+ 0.001);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kTurretGoal);
+
+ const auto catapult_return_offset =
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(*builder.fbb(),
+ -0.87);
+ auto catapult_builder = builder.MakeBuilder<CatapultGoal>();
+ catapult_builder.add_shot_position(0.3);
+ catapult_builder.add_shot_velocity(15.0);
+ catapult_builder.add_return_position(catapult_return_offset);
+ 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_back(12.0);
+ goal_builder.add_roller_speed_compensation(0.0);
+ goal_builder.add_catapult(catapult_offset);
+ goal_builder.add_fire(true);
+ goal_builder.add_turret(turret_offset);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+ superstructure_plant_.set_flipper_arm_left(
+ constants::Values::kFlipperArmRange().upper);
+ superstructure_plant_.set_flipper_arm_right(
+ constants::Values::kFlipperArmRange().upper);
+ RunFor(dt() * 2);
+
+ // 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
+ 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_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
+ 0.0 &&
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
+ constants::Values::kTransferRollerFrontWiggleVoltage() &&
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
+ -constants::Values::kTransferRollerFrontWiggleVoltage());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::SHOOTING);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
+ IntakeState::INTAKE_BACK_BALL);
+ EXPECT_TRUE(superstructure_status_fetcher_->flippers_open());
+ EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
+ constants::Values::kFlipperHoldVoltage());
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
+ 0.001);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
+
+ superstructure_plant_.set_flipper_arm_left(
+ constants::Values::kFlipperArmRange().upper);
+ superstructure_plant_.set_flipper_arm_right(
+ constants::Values::kFlipperArmRange().upper);
+ superstructure_plant_.set_intake_beambreak_back(false);
+ RunFor(std::chrono::seconds(2));
+
+ // After a bit, we should have completed the shot and be idle.
+ // Since the beambreak was triggered a bit ago, it should still think a ball
+ // 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_->intake_state(),
+ IntakeState::INTAKE_BACK_BALL);
+
+ // Since the intake beambreak hasn't triggered in a while, it should realize
+ // the ball was lost
+ RunFor(std::chrono::seconds(1));
+ 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_->intake_state(),
+ IntakeState::NO_BALL);
}
// Make sure that the front and back intakes are never switched
@@ -776,9 +1046,11 @@
TEST_F(SuperstructureTest, ShootCatapult) {
SetEnabled(true);
superstructure_plant_.intake_front()->InitializePosition(
- constants::Values::kIntakeRange().middle());
+ constants::Values::kIntakeRange().upper);
superstructure_plant_.intake_back()->InitializePosition(
- constants::Values::kIntakeRange().middle());
+ constants::Values::kIntakeRange().upper);
+ superstructure_plant_.turret()->InitializePosition(
+ constants::Values::kTurretFrontIntakePos());
WaitUntilZeroed();
@@ -794,7 +1066,6 @@
CatapultGoal::Builder catapult_goal_builder =
builder.MakeBuilder<CatapultGoal>();
- catapult_goal_builder.add_fire(false);
catapult_goal_builder.add_shot_position(0.3);
catapult_goal_builder.add_shot_velocity(15.0);
catapult_goal_builder.add_return_position(catapult_return_position_offset);
@@ -802,6 +1073,7 @@
catapult_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_fire(false);
goal_builder.add_catapult(catapult_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -817,6 +1089,10 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_goal_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), constants::Values::kTurretFrontIntakePos());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
catapult_return_position_offset =
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), constants::Values::kCatapultRange().lower,
@@ -825,7 +1101,6 @@
CatapultGoal::Builder catapult_goal_builder =
builder.MakeBuilder<CatapultGoal>();
- catapult_goal_builder.add_fire(true);
catapult_goal_builder.add_shot_position(0.5);
catapult_goal_builder.add_shot_velocity(20.0);
catapult_goal_builder.add_return_position(catapult_return_position_offset);
@@ -834,11 +1109,28 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_fire(true);
goal_builder.add_catapult(catapult_goal_offset);
+ goal_builder.add_turret(turret_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- RunFor(chrono::milliseconds(100));
+ // Make the superstructure statemachine progress to SHOOTING
+ superstructure_plant_.set_intake_beambreak_front(true);
+ superstructure_plant_.set_turret_beambreak(true);
+ superstructure_plant_.set_flipper_arm_left(
+ constants::Values::kFlipperArmRange().upper);
+ superstructure_plant_.set_flipper_arm_right(
+ constants::Values::kFlipperArmRange().upper);
+
+ RunFor(dt() * 4);
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADING);
+ superstructure_plant_.set_turret_beambreak(false);
+
+ RunFor(chrono::milliseconds(200));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_->mpc_active());
@@ -846,12 +1138,17 @@
EXPECT_GT(superstructure_status_fetcher_->catapult()->position(),
constants::Values::kCatapultRange().lower + 0.1);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::SHOOTING);
+ superstructure_plant_.set_intake_beambreak_front(false);
+
RunFor(chrono::milliseconds(1950));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
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);
}
} // namespace testing
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index a673361..8fa992e 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -23,6 +23,7 @@
intake_voltage_back:double (id: 5);
// Intake roller voltages
+ // positive is pulling into the robot
roller_voltage_front:double (id: 6);
roller_voltage_back:double (id: 7);
// One transfer motor for both sides
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 4487f1d..c74cbab 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -3,12 +3,47 @@
namespace y2022.control_loops.superstructure;
+// Contains which intake has a ball
+enum IntakeState : ubyte {
+ NO_BALL,
+ INTAKE_FRONT_BALL,
+ INTAKE_BACK_BALL,
+}
+
+// State of the superstructure state machine
+enum SuperstructureState : ubyte {
+ // Before a ball is intaked, when neither intake beambreak is triggered
+ IDLE,
+ // Transferring ball with transfer rollers. Moves turret to loading position.
+ TRANSFERRING,
+ // Loading the ball into the catapult
+ LOADING,
+ // The ball is loaded into the catapult
+ LOADED,
+ // Waiting for the turret to be at shooting goal and then telling the
+ // catapult to fire.
+ SHOOTING,
+}
+
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+ // The state of the superstructure
+
+ state:SuperstructureState (id: 10);
+ // Intaking state
+ intake_state:IntakeState (id: 11);
+ // Whether the flippers are open for shooting
+ flippers_open:bool (id: 12);
+ // Whether the flippers failed to open and we are retrying
+ reseating_in_catapult:bool (id: 13);
+ // Whether the catapult was told to fire,
+ // meaning that the turret and flippers are ready for firing
+ // and we were asked to fire. Different from fire flag in goal.
+ fire:bool (id: 14);
// Subsystem statuses
climber:frc971.control_loops.RelativeEncoderProfiledJointStatus (id: 2);