Add Extend Superstructure and State Machine
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: Id35e2156499384502275af306ff7042c7800d31f
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 2ddaf64..3216b69 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -35,12 +35,12 @@
Shooter::Iterate(
const y2024::control_loops::superstructure::Position *position,
const y2024::control_loops::superstructure::ShooterGoal *shooter_goal,
- double *catapult_output, double *altitude_output, double *turret_output,
- double *retention_roller_output,
+ bool fire, double *catapult_output, double *altitude_output,
+ double *turret_output, double *retention_roller_output,
double *retention_roller_stator_current_limit, double /*battery_voltage*/,
CollisionAvoidance *collision_avoidance, const double intake_pivot_position,
double *max_intake_pivot_position, double *min_intake_pivot_position,
- flatbuffers::FlatBufferBuilder *fbb) {
+ bool standby, flatbuffers::FlatBufferBuilder *fbb) {
drivetrain_status_fetcher_.Fetch();
// If our current is over the minimum current and our velocity is under our
@@ -91,9 +91,19 @@
bool aiming = false;
- // We don't have the note so we should be ready to intake it.
- if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
- (!piece_loaded && state_ == CatapultState::READY)) {
+ if (standby) {
+ // Note is going to AMP or TRAP, move turret to extend
+ // collision avoidance position.
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ turret_goal_builder.get(),
+ robot_constants_->common()->turret_avoid_extend_collision_position());
+
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ altitude_goal_builder.get(),
+ robot_constants_->common()->altitude_loading_position());
+ } else if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+ (!piece_loaded && state_ == CatapultState::READY)) {
+ // We don't have the note so we should be ready to intake it.
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
turret_goal_builder.get(),
robot_constants_->common()->turret_loading_position());
@@ -101,6 +111,7 @@
PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
altitude_goal_builder.get(),
robot_constants_->common()->altitude_loading_position());
+
} else {
// We have a game piece, lets start aiming.
if (drivetrain_status_fetcher_.get() != nullptr) {
@@ -121,11 +132,13 @@
// The builder will contain either the auto-aim goal, or the loading goal. Use
// it if we have no goal, or no subsystem goal, or if we are auto-aiming.
+
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
- shooter_goal->has_turret_position())
+ !standby && shooter_goal->has_turret_position())
? shooter_goal->turret_position()
: &turret_goal_builder->AsFlatbuffer();
+
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
shooter_goal->has_altitude_position())
@@ -203,6 +216,7 @@
switch (state_) {
case CatapultState::READY:
+ [[fallthrough]];
case CatapultState::LOADED: {
if (piece_loaded) {
state_ = CatapultState::LOADED;
@@ -212,8 +226,8 @@
const bool catapult_close = CatapultClose();
- if (subsystems_in_range && shooter_goal != nullptr &&
- shooter_goal->fire() && catapult_close && piece_loaded) {
+ if (subsystems_in_range && shooter_goal != nullptr && fire &&
+ catapult_close && piece_loaded) {
state_ = CatapultState::FIRING;
} else {
catapult_.set_controller_index(0);
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
index 1550fdc..87b0bc0 100644
--- a/y2024/control_loops/superstructure/shooter.h
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -64,14 +64,16 @@
}
flatbuffers::Offset<ShooterStatus> Iterate(
- const Position *position, const ShooterGoal *shooter_goal,
+ const Position *position, const ShooterGoal *shooter_goal, bool fire,
double *catapult_output, double *altitude_output, double *turret_output,
double *retention_roller_output,
double *retention_roller_stator_current_limit, double battery_voltage,
/* Hacky way to use collision avoidance in this class */
CollisionAvoidance *collision_avoidance,
const double intake_pivot_position, double *max_turret_intake_position,
- double *min_intake_pivot_position, flatbuffers::FlatBufferBuilder *fbb);
+ double *min_intake_pivot_position,
+ /* If true, go to extend collision avoidance position */ bool standby,
+ flatbuffers::FlatBufferBuilder *fbb);
private:
CatapultState state_ = CatapultState::RETRACTING;
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index eeaa2fa..ab863eb 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -1,13 +1,23 @@
#include "y2024/control_loops/superstructure/superstructure.h"
+#include <chrono>
+
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "aos/time/time.h"
#include "frc971/shooter_interpolation/interpolation.h"
#include "frc971/zeroing/wrap.h"
DEFINE_bool(ignore_distance, false,
- "If true, ignore distance when shooting and obay joystick_reader");
+ "If true, ignore distance when shooting and obey joystick_reader");
+
+// The threshold used when decided if the extend is close enough to a goal to
+// continue.
+constexpr double kExtendThreshold = 0.01;
+
+constexpr double kTurretLoadingThreshold = 0.01;
+constexpr double kAltitudeLoadingThreshold = 0.01;
namespace y2024::control_loops::superstructure {
@@ -18,11 +28,9 @@
using frc971::control_loops::RelativeEncoderProfiledJointStatus;
Superstructure::Superstructure(::aos::EventLoop *event_loop,
- std::shared_ptr<const constants::Values> values,
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- values_(values),
constants_fetcher_(event_loop),
robot_constants_(CHECK_NOTNULL(&constants_fetcher_.constants())),
drivetrain_status_fetcher_(
@@ -30,16 +38,22 @@
"/drivetrain")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- transfer_goal_(TransferRollerGoal::NONE),
intake_pivot_(robot_constants_->common()->intake_pivot(),
robot_constants_->robot()->intake_constants()),
climber_(
robot_constants_->common()->climber(),
robot_constants_->robot()->climber_constants()->zeroing_constants()),
- shooter_(event_loop, robot_constants_) {
+ shooter_(event_loop, robot_constants_),
+ extend_(
+ robot_constants_->common()->extend(),
+ robot_constants_->robot()->extend_constants()->zeroing_constants()) {
event_loop->SetRuntimeRealtimePriority(30);
}
+bool PositionNear(double position, double goal, double threshold) {
+ return std::abs(position - goal) < threshold;
+}
+
void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position *position,
aos::Sender<Output>::Builder *output,
@@ -54,65 +68,12 @@
intake_pivot_.Reset();
climber_.Reset();
shooter_.Reset();
+ extend_.Reset();
}
OutputT output_struct;
- double intake_pivot_position =
- robot_constants_->common()->intake_pivot_set_points()->retracted();
-
- if (unsafe_goal != nullptr &&
- unsafe_goal->intake_pivot_goal() == IntakePivotGoal::EXTENDED) {
- intake_pivot_position =
- robot_constants_->common()->intake_pivot_set_points()->extended();
- }
-
- IntakeRollerStatus intake_roller_state = IntakeRollerStatus::NONE;
-
- switch (unsafe_goal != nullptr ? unsafe_goal->intake_roller_goal()
- : IntakeRollerGoal::NONE) {
- case IntakeRollerGoal::NONE:
- output_struct.intake_roller_voltage = 0.0;
- break;
- case IntakeRollerGoal::SPIT:
- transfer_goal_ = TransferRollerGoal::TRANSFER_OUT;
- intake_roller_state = IntakeRollerStatus::SPITTING;
- output_struct.intake_roller_voltage =
- robot_constants_->common()->intake_roller_voltages()->spitting();
- break;
- case IntakeRollerGoal::INTAKE:
- transfer_goal_ = TransferRollerGoal::TRANSFER_IN;
- intake_roller_state = IntakeRollerStatus::INTAKING;
- output_struct.intake_roller_voltage =
- robot_constants_->common()->intake_roller_voltages()->intaking();
- break;
- }
-
- TransferRollerStatus transfer_roller_state = TransferRollerStatus::NONE;
-
- switch (unsafe_goal != nullptr ? transfer_goal_ : TransferRollerGoal::NONE) {
- case TransferRollerGoal::NONE:
- output_struct.transfer_roller_voltage = 0.0;
- break;
- case TransferRollerGoal::TRANSFER_IN:
- if (position->transfer_beambreak()) {
- transfer_goal_ = TransferRollerGoal::NONE;
- transfer_roller_state = TransferRollerStatus::NONE;
- output_struct.transfer_roller_voltage = 0.0;
- break;
- }
- transfer_roller_state = TransferRollerStatus::TRANSFERING_IN;
- output_struct.transfer_roller_voltage =
- robot_constants_->common()->transfer_roller_voltages()->transfer_in();
- break;
- case TransferRollerGoal::TRANSFER_OUT:
- transfer_roller_state = TransferRollerStatus::TRANSFERING_OUT;
- output_struct.transfer_roller_voltage = robot_constants_->common()
- ->transfer_roller_voltages()
- ->transfer_out();
- break;
- }
-
+ // Handle Climber Goal separately from main superstructure state machine
double climber_position =
robot_constants_->common()->climber_set_points()->retract();
@@ -135,6 +96,366 @@
}
}
+ // If we started off preloaded, skip to the ready state.
+ if (unsafe_goal != nullptr && unsafe_goal->shooter_goal() &&
+ unsafe_goal->shooter_goal()->preloaded()) {
+ if (state_ != SuperstructureState::READY &&
+ state_ != SuperstructureState::FIRING) {
+ state_ = SuperstructureState::READY;
+ }
+ }
+
+ // Handle the intake pivot goal separately from the main superstructure state
+ IntakeRollerStatus intake_roller_state = IntakeRollerStatus::NONE;
+ double intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->retracted();
+
+ if (unsafe_goal != nullptr) {
+ switch (unsafe_goal->intake_goal()) {
+ case IntakeGoal::INTAKE:
+ intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->extended();
+ break;
+ case IntakeGoal::SPIT:
+ intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->retracted();
+ break;
+ case IntakeGoal::NONE:
+ intake_pivot_position =
+ robot_constants_->common()->intake_pivot_set_points()->retracted();
+ break;
+ }
+ }
+
+ ExtendRollerStatus extend_roller_status = ExtendRollerStatus::IDLE;
+ ExtendStatus extend_goal = ExtendStatus::RETRACTED;
+
+ // True if the extend is moving towards a goal
+ bool extend_moving = false;
+
+ TransferRollerStatus transfer_roller_status = TransferRollerStatus::NONE;
+
+ const ExtendSetPoints *extend_set_points =
+ robot_constants_->common()->extend_set_points();
+
+ // Checks if the extend is close enough to the retracted position to be
+ // considered ready to accept note from the transfer rollers.
+ const bool extend_ready_for_transfer = PositionNear(
+ extend_.position(), extend_set_points->retracted(), kExtendThreshold);
+
+ // If true, the turret should be moved to the position to avoid collision with
+ // the extend.
+ bool move_turret_to_standby = false;
+
+ // Superstructure state machine:
+ // 1. IDLE. The intake is retracted and there is no note in the robot.
+ // Wait for a intake goal to switch state to INTAKING if the extend is ready
+ // 2. INTAKING. Intake the note and transfer it towards the extend.
+ // Give intake, transfer, and extend rollers positive voltage to intake and
+ // transfer. Switch to LOADED when the extend beambreak is triggered.
+ // 3. LOADED. The note is in the extend and the extend is retracted.
+ // Wait for a note goal to switch state to MOVING.
+ // For AMP/TRAP goals, check that the turret is in a position to avoid
+ // collision.
+ // 4. MOVING. The extend is moving towards a goal (AMP, TRAP, or CATAPULT).
+ // For CATAPULT goals, wait for the turret and altitude to be in a position to
+ // accept the note from the extend.
+ // Wait for the extend to reach the goal and switch state to READY if
+ // AMP or TRAP, or to LOADING_CATAPULT if CATAPULT.
+ // 5. LOADING_CATAPULT. The extend is at the position to load the catapult.
+ // Activate the extend roller to transfer the note to the catapult.
+ // Switch state to READY when the catapult beambreak is triggered.
+ // 6. READY. Ready for fire command. The note is either loaded in the catapult
+ // or in the extend and at the AMP or TRAP position. Wait for a fire command.
+ // 7. FIRING. The note is being fired, either from the extend or the catapult.
+ // Switch state back to IDLE when the note is fired.
+
+ switch (state_) {
+ case SuperstructureState::IDLE:
+ if (unsafe_goal != nullptr &&
+ unsafe_goal->intake_goal() == IntakeGoal::INTAKE &&
+ extend_ready_for_transfer) {
+ state_ = SuperstructureState::INTAKING;
+ }
+ extend_goal = ExtendStatus::RETRACTED;
+ catapult_requested_ = false;
+ break;
+ case SuperstructureState::INTAKING:
+
+ // Switch to LOADED state when the extend beambreak is triggered
+ // meaning the note is loaded in the extend
+ if (position->extend_beambreak()) {
+ state_ = SuperstructureState::LOADED;
+ }
+ intake_roller_state = IntakeRollerStatus::INTAKING;
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_IN;
+ extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_EXTEND;
+ extend_goal = ExtendStatus::RETRACTED;
+
+ if (!catapult_requested_ && unsafe_goal != nullptr &&
+ unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+ catapult_requested_ = true;
+ }
+
+ break;
+ case SuperstructureState::LOADED:
+ if (catapult_requested_ == true) {
+ state_ = SuperstructureState::MOVING;
+ break;
+ }
+
+ if (unsafe_goal != nullptr &&
+ unsafe_goal->note_goal() != NoteGoal::NONE) {
+ // If the goal is AMP or TRAP, check if the turret is in a position to
+ // avoid collision when the extend moves.
+ if (unsafe_goal->note_goal() == NoteGoal::AMP ||
+ unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ bool turret_ready_for_extend_move =
+ PositionNear(shooter_.turret().estimated_position(),
+ robot_constants_->common()
+ ->turret_avoid_extend_collision_position(),
+ kTurretLoadingThreshold);
+
+ if (turret_ready_for_extend_move) {
+ state_ = SuperstructureState::MOVING;
+ } else {
+ move_turret_to_standby = true;
+ }
+ } else if (unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+ // If catapult is requested, switch to MOVING state
+ state_ = SuperstructureState::MOVING;
+ }
+ }
+ extend_goal = ExtendStatus::RETRACTED;
+ if (!catapult_requested_ && unsafe_goal != nullptr &&
+ unsafe_goal->note_goal() == NoteGoal::CATAPULT) {
+ catapult_requested_ = true;
+ }
+ break;
+ case SuperstructureState::MOVING:
+
+ if (catapult_requested_) {
+ extend_goal = ExtendStatus::CATAPULT;
+
+ // Check if the extend is at the position to load the catapult
+ bool extend_ready_for_catapult_transfer =
+ PositionNear(extend_.position(), extend_set_points->catapult(),
+ kExtendThreshold);
+
+ // Check if the turret is at the position to accept the note from extend
+ bool turret_ready_for_load =
+ PositionNear(shooter_.turret().estimated_position(),
+ robot_constants_->common()->turret_loading_position(),
+ kTurretLoadingThreshold);
+
+ // Check if the altitude is at the position to accept the note from
+ // extend
+ bool altitude_ready_for_load = PositionNear(
+ shooter_.altitude().estimated_position(),
+ robot_constants_->common()->altitude_loading_position(),
+ kAltitudeLoadingThreshold);
+
+ if (extend_ready_for_catapult_transfer && turret_ready_for_load &&
+ altitude_ready_for_load) {
+ state_ = SuperstructureState::LOADING_CATAPULT;
+ }
+ } else {
+ if (unsafe_goal != nullptr) {
+ switch (unsafe_goal->note_goal()) {
+ case NoteGoal::AMP:
+ extend_goal = ExtendStatus::AMP;
+ move_turret_to_standby = true;
+ // Check if the extend is at the AMP position and if it is
+ // switch to READY state
+ if (PositionNear(extend_.position(), extend_set_points->amp(),
+ kExtendThreshold)) {
+ state_ = SuperstructureState::READY;
+ }
+ break;
+ case NoteGoal::TRAP:
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ // Check if the extend is at the TRAP position and if it is
+ // switch to READY state
+ if (PositionNear(extend_.position(), extend_set_points->trap(),
+ kExtendThreshold)) {
+ state_ = SuperstructureState::READY;
+ }
+ break;
+ case NoteGoal::NONE:
+ extend_goal = ExtendStatus::RETRACTED;
+ move_turret_to_standby = true;
+ if (extend_ready_for_transfer) {
+ state_ = SuperstructureState::LOADED;
+ }
+ break;
+ case NoteGoal::CATAPULT:
+ catapult_requested_ = true;
+ extend_goal = ExtendStatus::CATAPULT;
+ break;
+ }
+ }
+ }
+
+ extend_moving = true;
+ break;
+ case SuperstructureState::LOADING_CATAPULT:
+ extend_moving = false;
+ extend_goal = ExtendStatus::CATAPULT;
+ extend_roller_status = ExtendRollerStatus::TRANSFERING_TO_CATAPULT;
+
+ // Switch to READY state when the catapult beambreak is triggered
+ if (position->catapult_beambreak()) {
+ state_ = SuperstructureState::READY;
+ }
+ break;
+ case SuperstructureState::READY:
+ extend_moving = false;
+
+ // Switch to FIRING state when the fire button is pressed
+ if (unsafe_goal != nullptr && unsafe_goal->fire()) {
+ state_ = SuperstructureState::FIRING;
+ }
+
+ if (catapult_requested_) {
+ extend_goal = ExtendStatus::CATAPULT;
+ } else {
+ if (unsafe_goal != nullptr) {
+ if (unsafe_goal->note_goal() == NoteGoal::AMP) {
+ extend_goal = ExtendStatus::AMP;
+ move_turret_to_standby = true;
+ } else if (unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ extend_goal = ExtendStatus::TRAP;
+ move_turret_to_standby = true;
+ } else {
+ extend_goal = ExtendStatus::RETRACTED;
+ extend_moving = true;
+ state_ = SuperstructureState::MOVING;
+ }
+ }
+ }
+
+ break;
+ case SuperstructureState::FIRING:
+ if (catapult_requested_) {
+ extend_goal = ExtendStatus::CATAPULT;
+
+ // Reset the state to IDLE when the game piece is fired from the
+ // catapult. We consider the game piece to be fired from the catapult
+ // when the catapultbeambreak is no longer triggered.
+ if (!position->catapult_beambreak()) {
+ state_ = SuperstructureState::IDLE;
+ }
+ } else {
+ if (unsafe_goal != nullptr &&
+ unsafe_goal->note_goal() == NoteGoal::AMP) {
+ extend_roller_status = ExtendRollerStatus::SCORING_IN_AMP;
+ extend_goal = ExtendStatus::AMP;
+ } else if (unsafe_goal != nullptr &&
+ unsafe_goal->note_goal() == NoteGoal::TRAP) {
+ extend_roller_status = ExtendRollerStatus::SCORING_IN_TRAP;
+ extend_goal = ExtendStatus::TRAP;
+ }
+
+ // Reset the state to IDLE when the game piece is fired from the extend.
+ // We consider the game piece to be fired from the extend when
+ // the extend beambreak is no longer triggered and the fire button is
+ // released.
+ if (!position->extend_beambreak() && unsafe_goal != nullptr &&
+ !unsafe_goal->fire()) {
+ state_ = SuperstructureState::IDLE;
+ }
+ }
+ break;
+ }
+
+ if (unsafe_goal != nullptr &&
+ unsafe_goal->intake_goal() == IntakeGoal::SPIT) {
+ intake_roller_state = IntakeRollerStatus::SPITTING;
+ transfer_roller_status = TransferRollerStatus::TRANSFERING_OUT;
+ }
+
+ // Update Intake Roller voltage based on status from state machine.
+ switch (intake_roller_state) {
+ case IntakeRollerStatus::NONE:
+ output_struct.intake_roller_voltage = 0.0;
+ break;
+ case IntakeRollerStatus::SPITTING:
+ output_struct.intake_roller_voltage =
+ robot_constants_->common()->intake_roller_voltages()->spitting();
+ break;
+ case IntakeRollerStatus::INTAKING:
+ output_struct.intake_roller_voltage =
+ robot_constants_->common()->intake_roller_voltages()->intaking();
+ break;
+ }
+
+ // Update Transfer Roller voltage based on status from state machine.
+ switch (transfer_roller_status) {
+ case TransferRollerStatus::NONE:
+ output_struct.transfer_roller_voltage = 0.0;
+ break;
+ case TransferRollerStatus::TRANSFERING_IN:
+ output_struct.transfer_roller_voltage =
+ robot_constants_->common()->transfer_roller_voltages()->transfer_in();
+ break;
+ case TransferRollerStatus::TRANSFERING_OUT:
+ output_struct.transfer_roller_voltage = robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_out();
+ break;
+ }
+
+ // Update Extend Roller voltage based on status from state machine.
+ const ExtendRollerVoltages *extend_roller_voltages =
+ robot_constants_->common()->extend_roller_voltages();
+ switch (extend_roller_status) {
+ case ExtendRollerStatus::IDLE:
+ // No voltage applied when idle
+ output_struct.extend_roller_voltage = 0.0;
+ break;
+ case ExtendRollerStatus::TRANSFERING_TO_EXTEND:
+ output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+ break;
+ case ExtendRollerStatus::SCORING_IN_AMP:
+ [[fallthrough]];
+ case ExtendRollerStatus::SCORING_IN_TRAP:
+ // Apply scoring voltage during scoring in amp or trap
+ output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+ break;
+ case ExtendRollerStatus::TRANSFERING_TO_CATAPULT:
+ // Apply scoring voltage during transferring to catapult
+ output_struct.extend_roller_voltage = extend_roller_voltages->scoring();
+ break;
+ }
+
+ double extend_position = 0.0;
+
+ // Set the extend position based on the state machine output
+ switch (extend_goal) {
+ case ExtendStatus::RETRACTED:
+ extend_position = extend_set_points->retracted();
+ break;
+ case ExtendStatus::AMP:
+ extend_position = extend_set_points->amp();
+ break;
+ case ExtendStatus::TRAP:
+ extend_position = extend_set_points->trap();
+ break;
+ case ExtendStatus::CATAPULT:
+ extend_position = extend_set_points->catapult();
+ break;
+ case ExtendStatus::MOVING:
+ // Should never happen
+ break;
+ }
+
+ // Set the extend status based on the state machine output
+ // If the extend is moving, the status is MOVING, otherwise it is the same
+ // as extend_status
+ ExtendStatus extend_status =
+ (extend_moving ? ExtendStatus::MOVING : extend_goal);
+
if (joystick_state_fetcher_.Fetch() &&
joystick_state_fetcher_->has_alliance()) {
alliance_ = joystick_state_fetcher_->alliance();
@@ -183,12 +504,13 @@
const bool disabled = intake_pivot_.Correct(
intake_pivot_goal, position->intake_pivot(), intake_output == nullptr);
- // TODO(max): Change how we handle the collision with the turret and intake to
- // be clearer
+ // TODO(max): Change how we handle the collision with the turret and
+ // intake to be clearer
const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
shooter_.Iterate(
position,
unsafe_goal != nullptr ? unsafe_goal->shooter_goal() : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->fire() : false,
output != nullptr ? &output_struct.catapult_voltage : nullptr,
output != nullptr ? &output_struct.altitude_voltage : nullptr,
output != nullptr ? &output_struct.turret_voltage : nullptr,
@@ -198,7 +520,7 @@
: nullptr,
robot_state().voltage_battery(), &collision_avoidance_,
intake_pivot_.estimated_position(), &max_intake_pivot_position,
- &min_intake_pivot_position, status->fbb());
+ &min_intake_pivot_position, move_turret_to_standby, status->fbb());
intake_pivot_.set_min_position(min_intake_pivot_position);
intake_pivot_.set_max_position(max_intake_pivot_position);
@@ -216,25 +538,46 @@
const flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus>
intake_pivot_status_offset = intake_pivot_.MakeStatus(status->fbb());
+ aos::FlatbufferFixedAllocatorArray<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 512>
+ note_goal_buffer;
+
+ note_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *note_goal_buffer.fbb(), extend_position));
+
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *note_goal = ¬e_goal_buffer.message();
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ extend_status_offset = extend_.Iterate(
+ note_goal, position->extend(),
+ output != nullptr ? &output_struct.extend_voltage : nullptr,
+ status->fbb());
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- const bool zeroed =
- intake_pivot_.zeroed() && climber_.zeroed() && shooter_.zeroed();
- const bool estopped =
- intake_pivot_.estopped() || climber_.estopped() || shooter_.estopped();
+ const bool zeroed = intake_pivot_.zeroed() && climber_.zeroed() &&
+ shooter_.zeroed() && extend_.zeroed();
+ const bool estopped = intake_pivot_.estopped() || climber_.estopped() ||
+ shooter_.estopped() || extend_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
status_builder.add_intake_roller(intake_roller_state);
status_builder.add_intake_pivot(intake_pivot_status_offset);
- status_builder.add_transfer_roller(transfer_roller_state);
+ status_builder.add_transfer_roller(transfer_roller_status);
status_builder.add_climber(climber_status_offset);
status_builder.add_shooter(shooter_status_offset);
status_builder.add_collided(collided);
+ status_builder.add_extend_roller(extend_roller_status);
+ status_builder.add_extend_status(extend_status);
+ status_builder.add_extend(extend_status_offset);
+ status_builder.add_state(state_);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index 650092b..963d8c4 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
@@ -33,7 +34,6 @@
::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
explicit Superstructure(::aos::EventLoop *event_loop,
- std::shared_ptr<const constants::Values> values,
const ::std::string &name = "/superstructure");
inline const AbsoluteEncoderSubsystem &intake_pivot() const {
@@ -45,6 +45,9 @@
}
inline const Shooter &shooter() const { return shooter_; }
+ inline const PotAndAbsoluteEncoderSubsystem &extend() const {
+ return extend_;
+ }
double robot_velocity() const;
@@ -54,7 +57,6 @@
aos::Sender<Status>::Builder *status) override;
private:
- std::shared_ptr<const constants::Values> values_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const Constants *robot_constants_;
aos::Fetcher<frc971::control_loops::drivetrain::Status>
@@ -65,12 +67,19 @@
aos::Alliance alliance_ = aos::Alliance::kInvalid;
- TransferRollerGoal transfer_goal_;
+ bool catapult_requested_ = false;
+
+ SuperstructureState state_ = SuperstructureState::IDLE;
+
+ aos::monotonic_clock::time_point transfer_start_time_ =
+ aos::monotonic_clock::time_point::min();
+
AbsoluteEncoderSubsystem intake_pivot_;
PotAndAbsoluteEncoderSubsystem climber_;
Shooter shooter_;
+ PotAndAbsoluteEncoderSubsystem extend_;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index a57422b..b346e97 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -3,26 +3,13 @@
namespace y2024.control_loops.superstructure;
-// Represents goal for intake rollers
-enum IntakeRollerGoal : ubyte {
+// Represents goal for the intake pivot and rollers
+// INTAKE will extend the pivot and turn on the rollers to intake the note.
+// SPIT will extend the pivot and turn on the rollers (in reverse) to spit out the note.
+enum IntakeGoal : ubyte {
NONE = 0,
- SPIT = 1,
- INTAKE = 2,
-}
-
-// Represents goal for pivot on intake
-enum IntakePivotGoal : ubyte {
- RETRACTED = 0,
- EXTENDED = 1,
-}
-
-// Represents goal of transfer rollers
-// TRANSFER_IN is for transfering game piece in from the intake to the shooter
-// TRANSFER_OUT is for transfering game piece out to the intake for spitting
-enum TransferRollerGoal : ubyte {
- NONE = 0,
- TRANSFER_IN = 1,
- TRANSFER_OUT = 2,
+ INTAKE = 1,
+ SPIT = 2,
}
// Represents goal for climber
@@ -37,45 +24,39 @@
table ShooterGoal {
catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
- fire: bool (id: 1);
- // If true we ignore the other provided positions
- auto_aim: bool (id: 2);
+ auto_aim: bool (id: 1);
// Position for the turret when we aren't auto aiming
- turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
+ turret_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// Position for the altitude when we aren't auto aiming
- altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 4);
+ altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
// If true, we started with the ball loaded and should proceed to that state.
- preloaded:bool = false (id: 5);
+ preloaded:bool = false (id: 4);
}
-// Represents goal for extend
-// RETRACT is for retracting the extender to stowed position
-// In the retracted position, the game piece may be transfered to the catapult
-// AMP is for extending the extender to the AMP scoring position
-// TRAP is for extending the extender to the TRAP scoring position
-enum ExtendGoal : ubyte {
- RETRACT = 0,
+// Represents goal for the note movement through the robot
+// to various scoring positions
+// NONE represents no goal for the note
+// AMP represents the goal to move the note and the extend to the AMP scoring position
+// TRAP represents the goal to move the note and the extend to the TRAP scoring position
+// CATAPULT represents the goal to load the note in the catapult.
+// It will complete the catapult goal before accepting new goals.
+enum NoteGoal : ubyte {
+ NONE = 0,
AMP = 1,
TRAP = 2,
+ CATAPULT = 3,
}
-enum ExtendRollerGoal : ubyte {
- NONE = 0,
- SCORING = 1,
- REVERSING = 2,
-}
table Goal {
- intake_roller_goal:IntakeRollerGoal (id: 0);
- intake_pivot_goal:IntakePivotGoal (id: 1);
- catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 2);
- transfer_roller_goal:TransferRollerGoal (id: 3);
- climber_goal:ClimberGoal (id: 4);
- shooter_goal:ShooterGoal (id: 5);
- extend_goal:ExtendGoal (id: 6);
- extend_roller_goal:ExtendRollerGoal (id: 7);
+ intake_goal:IntakeGoal = NONE (id: 0);
+ catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 1);
+ climber_goal:ClimberGoal (id: 2);
+ shooter_goal:ShooterGoal (id: 3);
+ note_goal:NoteGoal (id: 4);
+ fire: bool (id: 5);
}
root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 701690b..d1f6641 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -16,6 +16,7 @@
#include "y2024/control_loops/superstructure/altitude/altitude_plant.h"
#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2024/control_loops/superstructure/climber/climber_plant.h"
+#include "y2024/control_loops/superstructure/extend/extend_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
#include "y2024/control_loops/superstructure/superstructure.h"
#include "y2024/control_loops/superstructure/turret/turret_plant.h"
@@ -63,7 +64,7 @@
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
event_loop_->MakeFetcher<Output>("/superstructure")),
- transfer_beambreak_(false),
+ extend_beambreak_(false),
catapult_beambreak_(false),
intake_pivot_(
new CappedTestPlant(intake_pivot::MakeIntakePivotPlant()),
@@ -158,6 +159,26 @@
->turret_constants()
->zeroing_constants()
->measured_absolute_position(),
+ dt_),
+ extend_(
+ new CappedTestPlant(extend::MakeExtendPlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->extend_constants()
+ ->zeroing_constants()
+ ->one_revolution_distance()),
+ {.subsystem_params = {simulated_robot_constants->common()->extend(),
+ simulated_robot_constants->robot()
+ ->extend_constants()
+ ->zeroing_constants()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->extend_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->extend()->range()),
+ simulated_robot_constants->robot()
+ ->extend_constants()
+ ->zeroing_constants()
+ ->measured_absolute_position(),
dt_) {
intake_pivot_.InitializePosition(
frc971::constants::Range::FromFlatbuffer(
@@ -179,6 +200,11 @@
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->turret()->range())
.middle());
+ extend_.InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->extend()->range())
+ .middle());
+
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -203,6 +229,9 @@
turret_.Simulate(
superstructure_output_fetcher_->turret_voltage(),
superstructure_status_fetcher_->shooter()->turret());
+
+ extend_.Simulate(superstructure_output_fetcher_->extend_voltage(),
+ superstructure_status_fetcher_->extend());
}
first_ = false;
SendPositionMessage();
@@ -241,23 +270,27 @@
flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
turret_.encoder()->GetSensorValues(&turret_builder);
+ frc971::PotAndAbsolutePosition::Builder extend_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> extend_offset =
+ extend_.encoder()->GetSensorValues(&extend_builder);
+
Position::Builder position_builder = builder.MakeBuilder<Position>();
- position_builder.add_transfer_beambreak(transfer_beambreak_);
+ position_builder.add_extend_beambreak(extend_beambreak_);
position_builder.add_catapult_beambreak(catapult_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
position_builder.add_catapult(catapult_offset);
position_builder.add_altitude(altitude_offset);
position_builder.add_turret(turret_offset);
-
position_builder.add_climber(climber_offset);
+ position_builder.add_extend(extend_offset);
+
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
- void set_transfer_beambreak(bool triggered) {
- transfer_beambreak_ = triggered;
- }
+ void set_extend_beambreak(bool triggered) { extend_beambreak_ = triggered; }
void set_catapult_beambreak(bool triggered) {
catapult_beambreak_ = triggered;
@@ -269,6 +302,8 @@
PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
+ PotAndAbsoluteEncoderSimulator *extend() { return &extend_; }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -279,7 +314,7 @@
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
- bool transfer_beambreak_;
+ bool extend_beambreak_;
bool catapult_beambreak_;
AbsoluteEncoderSimulator intake_pivot_;
@@ -287,6 +322,7 @@
PotAndAbsoluteEncoderSimulator catapult_;
PotAndAbsoluteEncoderSimulator altitude_;
PotAndAbsoluteEncoderSimulator turret_;
+ PotAndAbsoluteEncoderSimulator extend_;
bool first_ = true;
};
@@ -297,13 +333,12 @@
: ::frc971::testing::ControlLoopTest(
aos::configuration::ReadConfig("y2024/aos_config.json"),
std::chrono::microseconds(5050)),
- values_(std::make_shared<constants::Values>(constants::MakeValues())),
simulated_constants_dummy_(SendSimulationConstants(
event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
- superstructure_(superstructure_event_loop.get(), (values_)),
+ superstructure_(superstructure_event_loop.get()),
test_event_loop_(MakeEventLoop("test", roborio_)),
constants_fetcher_(test_event_loop_.get()),
simulated_robot_constants_(
@@ -354,11 +389,10 @@
EXPECT_FALSE(superstructure_status_fetcher_->collided());
- double set_point = superstructure_status_fetcher_->intake_pivot()
- ->unprofiled_goal_position();
+ double set_point =
+ superstructure_status_fetcher_->intake_pivot()->goal_position();
- if (superstructure_goal_fetcher_->intake_pivot_goal() ==
- IntakePivotGoal::EXTENDED) {
+ if (superstructure_goal_fetcher_->intake_goal() == IntakeGoal::INTAKE) {
set_point = simulated_robot_constants_->common()
->intake_pivot_set_points()
->extended();
@@ -366,9 +400,11 @@
EXPECT_NEAR(set_point,
superstructure_status_fetcher_->intake_pivot()->position(),
- 0.001);
+ 0.01);
- if (superstructure_goal_fetcher_->has_shooter_goal()) {
+ if (superstructure_goal_fetcher_->has_shooter_goal() &&
+ superstructure_goal_fetcher_->note_goal() != NoteGoal::AMP &&
+ superstructure_goal_fetcher_->note_goal() != NoteGoal::TRAP) {
if (superstructure_goal_fetcher_->shooter_goal()->has_turret_position() &&
!superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
EXPECT_NEAR(
@@ -378,6 +414,13 @@
superstructure_status_fetcher_->shooter()->turret()->position(),
0.001);
}
+ } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP ||
+ superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+ EXPECT_NEAR(
+ simulated_robot_constants_->common()
+ ->turret_avoid_extend_collision_position(),
+ superstructure_status_fetcher_->shooter()->turret()->position(),
+ 0.001);
}
if (superstructure_goal_fetcher_->has_shooter_goal()) {
@@ -421,6 +464,27 @@
EXPECT_NEAR(set_point,
superstructure_status_fetcher_->climber()->position(), 0.001);
}
+
+ if (superstructure_goal_fetcher_->has_note_goal()) {
+ double set_point = simulated_robot_constants_->common()
+ ->extend_set_points()
+ ->retracted();
+ if (superstructure_goal_fetcher_->note_goal() == NoteGoal::TRAP) {
+ set_point =
+ simulated_robot_constants_->common()->extend_set_points()->trap();
+ } else if (superstructure_goal_fetcher_->note_goal() == NoteGoal::AMP) {
+ set_point =
+ simulated_robot_constants_->common()->extend_set_points()->amp();
+ } else if (superstructure_goal_fetcher_->note_goal() ==
+ NoteGoal::CATAPULT) {
+ set_point = simulated_robot_constants_->common()
+ ->extend_set_points()
+ ->catapult();
+ }
+
+ EXPECT_NEAR(set_point,
+ superstructure_status_fetcher_->extend()->position(), 0.001);
+ }
}
void CheckIfZeroed() {
@@ -481,7 +545,6 @@
builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
}
- std::shared_ptr<const constants::Values> values_;
const bool simulated_constants_dummy_;
const aos::Node *const roborio_;
@@ -559,7 +622,8 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -591,6 +655,10 @@
simulated_robot_constants_->common()->climber()->range())
.lower);
+ superstructure_plant_.extend()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->extend()->range())
+ .lower);
WaitUntilZeroed();
{
@@ -621,15 +689,19 @@
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_extend_beambreak(true);
+
// Give it a lot of time to get there.
RunFor(chrono::seconds(15));
+
VerifyNearGoal();
}
@@ -669,12 +741,15 @@
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_note_goal(NoteGoal::AMP);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_extend_beambreak(true);
+
RunFor(chrono::seconds(20));
VerifyNearGoal();
@@ -710,13 +785,16 @@
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
+ superstructure_plant_.set_extend_beambreak(false);
+
RunFor(chrono::seconds(10));
VerifyNearGoal();
}
@@ -738,6 +816,8 @@
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.shooter().altitude().state());
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.extend().state());
}
// Tests that running disabled works
@@ -822,14 +902,12 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
- goal_builder.add_intake_roller_goal(IntakeRollerGoal::NONE);
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -841,14 +919,12 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
- goal_builder.add_intake_roller_goal(IntakeRollerGoal::SPIT);
+ goal_builder.add_intake_goal(IntakeGoal::SPIT);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -863,14 +939,12 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
- goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -880,13 +954,12 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -896,7 +969,17 @@
->transfer_roller_voltages()
->transfer_in());
- superstructure_plant_.set_transfer_beambreak(true);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(true);
RunFor(chrono::seconds(2));
@@ -926,21 +1009,20 @@
builder.MakeBuilder<ShooterGoal>();
shooter_goal_builder.add_auto_aim(true);
- shooter_goal_builder.add_fire(false);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_fire(false);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
superstructure_plant_.set_catapult_beambreak(false);
- superstructure_plant_.set_transfer_beambreak(false);
RunFor(chrono::seconds(5));
@@ -964,22 +1046,19 @@
builder.MakeBuilder<ShooterGoal>();
shooter_goal_builder.add_auto_aim(true);
- shooter_goal_builder.add_fire(false);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
- goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_fire(false);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
RunFor(chrono::seconds(5));
VerifyNearGoal();
@@ -994,32 +1073,114 @@
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
- // Signal through the transfer roller that we got it.
- superstructure_plant_.set_transfer_beambreak(true);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- RunFor(chrono::seconds(5));
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+ goal_builder.add_fire(false);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(false);
+
+ RunFor(10 * dt());
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::RETRACTED);
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::TRANSFERING_TO_EXTEND);
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 12.0);
+ EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+
+ superstructure_plant_.set_extend_beambreak(true);
+
+ RunFor(chrono::milliseconds(750));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::RETRACTED);
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::IDLE);
+
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 0.0);
// Verify we are in the loading position.
EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
simulated_robot_constants_->common()->turret_loading_position(),
0.01);
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
- 0.0, 0.01);
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_note_goal(NoteGoal::CATAPULT);
+ goal_builder.add_fire(false);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(true);
+
+ RunFor(chrono::milliseconds(500));
+
+ VerifyNearGoal();
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
superstructure_plant_.set_catapult_beambreak(true);
- superstructure_plant_.set_transfer_beambreak(true);
+ // Set retention roller to show that we are loaded.
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::CATAPULT);
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::TRANSFERING_TO_CATAPULT);
- RunFor(chrono::seconds(5));
+ superstructure_plant_.set_extend_beambreak(false);
- VerifyNearGoal();
+ RunFor(chrono::seconds(10));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::LOADED);
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::CATAPULT);
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::IDLE);
// Should now be loaded.
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
@@ -1053,7 +1214,6 @@
builder.MakeBuilder<ShooterGoal>();
shooter_goal_builder.add_auto_aim(false);
- shooter_goal_builder.add_fire(true);
shooter_goal_builder.add_catapult_goal(catapult_offset);
shooter_goal_builder.add_altitude_position(altitude_offset);
shooter_goal_builder.add_turret_position(turret_offset);
@@ -1063,14 +1223,13 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_fire(true);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
- superstructure_plant_.set_transfer_beambreak(false);
-
// Wait until the bot finishes auto-aiming.
WaitUntilNear(kTurretGoal, kAltitudeGoal);
@@ -1105,7 +1264,6 @@
builder.MakeBuilder<ShooterGoal>();
shooter_goal_builder.add_auto_aim(false);
- shooter_goal_builder.add_fire(false);
flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
shooter_goal_builder.Finish();
@@ -1113,17 +1271,19 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_shooter_goal(shooter_goal_offset);
+ goal_builder.add_fire(false);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
superstructure_plant_.set_catapult_beambreak(false);
- superstructure_plant_.set_transfer_beambreak(false);
RunFor(chrono::seconds(5));
VerifyNearGoal();
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
CatapultState::READY);
@@ -1290,4 +1450,139 @@
superstructure_status_fetcher_->shooter()->aimer()->target_distance());
}
-} // namespace y2024::control_loops::superstructure::testing
+// Test entire sequence of loading, transfering, and scoring at amp position.
+TEST_F(SuperstructureTest, ScoreInAmp) {
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
+ simulated_robot_constants_->common()
+ ->transfer_roller_voltages()
+ ->transfer_in());
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::TRANSFERING_TO_EXTEND);
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::INTAKING);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::INTAKE);
+ goal_builder.add_note_goal(NoteGoal::NONE);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(true);
+
+ RunFor(chrono::milliseconds(10));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::IDLE);
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::LOADED);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_note_goal(NoteGoal::AMP);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(100 * dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(),
+ ExtendStatus::MOVING);
+
+ RunFor(chrono::seconds(5));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(superstructure_status_fetcher_->extend()->position(),
+ simulated_robot_constants_->common()->extend_set_points()->amp(),
+ 0.01);
+ EXPECT_EQ(superstructure_status_fetcher_->extend_status(), ExtendStatus::AMP);
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::READY);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_note_goal(NoteGoal::AMP);
+ goal_builder.add_fire(true);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(true);
+
+ RunFor(chrono::milliseconds(10));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::SCORING_IN_AMP);
+ EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 12.0);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_goal(IntakeGoal::NONE);
+ goal_builder.add_note_goal(NoteGoal::AMP);
+ goal_builder.add_fire(false);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_extend_beambreak(false);
+
+ RunFor(chrono::milliseconds(100));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
+
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+
+ EXPECT_EQ(superstructure_status_fetcher_->extend_roller(),
+ ExtendRollerStatus::IDLE);
+ EXPECT_EQ(superstructure_output_fetcher_->extend_roller_voltage(), 0.0);
+}
+} // namespace y2024::control_loops::superstructure::testing
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure_main.cc b/y2024/control_loops/superstructure/superstructure_main.cc
index 58932c7..a5ab8ed 100644
--- a/y2024/control_loops/superstructure/superstructure_main.cc
+++ b/y2024/control_loops/superstructure/superstructure_main.cc
@@ -17,10 +17,7 @@
frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
- std::shared_ptr<const y2024::constants::Values> values =
- std::make_shared<const y2024::constants::Values>(
- y2024::constants::MakeValues());
- Superstructure superstructure(&event_loop, values);
+ Superstructure superstructure(&event_loop);
event_loop.Run();
diff --git a/y2024/control_loops/superstructure/superstructure_replay.cc b/y2024/control_loops/superstructure/superstructure_replay.cc
index bd7f9fd..ab88a46 100644
--- a/y2024/control_loops/superstructure/superstructure_replay.cc
+++ b/y2024/control_loops/superstructure/superstructure_replay.cc
@@ -48,8 +48,7 @@
roborio->OnStartup([roborio]() {
roborio->AlwaysStart<y2024::control_loops::superstructure::Superstructure>(
- "superstructure", std::make_shared<y2024::constants::Values>(
- y2024::constants::MakeValues()));
+ "superstructure");
});
std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 872fb21..7541b7e 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -3,6 +3,27 @@
namespace y2024.control_loops.superstructure;
+enum SuperstructureState : ubyte {
+ // Before a note has been intaked, the extend should be retracted.
+ IDLE = 0,
+ // Intaking a note and transferring it to the extned through the
+ // intake, transfer, and extend rollers.
+ INTAKING = 1,
+ // The note is in the extend and the extend is not moving.
+ LOADED = 2,
+ // The note is in the extend and the extend is moving towards a goal,
+ // either the catapult, amp, or trap.
+ MOVING = 3,
+ // For Catapult Path, the note is being transferred between the extend and the catapult.
+ LOADING_CATAPULT = 4,
+ // The note is either:
+ // 1. Loaded in the catapult and ready to fire
+ // 2. In the extend and the extend is at the amp or trap scoring position.
+ READY = 5,
+ // Fire goal recieved and the note is being fired from the catapult or being scored in the amp or trap.
+ FIRING = 6,
+}
+
// Contains if intake is intaking
enum IntakeRollerStatus : ubyte {
NONE = 0,
@@ -57,11 +78,30 @@
}
// Contains status of extend rollers
-// HAS_GAME_PIECE means we have a game piece
-// EMPTY means we don't have a game piece
enum ExtendRollerStatus: ubyte {
- HAS_GAME_PIECE = 0,
- EMPTY = 1
+ // Means the rollers are not moving.
+ IDLE = 0,
+ // Means we're transfer from the transfer rollers into the extend rollers.
+ TRANSFERING_TO_EXTEND = 1,
+ // Means we are transfering from the extend to the catapult.
+ TRANSFERING_TO_CATAPULT = 2,
+ // Means we're trying to score in the amp/trap
+ SCORING_IN_AMP = 3,
+ SCORING_IN_TRAP = 4,
+}
+
+// Contains the status of the extend subsystem
+enum ExtendStatus : ubyte {
+ // Means we are near 0 and ready to transfer a game piece.
+ RETRACTED = 0,
+ // Means we are moving to some goal.
+ MOVING = 1,
+ // Means we are currently at the catapult.
+ CATAPULT = 2,
+ // Means we are at the amp position.
+ AMP = 3,
+ // Means we are at the trap positon.
+ TRAP = 4,
}
table Status {
@@ -71,29 +111,33 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+ state : SuperstructureState (id: 2);
+
// Status of the rollers
- intake_roller:IntakeRollerStatus (id: 2);
+ intake_roller:IntakeRollerStatus (id: 3);
// Estimated angle and angular velocitiy of the intake.
- intake_pivot:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
+ intake_pivot:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 4);
// Status of transfer rollers
- transfer_roller:TransferRollerStatus (id: 4);
+ transfer_roller:TransferRollerStatus (id: 5);
// Estimated angle and angular velocitiy of the climber.
- climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 5);
+ climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6);
// Status of the subsytems involved in the shooter
- shooter:ShooterStatus (id: 6);
+ shooter:ShooterStatus (id: 7);
// Estimated angle and angular velocitiy of the extend.
- extend:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 7);
+ extend:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 8);
// State of the extender rollers
- extend_roller:ExtendRollerStatus (id: 8);
+ extend_roller:ExtendRollerStatus (id: 9);
// The status of if the turret and intake is colliding
- collided: bool (id: 9);
+ collided: bool (id: 10);
+
+ extend_status:ExtendStatus (id: 11);
}
root_type Status;