Merge "Add half of the first robots camera calibration"
diff --git a/y2024/BUILD b/y2024/BUILD
index 52386e8..f9102f9 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -296,6 +296,7 @@
"//frc971/autonomous:auto_fbs",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/input:action_joystick_input",
"//frc971/input:drivetrain_input",
"//frc971/input:joystick_input",
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
index 611f3cd..f69d9fc 100644
--- a/y2024/autonomous/BUILD
+++ b/y2024/autonomous/BUILD
@@ -49,6 +49,7 @@
"//aos/logging",
"//aos/util:phased_loop",
"//frc971/autonomous:base_autonomous_actor",
+ "//frc971/autonomous:user_button_localized_autonomous_actor",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
index 6b1d9a5..3c7f2fa 100644
--- a/y2024/autonomous/autonomous_actor.cc
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -34,74 +34,24 @@
namespace chrono = ::std::chrono;
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
- : frc971::autonomous::BaseAutonomousActor(
+ : frc971::autonomous::UserButtonLocalizedAutonomousActor(
event_loop,
control_loops::drivetrain::GetDrivetrainConfig(event_loop)),
localizer_control_sender_(
event_loop->MakeSender<
::frc971::control_loops::drivetrain::LocalizerControl>(
"/drivetrain")),
- joystick_state_fetcher_(
- event_loop->MakeFetcher<aos::JoystickState>("/aos")),
- robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
- auto_splines_(),
superstructure_goal_sender_(
- event_loop->MakeSender<::y2024::control_loops::superstructure::Goal>(
- "/superstructure")),
+ event_loop
+ ->MakeSender<::y2024::control_loops::superstructure::GoalStatic>(
+ "/superstructure")),
superstructure_status_fetcher_(
event_loop
->MakeFetcher<::y2024::control_loops::superstructure::Status>(
- "/superstructure")) {
- drivetrain_status_fetcher_.Fetch();
- replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
-
- event_loop->OnRun([this, event_loop]() {
- replan_timer_->Schedule(event_loop->monotonic_now());
- button_poll_->Schedule(event_loop->monotonic_now(),
- chrono::milliseconds(50));
- });
-
- // TODO(james): Really need to refactor this code since we keep using it.
- button_poll_ = event_loop->AddTimer([this]() {
- const aos::monotonic_clock::time_point now =
- this->event_loop()->context().monotonic_event_time;
- if (robot_state_fetcher_.Fetch()) {
- if (robot_state_fetcher_->user_button()) {
- user_indicated_safe_to_reset_ = true;
- MaybeSendStartingPosition();
- }
- }
- if (joystick_state_fetcher_.Fetch()) {
- if (joystick_state_fetcher_->has_alliance() &&
- (joystick_state_fetcher_->alliance() != alliance_)) {
- alliance_ = joystick_state_fetcher_->alliance();
- is_planned_ = false;
- // Only kick the planning out by 2 seconds. If we end up enabled in
- // that second, then we will kick it out further based on the code
- // below.
- replan_timer_->Schedule(now + std::chrono::seconds(2));
- }
- if (joystick_state_fetcher_->enabled()) {
- if (!is_planned_) {
- // Only replan once we've been disabled for 5 seconds.
- replan_timer_->Schedule(now + std::chrono::seconds(5));
- }
- }
- }
- });
-}
+ "/superstructure")),
+ auto_splines_() {}
void AutonomousActor::Replan() {
- if (!drivetrain_status_fetcher_.Fetch()) {
- replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
- AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
- return;
- }
-
- if (alliance_ == aos::Alliance::kInvalid) {
- return;
- }
- sent_starting_position_ = false;
if (FLAGS_spline_auto) {
test_spline_ =
PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
@@ -116,42 +66,8 @@
MaybeSendStartingPosition();
}
-void AutonomousActor::MaybeSendStartingPosition() {
- if (is_planned_ && user_indicated_safe_to_reset_ &&
- !sent_starting_position_) {
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
-}
-
-void AutonomousActor::Reset() {
- InitializeEncoders();
- ResetDrivetrain();
-
- joystick_state_fetcher_.Fetch();
- CHECK(joystick_state_fetcher_.get() != nullptr)
- << "Expect at least one JoystickState message before running auto...";
- alliance_ = joystick_state_fetcher_->alliance();
- preloaded_ = false;
-}
-
-bool AutonomousActor::RunAction(
+bool AutonomousActor::Run(
const ::frc971::autonomous::AutonomousActionParams *params) {
- Reset();
-
- AOS_LOG(INFO, "Params are %d\n", params->mode());
-
- if (!user_indicated_safe_to_reset_) {
- AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
- CHECK(starting_position_);
- SendStartingPosition(starting_position_.value());
- }
- // Clear this so that we don't accidentally resend things as soon as we
- // replan later.
- user_indicated_safe_to_reset_ = false;
- is_planned_ = false;
- starting_position_.reset();
-
AOS_LOG(INFO, "Params are %d\n", params->mode());
if (alliance_ == aos::Alliance::kInvalid) {
AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
@@ -165,15 +81,6 @@
return true;
}
-void AutonomousActor::SplineAuto() {
- CHECK(test_spline_);
-
- if (!test_spline_->WaitForPlan()) return;
- test_spline_->Start();
-
- if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
-}
-
void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
// Set up the starting position for the blue alliance.
@@ -192,4 +99,84 @@
AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
+
+void AutonomousActor::Reset() {
+ set_intake_goal(control_loops::superstructure::IntakeGoal::NONE);
+ set_note_goal(control_loops::superstructure::NoteGoal::NONE);
+ set_auto_aim(false);
+ set_fire(false);
+ set_preloaded(false);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendSuperstructureGoal() {
+ aos::Sender<control_loops::superstructure::GoalStatic>::StaticBuilder
+ goal_builder = superstructure_goal_sender_.MakeStaticBuilder();
+
+ goal_builder->set_intake_goal(intake_goal_);
+ goal_builder->set_note_goal(note_goal_);
+ goal_builder->set_fire(fire_);
+
+ control_loops::superstructure::ShooterGoalStatic *shooter_goal =
+ goal_builder->add_shooter_goal();
+
+ shooter_goal->set_auto_aim(auto_aim_);
+ shooter_goal->set_preloaded(preloaded_);
+
+ goal_builder.CheckOk(goal_builder.Send());
+}
+
+void AutonomousActor::Intake() {
+ set_intake_goal(control_loops::superstructure::IntakeGoal::INTAKE);
+ set_note_goal(control_loops::superstructure::NoteGoal::CATAPULT);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Aim() {
+ set_auto_aim(true);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Shoot() {
+ set_fire(true);
+ SendSuperstructureGoal();
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
+ set_preloaded(true);
+ SendSuperstructureGoal();
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ aos::common::actions::kLoopOffset);
+
+ bool loaded = false;
+ while (!loaded) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ loaded = (superstructure_status_fetcher_->shooter()->catapult_state() ==
+ control_loops::superstructure::CatapultState::LOADED);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
} // namespace y2024::autonomous
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
index c3d7d79..6796c94 100644
--- a/y2024/autonomous/autonomous_actor.h
+++ b/y2024/autonomous/autonomous_actor.h
@@ -3,62 +3,70 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
-#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/autonomous/user_button_localized_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2024/autonomous/auto_splines.h"
-#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_static.h"
+#include "y2024/control_loops/superstructure/superstructure_status_static.h"
namespace y2024::autonomous {
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor
+ : public ::frc971::autonomous::UserButtonLocalizedAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
- bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) override;
-
private:
+ void set_intake_goal(control_loops::superstructure::IntakeGoal intake_goal) {
+ intake_goal_ = intake_goal;
+ }
+ void set_note_goal(control_loops::superstructure::NoteGoal note_goal) {
+ note_goal_ = note_goal;
+ }
+ void set_auto_aim(bool auto_aim) { auto_aim_ = auto_aim; }
+ void set_fire(bool fire) { fire_ = fire; }
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
+
+ bool Run(const ::frc971::autonomous::AutonomousActionParams *params) override;
+ void Replan() override;
+ void SendStartingPosition(const Eigen::Vector3d &start) override;
+ void Reset() override;
+
+ void SplineAuto();
void SendSuperstructureGoal();
- void Reset();
+ void Intake();
+ void Aim();
+ void Shoot();
- void SendStartingPosition(const Eigen::Vector3d &start);
- void MaybeSendStartingPosition();
- void SplineAuto();
- void Replan();
+ [[nodiscard]] bool WaitForPreloaded();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
- aos::Fetcher<aos::RobotState> robot_state_fetcher_;
- aos::TimerHandler *replan_timer_;
- aos::TimerHandler *button_poll_;
+ aos::Sender<control_loops::superstructure::GoalStatic>
+ superstructure_goal_sender_;
- aos::Alliance alliance_ = aos::Alliance::kInvalid;
- AutonomousSplines auto_splines_;
- bool user_indicated_safe_to_reset_ = false;
- bool sent_starting_position_ = false;
-
- bool is_planned_ = false;
-
- std::optional<Eigen::Vector3d> starting_position_;
-
- bool preloaded_ = false;
-
- aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
aos::Fetcher<y2024::control_loops::superstructure::Status>
superstructure_status_fetcher_;
+ AutonomousSplines auto_splines_;
+
std::optional<SplineHandle> test_spline_;
- // List of arm angles from arm::PointsList
- const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+ control_loops::superstructure::IntakeGoal intake_goal_ =
+ control_loops::superstructure::IntakeGoal::NONE;
+
+ control_loops::superstructure::NoteGoal note_goal_ =
+ control_loops::superstructure::NoteGoal::NONE;
+
+ bool auto_aim_ = false;
+ bool fire_ = false;
+ bool preloaded_ = false;
};
} // namespace y2024::autonomous
-#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
\ No newline at end of file
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
index bcdd34a..1d18a63 100644
--- a/y2024/constants/BUILD
+++ b/y2024/constants/BUILD
@@ -1,5 +1,6 @@
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
load("//tools/build_rules:template.bzl", "jinja2_template")
+load("//y2024/constants:validator.bzl", "constants_json")
cc_library(
name = "simulated_constants_sender",
@@ -17,7 +18,7 @@
)
jinja2_template(
- name = "test_constants.json",
+ name = "test_constants_unvalidated.json",
src = "test_constants.jinja2.json",
includes = glob([
"test_data/*.json",
@@ -39,7 +40,7 @@
)
jinja2_template(
- name = "constants.json",
+ name = "constants_unvalidated.json",
src = "constants.jinja2.json",
includes = [
"7971.json",
@@ -95,15 +96,27 @@
],
)
-cc_test(
- name = "constants_validator_test",
- srcs = ["constants_validator_test.cc"],
- data = [":constants.json"],
+cc_binary(
+ name = "constants_formatter",
+ srcs = ["constants_formatter.cc"],
+ data = [":constants_unvalidated.json"],
visibility = ["//visibility:public"],
deps = [
":constants_list_fbs",
+ "//aos:init",
"//aos:json_to_flatbuffer",
- "//aos/testing:googletest",
"@com_github_google_glog//:glog",
],
)
+
+constants_json(
+ name = "constants_json",
+ src = ":constants_unvalidated.json",
+ out = "constants.json",
+)
+
+constants_json(
+ name = "test_constants_json",
+ src = ":constants_unvalidated.json",
+ out = "test_constants.json",
+)
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index 58f8d75..38d0a76 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -85,7 +85,7 @@
},
"climber_set_points": {
"full_extend": 0.8,
- "half_extend": 0.6,
+ "stowed": 0.4,
"retract": 0.2
},
"climber": {
@@ -215,5 +215,23 @@
"retention_roller_voltages": {
"retaining": 1.5,
"spitting": -6.0
- }
+ },
+ // TODO(Filip): Update the speaker and amp shooter setpoints
+ "shooter_speaker_set_point": {
+ "turret_position": 0.0,
+ "altitude_position": 0.0,
+ "shot_velocity": 0.0
+ },
+ "shooter_podium_set_point":{
+ "turret_position": 0.0,
+ "altitude_position": 0.0,
+ "shot_velocity": 0.0
+ },
+ "extend_set_points": {
+ "trap": 0.2,
+ "amp": 0.3,
+ "catapult": 0.1,
+ "retracted": 0.0
+ },
+ "turret_avoid_extend_collision_position": 1.0
}
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index df9a6fb..883a945 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -51,7 +51,7 @@
// and retracted, which represents meters for when ClimberGoal is RETRACT
table ClimberSetPoints {
full_extend:double (id: 0);
- half_extend:double (id: 1);
+ stowed:double (id: 1);
retract:double (id: 2);
}
@@ -97,8 +97,14 @@
// Extend positions
table ExtendSetPoints {
- extended:double (id: 0);
- retracted:double (id: 1);
+ // The position which lets us score on the trap.
+ trap:double (id: 0);
+ // The position which lets us score on the amp.
+ amp:double (id: 1);
+ // The position which lets us transfer into the catapult.
+ catapult:double (id: 2);
+ // The position near 0 where we are ready to intake a note.
+ retracted:double (id: 3);
}
table RobotConstants {
@@ -110,6 +116,12 @@
extend_constants:PotAndAbsEncoderConstants (id: 5);
}
+table ShooterSetPoint {
+ turret_position: double (id: 0);
+ altitude_position: double (id: 1);
+ shot_velocity:double (id: 2);
+}
+
table Pose {
// Pos is a 3x1 matrix which contains the (x, y, z) component of the Pose.
pos: frc971.fbs.Matrix (id: 0);
@@ -145,13 +157,17 @@
altitude:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 13);
turret:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 14);
extend:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 15);
- extend_setpoints:ExtendSetPoints (id: 16);
- extend_roller_voltages:ExtendRollerVoltages (id: 17);
- shooter_targets:ShooterTargets (id: 18);
- altitude_loading_position: double (id: 19);
- retention_roller_voltages:RetentionRollerVoltages (id: 20);
-
- min_altitude_shooting_angle: double (id: 21);
+ extend_roller_voltages:ExtendRollerVoltages (id: 16);
+ shooter_targets:ShooterTargets (id: 17);
+ altitude_loading_position: double (id: 18);
+ retention_roller_voltages:RetentionRollerVoltages (id: 19);
+ min_altitude_shooting_angle: double (id: 20);
+ shooter_speaker_set_point: ShooterSetPoint (id: 21);
+ shooter_podium_set_point: ShooterSetPoint (id: 22);
+ extend_set_points:ExtendSetPoints (id: 23);
+ // The position to move the turret to when avoiding collision
+ // with the extend when the extend is moving to amp/trap position.
+ turret_avoid_extend_collision_position: double (id: 24);
}
table Constants {
diff --git a/y2024/constants/constants_formatter.cc b/y2024/constants/constants_formatter.cc
new file mode 100644
index 0000000..d857407
--- /dev/null
+++ b/y2024/constants/constants_formatter.cc
@@ -0,0 +1,26 @@
+#include "glog/logging.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/util/file.h"
+#include "y2024/constants/constants_list_generated.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ CHECK(argc == 3) << ": Expected input and output json files to be passed in.";
+
+ aos::FlatbufferDetachedBuffer<y2024::ConstantsList> constants =
+ aos::JsonFileToFlatbuffer<y2024::ConstantsList>(argv[1]);
+
+ // Make sure the file is valid json before we output a formatted version.
+ CHECK(constants.message().constants() != nullptr)
+ << ": Failed to parse " << std::string(argv[2]);
+
+ aos::util::WriteStringToFileOrDie(
+ std::string(argv[2]),
+ aos::FlatbufferToJson(constants, {.multi_line = true}));
+
+ return 0;
+}
diff --git a/y2024/constants/constants_validator_test.cc b/y2024/constants/constants_validator_test.cc
deleted file mode 100644
index 79dedf6..0000000
--- a/y2024/constants/constants_validator_test.cc
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "glog/logging.h"
-#include "gtest/gtest.h"
-
-#include "aos/json_to_flatbuffer.h"
-#include "y2024/constants/constants_list_generated.h"
-
-namespace y2024::constants::testing {
-class ConstantsValidatorTest : public ::testing::Test {};
-
-TEST_F(ConstantsValidatorTest, CheckConstants) {
- ASSERT_TRUE(aos::JsonFileToFlatbuffer<y2024::ConstantsList>(
- "y2024/constants/constants.json")
- .message()
- .constants() != nullptr)
- << ": Failed to parse y2024/constants/constants.json";
-}
-
-} // namespace y2024::constants::testing
diff --git a/y2024/constants/test_data/test_team.json b/y2024/constants/test_data/test_team.json
index 3c69701..ec59033 100644
--- a/y2024/constants/test_data/test_team.json
+++ b/y2024/constants/test_data/test_team.json
@@ -3,6 +3,7 @@
{% from 'y2024/constants/common.jinja2' import catapult_zero %}
{% from 'y2024/constants/common.jinja2' import altitude_zero %}
{% from 'y2024/constants/common.jinja2' import turret_zero %}
+{% from 'y2024/constants/common.jinja2' import extend_zero %}
{
"cameras": [
@@ -52,8 +53,16 @@
) %}
"zeroing_constants": {{ turret_zero | tojson(indent=2)}},
"potentiometer_offset": 0.0
+ },
+ "extend_constants": {
+ {% set _ = extend_zero.update(
+ {
+ "measured_absolute_position" : 0.0
+ }
+ ) %}
+ "zeroing_constants": {{ extend_zero | tojson(indent=2)}},
+ "potentiometer_offset": 0.0
}
},
{% include 'y2024/constants/common.json' %}
}
-
diff --git a/y2024/constants/validator.bzl b/y2024/constants/validator.bzl
new file mode 100644
index 0000000..6121f58
--- /dev/null
+++ b/y2024/constants/validator.bzl
@@ -0,0 +1,13 @@
+load("@aspect_bazel_lib//lib:run_binary.bzl", "run_binary")
+
+# Validates the constants.json file and outputs a formatted version.
+# TODO(max): Make this generic/template it out into frc971
+def constants_json(name, src, out):
+ run_binary(
+ name = name,
+ tool = "//y2024/constants:constants_formatter",
+ srcs = [src],
+ outs = [out],
+ args = ["$(location %s)" % (src)] + ["$(location %s)" % (out)],
+ visibility = ["//visibility:public"],
+ )
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..ce5b6a9 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();
@@ -122,19 +83,376 @@
climber_position =
robot_constants_->common()->climber_set_points()->full_extend();
break;
- case ClimberGoal::HALF_EXTEND:
- climber_position =
- robot_constants_->common()->climber_set_points()->half_extend();
- break;
case ClimberGoal::RETRACT:
climber_position =
robot_constants_->common()->climber_set_points()->retract();
break;
- default:
+ case ClimberGoal::STOWED:
+ climber_position =
+ robot_constants_->common()->climber_set_points()->stowed();
+ }
+ }
+
+ // 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 +501,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 +517,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 +535,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..42caf9b 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -3,79 +3,60 @@
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
// FULL_EXTEND is for fully extending the climber
-// HALF_EXTEND is for partially extending the climber
// RETRACT is for retracting the climber
enum ClimberGoal : ubyte {
FULL_EXTEND = 0,
- HALF_EXTEND = 1,
- RETRACT = 2,
+ RETRACT = 1,
+ STOWED = 2,
}
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);
+ // If true, we started with the ball loaded and should proceed to that state.
+ 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);
+ climber_goal:ClimberGoal (id: 1);
+ shooter_goal:ShooterGoal (id: 2);
+ note_goal:NoteGoal (id: 3);
+ fire: bool (id: 4);
}
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..eb58424 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()) {
@@ -411,15 +454,36 @@
set_point = simulated_robot_constants_->common()
->climber_set_points()
->full_extend();
- } else if (superstructure_goal_fetcher_->climber_goal() ==
- ClimberGoal::HALF_EXTEND) {
+ }
+
+ if (superstructure_goal_fetcher_->climber_goal() == ClimberGoal::STOWED) {
set_point = simulated_robot_constants_->common()
->climber_set_points()
- ->half_extend();
+ ->stowed();
+ }
+ 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_->climber()->position(), 0.001);
+ superstructure_status_fetcher_->extend()->position(), 0.001);
}
}
@@ -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
@@ -779,22 +859,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_climber_goal(ClimberGoal::HALF_EXTEND);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(chrono::seconds(5));
-
- VerifyNearGoal();
-
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
@@ -822,14 +886,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 +903,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 +923,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 +938,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 +953,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 +993,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 +1030,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 +1057,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 +1198,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 +1207,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 +1248,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 +1255,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 +1434,190 @@
superstructure_status_fetcher_->shooter()->aimer()->target_distance());
}
+// 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);
+}
+
+TEST_F(SuperstructureTest, Climbing) {
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::STOWED);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_climber_goal(ClimberGoal::RETRACT);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ // TODO(max): Fill this with the logic to move the altitude and turret on the
+ // chain.
+}
} // namespace y2024::control_loops::superstructure::testing
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;
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
index baac66f..6344127 100644
--- a/y2024/joystick_reader.cc
+++ b/y2024/joystick_reader.cc
@@ -13,6 +13,7 @@
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/input/action_joystick_input.h"
#include "frc971/input/driver_station_data.h"
#include "frc971/input/drivetrain_input.h"
@@ -21,11 +22,10 @@
#include "frc971/zeroing/wrap.h"
#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
-#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_static.h"
+#include "y2024/control_loops/superstructure/superstructure_status_static.h"
+
using frc971::CreateProfileParameters;
-using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
-using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using frc971::input::driver_station::ButtonLocation;
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
@@ -34,20 +34,39 @@
namespace y2024::input::joysticks {
+namespace superstructure = y2024::control_loops::superstructure;
+
+// TODO(Xander): add x,y location from physical wiring
+const ButtonLocation kIntake(0, 0);
+const ButtonLocation kSpit(0, 0);
+const ButtonLocation kCatapultLoad(0, 0);
+const ButtonLocation kAmp(0, 0);
+const ButtonLocation kTrap(0, 0);
+const ButtonLocation kAutoAim(0, 0);
+const ButtonLocation kAimSpeaker(0, 0);
+const ButtonLocation kAimPodium(0, 0);
+const ButtonLocation kShoot(0, 0);
+const ButtonLocation kClimb(0, 0);
+const ButtonLocation kExtraButtonOne(0, 0);
+const ButtonLocation kExtraButtonTwo(0, 0);
+const ButtonLocation kExtraButtonThree(0, 0);
+const ButtonLocation kExtraButtonFour(0, 0);
+
class Reader : public ::frc971::input::ActionJoystickInput {
public:
- Reader(::aos::EventLoop *event_loop)
+ Reader(::aos::EventLoop *event_loop, const y2024::Constants *robot_constants)
: ::frc971::input::ActionJoystickInput(
event_loop,
::y2024::control_loops::drivetrain::GetDrivetrainConfig(event_loop),
::frc971::input::DrivetrainInputReader::InputType::kPistol,
{.use_redundant_joysticks = true}),
superstructure_goal_sender_(
- event_loop->MakeSender<control_loops::superstructure::Goal>(
+ event_loop->MakeSender<control_loops::superstructure::GoalStatic>(
"/superstructure")),
superstructure_status_fetcher_(
event_loop->MakeFetcher<control_loops::superstructure::Status>(
- "/superstructure")) {}
+ "/superstructure")),
+ robot_constants_(CHECK_NOTNULL(robot_constants)) {}
void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
@@ -59,13 +78,93 @@
AOS_LOG(ERROR, "Got no superstructure status message.\n");
return;
}
+
+ aos::Sender<superstructure::GoalStatic>::StaticBuilder
+ superstructure_goal_builder =
+ superstructure_goal_sender_.MakeStaticBuilder();
+
+ if (data.IsPressed(kIntake)) {
+ // Intake is pressed
+ superstructure_goal_builder->set_intake_goal(
+ superstructure::IntakeGoal::INTAKE);
+ } else if (data.IsPressed(kSpit)) {
+ // If Intake not pressed and spit pressed, spit
+ superstructure_goal_builder->set_intake_goal(
+ superstructure::IntakeGoal::SPIT);
+ }
+
+ // Set note goal for the robot. Loading the catapult will always be
+ // preferred over scoring in the Amp or Trap.
+ if (data.IsPressed(kCatapultLoad)) {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::CATAPULT);
+ } else if (data.IsPressed(kAmp)) {
+ superstructure_goal_builder->set_note_goal(superstructure::NoteGoal::AMP);
+ } else if (data.IsPressed(kTrap)) {
+ superstructure_goal_builder->set_note_goal(
+ superstructure::NoteGoal::TRAP);
+ }
+
+ // Firing note when requested
+ superstructure_goal_builder->set_fire(data.IsPressed(kShoot));
+
+ // Shooter goal contains all speaker-related goals
+ auto shooter_goal = superstructure_goal_builder->add_shooter_goal();
+
+ shooter_goal->set_auto_aim(data.IsPressed(kAimSpeaker));
+
+ // Updating aiming for shooter goal, only one type of aim should be possible
+ // at a time, auto-aiming is preferred over the setpoints.
+ if (data.IsPressed(kAutoAim)) {
+ shooter_goal->set_auto_aim(true);
+ } else if (data.IsPressed(kAimSpeaker)) {
+ auto catapult_goal = shooter_goal->add_catapult_goal();
+ catapult_goal->set_shot_velocity(robot_constants_->common()
+ ->shooter_speaker_set_point()
+ ->shot_velocity());
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_altitude_position(),
+ robot_constants_->common()
+ ->shooter_speaker_set_point()
+ ->altitude_position());
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_turret_position(), robot_constants_->common()
+ ->shooter_speaker_set_point()
+ ->turret_position());
+ } else if (data.IsPressed(kAimPodium)) {
+ auto catapult_goal = shooter_goal->add_catapult_goal();
+ catapult_goal->set_shot_velocity(robot_constants_->common()
+ ->shooter_podium_set_point()
+ ->shot_velocity());
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_altitude_position(),
+ robot_constants_->common()
+ ->shooter_podium_set_point()
+ ->altitude_position());
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ shooter_goal->add_turret_position(), robot_constants_->common()
+ ->shooter_podium_set_point()
+ ->turret_position());
+ }
+
+ // Extend climbers if pressed, retract otherwise
+ if (data.IsPressed(kClimb)) {
+ superstructure_goal_builder->set_climber_goal(
+ superstructure::ClimberGoal::FULL_EXTEND);
+ } else {
+ superstructure_goal_builder->set_climber_goal(
+ superstructure::ClimberGoal::RETRACT);
+ }
+
+ superstructure_goal_builder.CheckOk(superstructure_goal_builder.Send());
}
private:
- ::aos::Sender<control_loops::superstructure::Goal>
+ ::aos::Sender<control_loops::superstructure::GoalStatic>
superstructure_goal_sender_;
::aos::Fetcher<control_loops::superstructure::Status>
superstructure_status_fetcher_;
+ const y2024::Constants *robot_constants_;
};
} // namespace y2024::input::joysticks
@@ -77,8 +176,13 @@
aos::configuration::ReadConfig("aos_config.json");
frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+ frc971::constants::ConstantsFetcher<y2024::Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+ const y2024::Constants *robot_constants = &constants_fetcher.constants();
+
::aos::ShmEventLoop event_loop(&config.message());
- ::y2024::input::joysticks::Reader reader(&event_loop);
+ ::y2024::input::joysticks::Reader reader(&event_loop, robot_constants);
event_loop.Run();
diff --git a/y2024/www/field.html b/y2024/www/field.html
index ac94060..8bdcf71 100644
--- a/y2024/www/field.html
+++ b/y2024/www/field.html
@@ -9,7 +9,7 @@
<div id="readouts">
<table>
<tr>
- <th colspan="2">Robot State</th>
+ <th colspan="2" style="text-align: center;">Robot State</th>
</tr>
<tr>
<td>X</td>
@@ -27,7 +27,7 @@
<table>
<tr>
- <th colspan="2">Images</th>
+ <th colspan="2" style="text-align: center;">Images</th>
</tr>
<tr>
<td> Images Accepted </td>
@@ -35,25 +35,141 @@
</tr>
</table>
- <table>
- <tr>
- <th colspan="2">Superstructure</th>
- </tr>
- <!-- TODO: Add superstructure state -->
- </table>
- <table>
+ <table>
<tr>
- <th colspan="2">Game Piece</th>
+ <th colspan="2" style="text-align: center;">Superstructure</th>
</tr>
<tr>
- <td>Game Piece Held</td>
- <td id="game_piece"> NA </td>
+ <td>Superstructure State</td>
+ <td id="superstructure_state"> NA </td>
</tr>
<tr>
- <td>Game Piece Position (+ = left, 0 = empty)</td>
- <td id="game_piece_position"> NA </td>
+ <td>Intake Roller State</td>
+ <td id="intake_roller_state"> NA </td>
</tr>
- </table>
+ <tr>
+ <td>Transfer Roller State</td>
+ <td id="transfer_roller_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Extend State</td>
+ <td id="extend_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Extend Roller State</td>
+ <td id="extend_roller_state"> NA </td>
+ </tr>
+ <tr>
+ <th colspan="2">Intake Pivot</th>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="intake_pivot"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="intake_pivot_abs"> NA </td>
+ </tr>
+ <tr>
+ <th colspan="2">Climber</th>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="climber"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="climber_abs"> NA </td>
+ </tr>
+ <tr>
+ <td>Pot Position</td>
+ <td id="climber_pot"> NA </td>
+ </tr>
+ <tr>
+ <th colspan="2">Extend</th>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="extend"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="extend_abs"> NA </td>
+ </tr>
+ <tr>
+ <td>Pot Position</td>
+ <td id="extend_pot"> NA </td>
+ </tr>
+ <tr>
+ <th colspan="2">Turret</th>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="turret"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="turret_abs"> NA </td>
+ </tr>
+ <tr>
+ <td>Pot Position</td>
+ <td id="turret_pot"> NA </td>
+ </tr>
+ <th colspan="2">Catapult</th>
+ </tr>
+ <tr>
+ <td>Catapult State</td>
+ <td id="catapult_state"> NA </td>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="catapult"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="catapult_abs"> NA </td>
+ </tr>
+ <tr>
+ <td>Pot Position</td>
+ <td id="catapult_pot"> NA </td>
+ </tr>
+ <tr>
+ <th colspan="2">Altitude</th>
+ </tr>
+ <tr>
+ <td>Position</td>
+ <td id="altitude"> NA </td>
+ </tr>
+ <tr>
+ <td>Absolute Position</td>
+ <td id="altitude_abs"> NA </td>
+ </tr>
+ <tr>
+ <td>Pot Position</td>
+ <td id="altitude_pot"> NA </td>
+ </tr>
+ </table>
+ <table>
+ <tr>
+ <th colspan="2" style="text-align: center;">Aimer</th>
+ </tr>
+ <tr>
+ <td>Turret Position</td>
+ <td id="turret_position"> NA </td>
+ </tr>
+ <tr>
+ <td>Turret Velocity </td>
+ <td id="turret_velocity"> NA </td>
+ </tr>
+ <tr>
+ <td>Target Distance</td>
+ <td id="target_distance"> NA </td>
+ </tr>
+ <tr>
+ <td>Shot Distance</td>
+ <td id="shot_distance"> NA </td>
+ </tr>
+ </table>
<h3>Zeroing Faults:</h3>
<p id="zeroing_faults"> NA </p>
@@ -71,7 +187,7 @@
</div>
<table>
<tr>
- <th colspan="2"> Drivetrain Encoder Positions </th>
+ <th colspan="2" style="text-align: center;"> Drivetrain Encoder Positions </th>
</tr>
<tr>
<td> Left Encoder Position</td>
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
index 81050dd..fe13fcb 100644
--- a/y2024/www/field_handler.ts
+++ b/y2024/www/field_handler.ts
@@ -6,6 +6,7 @@
import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {SuperstructureState, IntakeRollerStatus, CatapultState, TransferRollerStatus, ExtendRollerStatus, ExtendStatus, Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated'
import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
import {TargetMap} from '../../frc971/vision/target_map_generated'
@@ -25,6 +26,7 @@
private drivetrainStatus: DrivetrainStatus|null = null;
private drivetrainPosition: DrivetrainPosition|null = null;
private drivetrainCANPosition: DrivetrainCANPosition|null = null;
+ private superstructureStatus: SuperstructureStatus|null = null;
private x: HTMLElement = (document.getElementById('x') as HTMLElement);
private y: HTMLElement = (document.getElementById('y') as HTMLElement);
@@ -33,6 +35,72 @@
private fieldImage: HTMLImageElement = new Image();
+ private zeroingFaults: HTMLElement =
+ (document.getElementById('zeroing_faults') as HTMLElement);
+
+ private superstructureState: HTMLElement =
+ (document.getElementById('superstructure_state') as HTMLElement);
+
+ private intakeRollerState: HTMLElement =
+ (document.getElementById('intake_roller_state') as HTMLElement);
+ private transferRollerState: HTMLElement =
+ (document.getElementById('transfer_roller_state') as HTMLElement);
+ private extendState: HTMLElement =
+ (document.getElementById('extend_state') as HTMLElement);
+ private extendRollerState: HTMLElement =
+ (document.getElementById('extend_roller_state') as HTMLElement);
+ private catapultState: HTMLElement =
+ (document.getElementById('catapult_state') as HTMLElement);
+
+ private intakePivot: HTMLElement =
+ (document.getElementById('intake_pivot') as HTMLElement);
+ private intakePivotAbs: HTMLElement =
+ (document.getElementById('intake_pivot_abs') as HTMLElement);
+
+ private climber: HTMLElement =
+ (document.getElementById('climber') as HTMLElement);
+ private climberAbs: HTMLElement =
+ (document.getElementById('climber_abs') as HTMLElement);
+ private climberPot: HTMLElement =
+ (document.getElementById('climber_pot') as HTMLElement);
+
+ private extend: HTMLElement =
+ (document.getElementById('extend') as HTMLElement);
+ private extendAbs: HTMLElement =
+ (document.getElementById('extend_abs') as HTMLElement);
+ private extendPot: HTMLElement =
+ (document.getElementById('extend_pot') as HTMLElement);
+
+ private turret: HTMLElement =
+ (document.getElementById('turret') as HTMLElement);
+ private turretAbs: HTMLElement =
+ (document.getElementById('turret_abs') as HTMLElement);
+ private turretPot: HTMLElement =
+ (document.getElementById('turret_pot') as HTMLElement);
+
+ private catapult: HTMLElement =
+ (document.getElementById('catapult') as HTMLElement);
+ private catapultAbs: HTMLElement =
+ (document.getElementById('turret_abs') as HTMLElement);
+ private catapultPot: HTMLElement =
+ (document.getElementById('catapult_pot') as HTMLElement);
+
+ private altitude: HTMLElement =
+ (document.getElementById('altitude') as HTMLElement);
+ private altitudeAbs: HTMLElement =
+ (document.getElementById('altitude_abs') as HTMLElement);
+ private altitudePot: HTMLElement =
+ (document.getElementById('altitude_pot') as HTMLElement);
+
+ private turret_position: HTMLElement =
+ (document.getElementById('turret_position') as HTMLElement);
+ private turret_velocity: HTMLElement =
+ (document.getElementById('turret_velocity') as HTMLElement);
+ private target_distance: HTMLElement =
+ (document.getElementById('target_distance') as HTMLElement);
+ private shot_distance: HTMLElement =
+ (document.getElementById('shot_distance') as HTMLElement);
+
private leftDrivetrainEncoder: HTMLElement =
(document.getElementById('left_drivetrain_encoder') as HTMLElement);
private rightDrivetrainEncoder: HTMLElement =
@@ -69,6 +137,11 @@
'/localizer', 'frc971.controls.LocalizerOutput', (data) => {
this.handleLocalizerOutput(data);
});
+ this.connection.addHandler(
+ '/superstructure', "y2024.control_loops.superstructure.Status",
+ (data) => {
+ this.handleSuperstructureStatus(data)
+ });
});
}
@@ -92,6 +165,11 @@
this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
}
+ private handleSuperstructureStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
+ }
+
drawField(): void {
const ctx = this.canvas.getContext('2d');
ctx.save();
@@ -102,6 +180,24 @@
ctx.restore();
}
+ drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+ void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.beginPath();
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
drawRobot(
x: number, y: number, theta: number, color: string = 'blue',
dashed: boolean = false): void {
@@ -134,24 +230,212 @@
div.classList.remove('faulted');
div.classList.add('zeroing');
div.classList.remove('near');
-}
+ }
+
+ setEstopped(div: HTMLElement): void {
+ div.innerHTML = 'estopped';
+ div.classList.add('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ setTargetValue(
+ div: HTMLElement, target: number, val: number, tolerance: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ if (Math.abs(target - val) < tolerance) {
+ div.classList.add('near');
+ } else {
+ div.classList.remove('near');
+ }
+ }
setValue(div: HTMLElement, val: number): void {
div.innerHTML = val.toFixed(4);
div.classList.remove('faulted');
div.classList.remove('zeroing');
div.classList.remove('near');
-}
+ }
draw(): void {
this.reset();
this.drawField();
- if (this.drivetrainPosition) {
- this.leftDrivetrainEncoder.innerHTML =
- this.drivetrainPosition.leftEncoder().toString();
+ if (this.superstructureStatus) {
+ this.superstructureState.innerHTML =
+ SuperstructureState[this.superstructureStatus.state()];
- this.rightDrivetrainEncoder.innerHTML =
- this.drivetrainPosition.rightEncoder().toString();
+ this.intakeRollerState.innerHTML =
+ IntakeRollerStatus[this.superstructureStatus.intakeRoller()];
+ this.transferRollerState.innerHTML =
+ TransferRollerStatus[this.superstructureStatus.transferRoller()];
+ this.extendState.innerHTML =
+ ExtendStatus[this.superstructureStatus.extendStatus()];
+ this.extendRollerState.innerHTML =
+ ExtendRollerStatus[this.superstructureStatus.extendRoller()];
+ this.catapultState.innerHTML =
+ CatapultState[this.superstructureStatus.shooter().catapultState()];
+
+ this.turret_position.innerHTML = this.superstructureStatus.shooter().aimer().turretPosition().toString();
+ this.turret_velocity.innerHTML = this.superstructureStatus.shooter().aimer().turretVelocity().toString();
+ this.target_distance.innerHTML = this.superstructureStatus.shooter().aimer().targetDistance().toString();
+ this.shot_distance.innerHTML = this.superstructureStatus.shooter().aimer().shotDistance().toString();
+
+ if (!this.superstructureStatus.intakePivot() ||
+ !this.superstructureStatus.intakePivot().zeroed()) {
+ this.setZeroing(this.intakePivot);
+ } else if (this.superstructureStatus.intakePivot().estopped()) {
+ this.setEstopped(this.intakePivot);
+ } else {
+ this.setTargetValue(
+ this.intakePivot,
+ this.superstructureStatus.intakePivot().unprofiledGoalPosition(),
+ this.superstructureStatus.intakePivot().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.intakePivotAbs.innerHTML = this.superstructureStatus.intakePivot().estimatorState().absolutePosition().toString();
+
+ if (!this.superstructureStatus.climber() ||
+ !this.superstructureStatus.climber().zeroed()) {
+ this.setZeroing(this.climber);
+ } else if (this.superstructureStatus.climber().estopped()) {
+ this.setEstopped(this.climber);
+ } else {
+ this.setTargetValue(
+ this.climber,
+ this.superstructureStatus.climber().unprofiledGoalPosition(),
+ this.superstructureStatus.climber().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.climberAbs.innerHTML = this.superstructureStatus.climber().estimatorState().absolutePosition().toString();
+ this.climberPot.innerHTML = this.superstructureStatus.climber().estimatorState().potPosition().toString();
+
+ if (!this.superstructureStatus.extend() ||
+ !this.superstructureStatus.extend().zeroed()) {
+ this.setZeroing(this.extend);
+ } else if (this.superstructureStatus.extend().estopped()) {
+ this.setEstopped(this.extend);
+ } else {
+ this.setTargetValue(
+ this.climber,
+ this.superstructureStatus.extend().unprofiledGoalPosition(),
+ this.superstructureStatus.extend().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.extendAbs.innerHTML = this.superstructureStatus.extend().estimatorState().absolutePosition().toString();
+ this.extendPot.innerHTML = this.superstructureStatus.extend().estimatorState().potPosition().toString();
+
+ if (!this.superstructureStatus.shooter().turret() ||
+ !this.superstructureStatus.shooter().turret().zeroed()) {
+ this.setZeroing(this.turret);
+ } else if (this.superstructureStatus.shooter().turret().estopped()) {
+ this.setEstopped(this.turret);
+ } else {
+ this.setTargetValue(
+ this.turret,
+ this.superstructureStatus.shooter().turret().unprofiledGoalPosition(),
+ this.superstructureStatus.shooter().turret().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.turretAbs.innerHTML = this.superstructureStatus.shooter().turret().estimatorState().absolutePosition().toString();
+ this.turretPot.innerHTML = this.superstructureStatus.shooter().turret().estimatorState().potPosition().toString();
+
+ if (!this.superstructureStatus.shooter().catapult() ||
+ !this.superstructureStatus.shooter().catapult().zeroed()) {
+ this.setZeroing(this.catapult);
+ } else if (this.superstructureStatus.shooter().catapult().estopped()) {
+ this.setEstopped(this.catapult);
+ } else {
+ this.setTargetValue(
+ this.catapult,
+ this.superstructureStatus.shooter().catapult().unprofiledGoalPosition(),
+ this.superstructureStatus.shooter().catapult().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.catapultAbs.innerHTML = this.superstructureStatus.shooter().catapult().estimatorState().absolutePosition().toString();
+ this.catapultPot.innerHTML = this.superstructureStatus.shooter().catapult().estimatorState().potPosition().toString();
+
+ if (!this.superstructureStatus.shooter().altitude() ||
+ !this.superstructureStatus.shooter().altitude().zeroed()) {
+ this.setZeroing(this.altitude);
+ } else if (this.superstructureStatus.shooter().altitude().estopped()) {
+ this.setEstopped(this.altitude);
+ } else {
+ this.setTargetValue(
+ this.altitude,
+ this.superstructureStatus.shooter().altitude().unprofiledGoalPosition(),
+ this.superstructureStatus.shooter().altitude().estimatorState().position(),
+ 1e-3);
+ }
+
+ this.altitudeAbs.innerHTML = this.superstructureStatus.shooter().altitude().estimatorState().absolutePosition().toString();
+ this.altitudePot.innerHTML = this.superstructureStatus.shooter().altitude().estimatorState().potPosition().toString();
+
+ let zeroingErrors: string = 'Intake Pivot Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.intakePivot()
+ .estimatorState()
+ .errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.intakePivot()
+ .estimatorState()
+ .errors(i)] +
+ '<br/>';
+ }
+ zeroingErrors += '<br/>' +
+ 'Climber Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.climber().estimatorState().errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.climber().estimatorState().errors(i)] +
+ '<br/>';
+ }
+ zeroingErrors += '<br/>' +
+ 'Extend Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.extend().estimatorState().errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.extend().estimatorState().errors(i)] +
+ '<br/>';
+ }
+ zeroingErrors += '<br/>' +
+ 'Turret Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.shooter().turret().estimatorState().errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.shooter().turret().estimatorState().errors(i)] +
+ '<br/>';
+ }
+ zeroingErrors += '<br/>' +
+ 'Catapult Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.shooter().catapult().estimatorState().errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.shooter().catapult().estimatorState().errors(i)] +
+ '<br/>';
+ }
+ zeroingErrors += '<br/>' +
+ 'Altitude Errors:' +
+ '<br/>';
+ for (let i = 0; i < this.superstructureStatus.shooter().altitude().estimatorState().errorsLength();
+ i++) {
+ zeroingErrors += ZeroingError[this.superstructureStatus.shooter().altitude().estimatorState().errors(i)] +
+ '<br/>';
+ }
+ this.zeroingFaults.innerHTML = zeroingErrors;
+ }
+
+ if (this.drivetrainPosition) {
+ this.leftDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.leftEncoder().toString();
+
+ this.rightDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.rightEncoder().toString();
}
if (this.drivetrainCANPosition) {
@@ -191,6 +475,7 @@
this.localizerOutput.x(), this.localizerOutput.y(),
this.localizerOutput.theta());
}
+
window.requestAnimationFrame(() => this.draw());
}
diff --git a/y2024/www/styles.css b/y2024/www/styles.css
index c2c44d2..3259478 100644
--- a/y2024/www/styles.css
+++ b/y2024/www/styles.css
@@ -25,13 +25,20 @@
border-collapse: collapse;
padding: 5px;
margin: 10px;
+ table-layout: fixed;
+ width: 450px;
+ overflow: hidden;
}
th, td {
- text-align: right;
+ text-align: left;
width: 70px;
}
+table:first-child {
+ text-align: center;
+}
+
td:first-child {
width: 150px;
}
@@ -71,4 +78,4 @@
display: table-cell;
padding: 5px;
text-align: right;
-}
+}
\ No newline at end of file