Merge "Add human player station to target selector"
diff --git a/frc971/rockpi/contents/etc/systemd/system/tmp.mount b/frc971/rockpi/contents/etc/systemd/system/tmp.mount
new file mode 100644
index 0000000..517dd49
--- /dev/null
+++ b/frc971/rockpi/contents/etc/systemd/system/tmp.mount
@@ -0,0 +1,28 @@
+# SPDX-License-Identifier: LGPL-2.1-or-later
+#
+# This file is part of systemd.
+#
+# systemd is free software; you can redistribute it and/or modify it
+# under the terms of the GNU Lesser General Public License as published by
+# the Free Software Foundation; either version 2.1 of the License, or
+# (at your option) any later version.
+
+[Unit]
+Description=Temporary Directory /tmp
+Documentation=https://systemd.io/TEMPORARY_DIRECTORIES
+Documentation=man:file-hierarchy(7)
+Documentation=https://www.freedesktop.org/wiki/Software/systemd/APIFileSystems
+ConditionPathIsSymbolicLink=!/tmp
+DefaultDependencies=no
+Conflicts=umount.target
+Before=local-fs.target umount.target
+After=swap.target
+
+[Mount]
+What=tmpfs
+Where=/tmp
+Type=tmpfs
+Options=mode=1777,nosuid,nodev
+
+[Install]
+WantedBy=local-fs.target
diff --git a/y2023/autonomous/BUILD b/y2023/autonomous/BUILD
index 6d6922b..cfcc19c 100644
--- a/y2023/autonomous/BUILD
+++ b/y2023/autonomous/BUILD
@@ -53,9 +53,11 @@
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config",
"//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2023:constants",
"//y2023/control_loops/drivetrain:drivetrain_base",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
"//y2023/control_loops/superstructure:superstructure_status_fbs",
+ "//y2023/control_loops/superstructure/arm:generated_graph",
],
)
diff --git a/y2023/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index 0cc98a2..09cf4ea 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -4,7 +4,7 @@
#include "aos/flatbuffer_merge.h"
namespace y2023 {
-namespace actors {
+namespace autonomous {
namespace {
flatbuffers::Offset<frc971::MultiSpline> FixSpline(
@@ -92,6 +92,66 @@
return FixSpline(builder, multispline_builder.Finish(), alliance);
}
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_1_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_2_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_3_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_4_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::Spline5(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(spline_5_, builder->fbb()),
+ alliance);
+}
+
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -112,15 +172,5 @@
return FixSpline(builder, multispline_builder.Finish(), alliance);
}
-flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
- aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
- *builder,
- aos::Alliance alliance) {
- return FixSpline(
- builder,
- aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
- alliance);
-}
-
-} // namespace actors
+} // namespace autonomous
} // namespace y2023
diff --git a/y2023/autonomous/auto_splines.h b/y2023/autonomous/auto_splines.h
index 1280693..5c3bff9 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -1,10 +1,12 @@
-#ifndef Y2023_ACTORS_AUTO_SPLINES_H_
-#define Y2023_ACTORS_AUTO_SPLINES_H_
+#ifndef Y2023_AUTONOMOUS_AUTO_SPLINES_H_
+#define Y2023_AUTONOMOUS_AUTO_SPLINES_H_
#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/input/joystick_state_generated.h"
/*
The cooridinate system for the autonomous splines is the same as the spline
@@ -13,13 +15,23 @@
*/
namespace y2023 {
-namespace actors {
+namespace autonomous {
class AutonomousSplines {
public:
AutonomousSplines()
- : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
- "splines/test_spline.json")) {}
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")),
+ spline_1_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_1.json")),
+ spline_2_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_2.json")),
+ spline_3_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_3.json")),
+ spline_4_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_4.json")),
+ spline_5_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/spline_5.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
@@ -33,11 +45,37 @@
aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
*builder,
aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> Spline1(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> Spline2(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> Spline3(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> Spline4(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ flatbuffers::Offset<frc971::MultiSpline> Spline5(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
private:
aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_1_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_2_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_3_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_4_;
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> spline_5_;
};
-} // namespace actors
+} // namespace autonomous
} // namespace y2023
-#endif // Y2023_ACTORS_AUTO_SPLINES_H_
+#endif // Y2023_AUTONOMOUS_AUTO_SPLINES_H_
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index 8e99af6..77965e4 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -5,13 +5,18 @@
#include <cmath>
#include "aos/logging/logging.h"
+#include "aos/util/math.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2023/autonomous/auto_splines.h"
+#include "y2023/constants.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/control_loops/superstructure/arm/generated_graph.h"
DEFINE_bool(spline_auto, true, "Run simple test S-spline auto mode.");
+DEFINE_bool(charged_up, true, "If true run charged up autonomous mode");
namespace y2023 {
-namespace actors {
+namespace autonomous {
using ::aos::monotonic_clock;
using ::frc971::ProfileParametersT;
@@ -28,7 +33,9 @@
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
- auto_splines_() {
+ auto_splines_(),
+ arm_goal_position_(control_loops::superstructure::arm::NeutralIndex()),
+ points_(control_loops::superstructure::arm::PointList()) {
replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
event_loop->OnRun([this, event_loop]() {
@@ -51,8 +58,9 @@
(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.
+ // 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_->Setup(now + std::chrono::seconds(2));
}
if (joystick_state_fetcher_->enabled()) {
@@ -77,6 +85,27 @@
SplineDirection::kForward);
starting_position_ = test_spline_->starting_position();
+ } else if (FLAGS_charged_up) {
+ charged_up_splines_ = {
+ PlanSpline(std::bind(&AutonomousSplines::Spline1, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::Spline2, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::Spline3, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ PlanSpline(std::bind(&AutonomousSplines::Spline4, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward),
+ PlanSpline(std::bind(&AutonomousSplines::Spline5, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kBackward),
+ };
+
+ starting_position_ = charged_up_splines_.value()[0].starting_position();
+ CHECK(starting_position_);
}
is_planned_ = true;
@@ -113,8 +142,8 @@
CHECK(starting_position_);
SendStartingPosition(starting_position_.value());
}
- // Clear this so that we don't accidentally resend things as soon as we replan
- // later.
+ // 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();
@@ -160,5 +189,204 @@
}
}
-} // namespace actors
+// Charged Up 3 Game Object Autonomous.
+void AutonomousActor::ChargedUp() {
+ aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
+
+ CHECK(charged_up_splines_);
+
+ auto &splines = *charged_up_splines_;
+
+ // Tell the superstructure a cone was preloaded
+ if (!WaitForPreloaded()) return;
+
+ // Place first cone on mid level
+ MidConeScore();
+
+ // Wait until the arm is at the goal to spit
+ if (!WaitForArmGoal()) return;
+ Spit();
+
+ AOS_LOG(
+ INFO, "Placed first cone %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube nearest to the starting zone
+ if (!splines[0].WaitForPlan()) return;
+ splines[0].Start();
+
+ // Move arm into position to pickup a cube and start cube intake
+ PickupCube();
+ IntakeCube();
+
+ if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+
+ // Drive back to grid and place cube on high level
+ if (!splines[1].WaitForPlan()) return;
+ splines[1].Start();
+
+ HighCubeScore();
+
+ if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+
+ if (!WaitForArmGoal()) return;
+ Spit();
+
+ AOS_LOG(
+ INFO, "Placed first cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive and intake the cube second nearest to the starting zone
+ if (!splines[2].WaitForPlan()) return;
+ splines[2].Start();
+
+ PickupCube();
+ IntakeCube();
+
+ if (!splines[2].WaitForSplineDistanceRemaining(0.02)) return;
+
+ // Drive back to grid and place object on mid level
+ if (!splines[3].WaitForPlan()) return;
+ splines[3].Start();
+
+ MidCubeScore();
+
+ if (!splines[3].WaitForSplineDistanceRemaining(0.02)) return;
+
+ if (!WaitForArmGoal()) return;
+ Spit();
+
+ AOS_LOG(
+ INFO, "Placed second cube %lf s\n",
+ aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
+
+ // Drive onto charging station
+ if (!splines[4].WaitForPlan()) return;
+ splines[4].Start();
+
+ if (!splines[4].WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendSuperstructureGoal() {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ control_loops::superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<control_loops::superstructure::Goal>();
+
+ superstructure_builder.add_arm_goal_position(arm_goal_position_);
+ superstructure_builder.add_preloaded_with_cone(preloaded_);
+ superstructure_builder.add_roller_goal(roller_goal_);
+ superstructure_builder.add_wrist(wrist_goal_);
+
+ if (builder.Send(superstructure_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
+ set_preloaded(true);
+ SendSuperstructureGoal();
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ ActorBase::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_->end_effector_state() ==
+ control_loops::superstructure::EndEffectorState::LOADED);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
+void AutonomousActor::MidConeScore() {
+ set_arm_goal_position(
+ control_loops::superstructure::arm::ScoreFrontMidConeUpIndex());
+ set_wrist_goal(0.05);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::HighCubeScore() {
+ set_arm_goal_position(
+ control_loops::superstructure::arm::ScoreFrontHighCubeIndex());
+ set_wrist_goal(0.6);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::MidCubeScore() {
+ set_arm_goal_position(
+ control_loops::superstructure::arm::ScoreFrontMidCubeIndex());
+ set_wrist_goal(0.6);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::PickupCube() {
+ set_arm_goal_position(
+ control_loops::superstructure::arm::GroundPickupBackCubeIndex());
+ set_wrist_goal(0.6);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::Spit() {
+ set_roller_goal(control_loops::superstructure::RollerGoal::SPIT);
+ SendSuperstructureGoal();
+}
+
+void AutonomousActor::IntakeCube() {
+ set_roller_goal(control_loops::superstructure::RollerGoal::INTAKE_CUBE);
+ SendSuperstructureGoal();
+}
+
+[[nodiscard]] bool AutonomousActor::WaitForArmGoal() {
+ constexpr double kEpsTheta = 0.01;
+
+ ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
+ event_loop()->monotonic_now(),
+ ActorBase::kLoopOffset);
+
+ bool at_goal = false;
+ while (!at_goal) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ phased_loop.SleepUntilNext();
+ superstructure_status_fetcher_.Fetch();
+ CHECK(superstructure_status_fetcher_.get() != nullptr);
+
+ // Check that the status had the right goal
+ at_goal = (std::abs(points_[arm_goal_position_](0) -
+ superstructure_status_fetcher_->arm()->theta0()) <
+ kEpsTheta &&
+ std::abs(points_[arm_goal_position_](1) -
+ superstructure_status_fetcher_->arm()->theta1()) <
+ kEpsTheta &&
+ std::abs(points_[arm_goal_position_](2) -
+ superstructure_status_fetcher_->arm()->theta2()) <
+ kEpsTheta) &&
+ (std::abs(wrist_goal_ -
+ superstructure_status_fetcher_->wrist()->position()) <
+ kEpsTheta);
+ }
+
+ set_preloaded(false);
+ SendSuperstructureGoal();
+
+ return true;
+}
+
+} // namespace autonomous
} // namespace y2023
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index cf0b458..d13003d 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -1,5 +1,5 @@
-#ifndef Y2023_ACTORS_AUTONOMOUS_ACTOR_H_
-#define Y2023_ACTORS_AUTONOMOUS_ACTOR_H_
+#ifndef Y2023_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+#define Y2023_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
@@ -8,9 +8,11 @@
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2023/autonomous/auto_splines.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
namespace y2023 {
-namespace actors {
+namespace autonomous {
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
@@ -20,11 +22,39 @@
const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
+ void set_arm_goal_position(uint32_t requested_arm_goal_position) {
+ arm_goal_position_ = requested_arm_goal_position;
+ }
+
+ void set_roller_goal(
+ control_loops::superstructure::RollerGoal requested_roller_goal) {
+ roller_goal_ = requested_roller_goal;
+ }
+
+ void set_wrist_goal(double requested_wrist_goal) {
+ wrist_goal_ = requested_wrist_goal;
+ }
+
+ void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
+
+ void SendSuperstructureGoal();
+ void HighCubeScore();
+ void MidCubeScore();
+ void MidConeScore();
+ void PickupCube();
+ void Spit();
+ void IntakeCube();
+
+ [[nodiscard]] bool WaitForArmGoal();
+
+ [[nodiscard]] bool WaitForPreloaded();
+
void Reset();
void SendStartingPosition(const Eigen::Vector3d &start);
void MaybeSendStartingPosition();
void SplineAuto();
+ void ChargedUp();
void Replan();
aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
@@ -32,10 +62,13 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+ double wrist_goal_ = 0.0;
+ control_loops::superstructure::RollerGoal roller_goal_ =
+ control_loops::superstructure::RollerGoal::IDLE;
+
aos::TimerHandler *replan_timer_;
aos::TimerHandler *button_poll_;
- std::optional<SplineHandle> test_spline_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
bool user_indicated_safe_to_reset_ = false;
@@ -44,9 +77,22 @@
bool is_planned_ = false;
std::optional<Eigen::Vector3d> starting_position_;
+
+ uint32_t arm_goal_position_;
+ bool preloaded_ = false;
+
+ aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
+ aos::Fetcher<y2023::control_loops::superstructure::Status>
+ superstructure_status_fetcher_;
+
+ std::optional<SplineHandle> test_spline_;
+ std::optional<std::array<SplineHandle, 5>> charged_up_splines_;
+
+ // List of arm angles from arm::PointsList
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
};
-} // namespace actors
+} // namespace autonomous
} // namespace y2023
-#endif // Y2023_ACTORS_AUTONOMOUS_ACTOR_H_
+#endif // Y2023_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
diff --git a/y2023/autonomous/autonomous_actor_main.cc b/y2023/autonomous/autonomous_actor_main.cc
index 1ee3c15..08a8960 100644
--- a/y2023/autonomous/autonomous_actor_main.cc
+++ b/y2023/autonomous/autonomous_actor_main.cc
@@ -11,7 +11,7 @@
aos::configuration::ReadConfig("aos_config.json");
::aos::ShmEventLoop event_loop(&config.message());
- ::y2023::actors::AutonomousActor autonomous(&event_loop);
+ ::y2023::autonomous::AutonomousActor autonomous(&event_loop);
event_loop.Run();
diff --git a/y2023/constants.cc b/y2023/constants.cc
index eed17c4..4bd2096 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -105,7 +105,7 @@
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 5.78862525947414;
+ 0.894159203288852;
break;
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index e8fdb3c..4efba61 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -75,7 +75,8 @@
10.0 / ::std::pow(12.0, 2))
.finished()
.asDiagonal()),
- .max_controllable_offset = 0.5}};
+ .max_controllable_offset = 0.5},
+ };
return kDrivetrainConfig;
};
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 12a8967..e8589ef 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -4,6 +4,18 @@
from y2023.control_loops.python.graph_tools import *
+
+def ThetaSegment(name, start, end):
+ control = np.array([(start[0] + end[0]) / 2.0, (start[1] + end[1]) / 2.0])
+ return ThetaSplineSegment(
+ name=name,
+ start=start,
+ control1=control,
+ control2=control,
+ end=end,
+ )
+
+
named_segments = []
points = {}
@@ -37,7 +49,7 @@
points[
'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
- 0.263207, 0.24, -np.pi / 2.0, circular_index=0)
+ 0.30, 0.24, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -75,6 +87,136 @@
control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
))
+points['ScoreBackLowConeDownTip'] = to_theta_with_circular_index_and_roll(
+ -1.17422, 0.441203, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackLowConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.0959727041167358, -0.48933188185224896]),
+ control2=np.array([3.11854219540683, -1.0398000886366843]),
+ end=points['ScoreBackLowConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontLowConeDownTip'] = to_theta_with_circular_index_and_roll(
+ 0.327783, 0.430704, np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontLowConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.6217558044411176, 0.6335548380532725]),
+ control2=np.array([4.2557660430407935, 1.0411926555706872]),
+ end=points['ScoreFrontLowConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreBackMidConeDownTip'] = to_theta_with_circular_index_and_roll(
+ -1.49, 0.818521, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackMidConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([3.193704394908777, -0.46076706416611657]),
+ control2=np.array([3.6421839688861786, -0.8129214904599373]),
+ end=points['ScoreBackMidConeDownTip'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points[
+ 'ScoreBackMidConeDownTipPlaced'] = to_theta_with_circular_index_and_roll(
+ -1.43, 0.65, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreBackMidConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([3.193704394908777, -0.46076706416611657]),
+ control2=np.array([3.6421839688861786, -0.8129214904599373]),
+ end=points['ScoreBackMidConeDownTipPlaced'],
+ control_alpha_rolls=[(0.20, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreBackMidConeDownTipToScoreBackMidConeDownTipPlaced",
+ start=points['ScoreBackMidConeDownTip'],
+ end=points['ScoreBackMidConeDownTipPlaced'],
+ ))
+
+points['ScoreFrontMidConeDownTip'] = np.array(
+ (6.37001629521978, 2.04450540030891, np.pi / 2.0))
+#to_theta_with_circular_index_and_roll(
+#0.708449, 0.869738, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontMidConeDownTip'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontMidConeDownTipPlaced'] = np.array(
+ (6.42001629521978, 2.30450540030891, np.pi / 2.0))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontMidConeDownTipPlaced'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreFrontMidConeDownTipToScoreFrontMidConeDownTipPlaced",
+ start=points['ScoreFrontMidConeDownTip'],
+ end=points['ScoreFrontMidConeDownTipPlaced'],
+ ))
+
+points['ScoreFrontHighConeDownTip'] = np.array(
+ (7.07190783461154, 1.55094570328448, np.pi / 2.0))
+#to_theta_with_circular_index_and_roll(
+#0.708449, 0.869738, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighConeDownTip",
+ start=points['Neutral'],
+ control1=np.array([4.579377666056791, 0.3789471836198275]),
+ control2=np.array([5.140992799899862, 1.5135884307866865]),
+ end=points['ScoreFrontHighConeDownTip'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreFrontHighConeDownTipPlaced'] = np.array(
+ (6.93190783461154, 1.80094570328448, np.pi / 2.0))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighConeDownTipPlaced",
+ start=points['Neutral'],
+ control1=np.array([5.997741842590495, 1.8354263885166913]),
+ control2=np.array([6.141018843972322, 1.0777341552037734]),
+ end=points['ScoreFrontHighConeDownTipPlaced'],
+ control_alpha_rolls=[(0.50, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSegment(
+ name="ScoreFrontHighConeDownTipToScoreFrontHighConeDownTipPlaced",
+ start=points['ScoreFrontHighConeDownTip'],
+ end=points['ScoreFrontHighConeDownTipPlaced'],
+ ))
+
points['ScoreFrontHighConeDownBase'] = to_theta_with_circular_index_and_roll(
1.04686, 1.13243, -np.pi / 2.0, circular_index=0)
@@ -89,7 +231,7 @@
))
points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.30, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.28, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -115,7 +257,7 @@
))
points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
- -1.33013, 1.08354, np.pi / 2.0, circular_index=1)
+ -1.45013, 1.04354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -161,7 +303,7 @@
))
points['ScoreBackMidConeDownBase'] = to_theta_with_circular_index_and_roll(
- -1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)
+ -1.37792406, 0.87332449, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -187,7 +329,7 @@
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1050539, 1.34, np.pi / 2.0, circular_index=0)
+ -1.1050539, 1.325, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -228,7 +370,7 @@
))
points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
- 0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)
+ 0.64, 1.03, -np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -241,7 +383,7 @@
))
points['ScoreFrontLowCube'] = to_theta_with_circular_index_and_roll(
- 0.325603, 0.30, np.pi / 2.0, circular_index=0)
+ 0.325603, 0.39, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/control_loops/python/graph_tools.py b/y2023/control_loops/python/graph_tools.py
index ac926aa..b4d047a 100644
--- a/y2023/control_loops/python/graph_tools.py
+++ b/y2023/control_loops/python/graph_tools.py
@@ -183,7 +183,7 @@
LOWER_DELTA_LIMIT = -1.98 * np.pi
# TODO(milind): put actual proximal limits
-UPPER_PROXIMAL_LIMIT = np.pi * 2.0
+UPPER_PROXIMAL_LIMIT = np.pi * 3.0
LOWER_PROXIMAL_LIMIT = -np.pi * 2.0
UPPER_DISTAL_LIMIT = 0.75 * np.pi
@@ -193,12 +193,36 @@
LOWER_ROLL_JOINT_LIMIT = -0.75 * np.pi
-def arm_past_limit(theta1, theta2, theta3):
+def arm_past_limit(theta1, theta2, theta3, verbose=True):
delta = theta2 - theta1
- return delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT or \
- theta1 > UPPER_PROXIMAL_LIMIT or theta1 < LOWER_PROXIMAL_LIMIT or \
- theta2 > UPPER_DISTAL_LIMIT or theta2 < LOWER_DISTAL_LIMIT or \
- theta3 > UPPER_ROLL_JOINT_LIMIT or theta3 < LOWER_ROLL_JOINT_LIMIT
+ if delta > UPPER_DELTA_LIMIT or delta < LOWER_DELTA_LIMIT:
+ if verbose:
+ print(
+ f'Delta {delta} outside {LOWER_DELTA_LIMIT}, {UPPER_DELTA_LIMIT}'
+ )
+ return True
+ if theta1 > UPPER_PROXIMAL_LIMIT or theta1 < LOWER_PROXIMAL_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta1} outside {LOWER_PROXIMAL_LIMIT}, {UPPER_PROXIMAL_LIMIT}'
+ )
+ return True
+
+ if theta2 > UPPER_DISTAL_LIMIT or theta2 < LOWER_DISTAL_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta2} outside {LOWER_DISTAL_LIMIT}, {UPPER_DISTAL_LIMIT}'
+ )
+ return True
+
+ if theta3 > UPPER_ROLL_JOINT_LIMIT or theta3 < LOWER_ROLL_JOINT_LIMIT:
+ if verbose:
+ print(
+ f'Proximal {theta3} outside {LOWER_ROLL_JOINT_LIMIT}, {UPPER_ROLL_JOINT_LIMIT}'
+ )
+ return True
+
+ return False
def get_circular_index(theta):
@@ -376,7 +400,7 @@
def arm_past_limit(self, points, verbose=True):
for point in points:
- if arm_past_limit(*point):
+ if arm_past_limit(*point, verbose=verbose):
if verbose:
print("Arm past limit for path %s in point %s" %
(self.name, point))
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index ad962ad..e5bc830 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -156,6 +156,35 @@
],
)
+cc_library(
+ name = "led_indicator_lib",
+ srcs = ["led_indicator.cc"],
+ hdrs = ["led_indicator.h"],
+ data = [
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//third_party:phoenix",
+ "//third_party:wpilib",
+ "//y2023/vision:game_pieces_fbs",
+ ],
+)
+
cc_binary(
name = "superstructure_replay",
srcs = ["superstructure_replay.cc"],
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 4d5d43e..8ab2f27 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -20,12 +20,26 @@
void EndEffector::RunIteration(
const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
double falcon_current, double cone_position, bool beambreak,
- double *roller_voltage) {
+ double *roller_voltage, bool preloaded_with_cone) {
*roller_voltage = 0.0;
constexpr double kMinCurrent = 40.0;
constexpr double kMaxConePosition = 0.92;
+ // If we started off preloaded, skip to the loaded state.
+ // Make sure we weren't already there just in case.
+ if (preloaded_with_cone) {
+ switch (state_) {
+ case EndEffectorState::IDLE:
+ case EndEffectorState::INTAKING:
+ state_ = EndEffectorState::LOADED;
+ break;
+ case EndEffectorState::LOADED:
+ case EndEffectorState::SPITTING:
+ break;
+ }
+ }
+
// Let them switch game pieces
if (roller_goal == RollerGoal::INTAKE_CONE_UP) {
game_piece_ = vision::Class::CONE_UP;
@@ -90,8 +104,8 @@
break;
case EndEffectorState::LOADED:
timer_ = timestamp;
- // If loaded and beam break not triggered, intake
- if (!beambreak_status) {
+ // If loaded and beam break not triggered and not preloaded, intake
+ if (!beambreak_status && !preloaded_with_cone) {
state_ = EndEffectorState::INTAKING;
}
break;
diff --git a/y2023/control_loops/superstructure/end_effector.h b/y2023/control_loops/superstructure/end_effector.h
index 5ae96da..da0ce5e 100644
--- a/y2023/control_loops/superstructure/end_effector.h
+++ b/y2023/control_loops/superstructure/end_effector.h
@@ -18,14 +18,14 @@
static constexpr double kRollerConeSuckVoltage() { return 12.0; }
static constexpr double kRollerConeSpitVoltage() { return -9.0; }
- static constexpr double kRollerCubeSuckVoltage() { return -5.0; }
+ static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
EndEffector();
void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
RollerGoal roller_goal, double falcon_current,
double cone_position, bool beambreak,
- double *intake_roller_voltage);
+ double *intake_roller_voltage, bool preloaded_with_cone);
EndEffectorState state() const { return state_; }
vision::Class game_piece() const { return game_piece_; }
void Reset();
diff --git a/y2023/control_loops/superstructure/led_indicator.cc b/y2023/control_loops/superstructure/led_indicator.cc
new file mode 100644
index 0000000..68e7f14
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.cc
@@ -0,0 +1,183 @@
+#include "y2023/control_loops/superstructure/led_indicator.h"
+
+namespace led = ctre::phoenix::led;
+
+namespace y2023::control_loops::superstructure {
+
+LedIndicator::LedIndicator(aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ drivetrain_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_position_fetcher_(
+ event_loop_->MakeFetcher<Position>("/superstructure")),
+ server_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/roborio/aos")),
+ client_statistics_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ClientStatistics>(
+ "/roborio/aos")),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ gyro_reading_fetcher_(
+ event_loop_->MakeFetcher<frc971::sensors::GyroReading>(
+ "/drivetrain")),
+ drivetrain_status_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {
+ led::CANdleConfiguration config;
+ config.statusLedOffWhenActive = true;
+ config.disableWhenLOS = false;
+ config.brightnessScalar = 1.0;
+ candle_.ConfigAllSettings(config, 0);
+
+ event_loop_->AddPhasedLoop([&](int) { DecideColor(); },
+ std::chrono::milliseconds(20));
+}
+
+// This method will be called once per scheduler run
+void LedIndicator::DisplayLed(uint8_t r, uint8_t g, uint8_t b) {
+ candle_.SetLEDs(static_cast<int>(r), static_cast<int>(g),
+ static_cast<int>(b));
+}
+
+namespace {
+bool DisconnectedPiServer(
+ const aos::message_bridge::ServerStatistics &server_stats) {
+ for (const auto *pi_server_status : *server_stats.connections()) {
+ if (pi_server_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_server_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool DisconnectedPiClient(
+ const aos::message_bridge::ClientStatistics &client_stats) {
+ for (const auto *pi_client_status : *client_stats.connections()) {
+ if (pi_client_status->state() == aos::message_bridge::State::DISCONNECTED &&
+ pi_client_status->node()->name()->string_view() != "logger") {
+ return true;
+ }
+ }
+ return false;
+}
+} // namespace
+
+void LedIndicator::DecideColor() {
+ superstructure_status_fetcher_.Fetch();
+ superstructure_position_fetcher_.Fetch();
+ server_statistics_fetcher_.Fetch();
+ drivetrain_output_fetcher_.Fetch();
+ client_statistics_fetcher_.Fetch();
+ gyro_reading_fetcher_.Fetch();
+ localizer_output_fetcher_.Fetch();
+
+ if (localizer_output_fetcher_.get()) {
+ if (localizer_output_fetcher_->image_accepted_count() !=
+ last_accepted_count_) {
+ last_accepted_count_ = localizer_output_fetcher_->image_accepted_count();
+ last_accepted_time_ = event_loop_->monotonic_now();
+ }
+ }
+
+ // Estopped
+ if (superstructure_status_fetcher_.get() &&
+ superstructure_status_fetcher_->estopped()) {
+ DisplayLed(255, 0, 0);
+ return;
+ }
+
+ // Not zeroed
+ if (superstructure_status_fetcher_.get() &&
+ !superstructure_status_fetcher_->zeroed()) {
+ DisplayLed(255, 0, 255);
+ return;
+ }
+
+ // If the imu gyro readings are not being sent/updated recently
+ if (!gyro_reading_fetcher_.get() ||
+ gyro_reading_fetcher_.context().monotonic_event_time <
+ event_loop_->monotonic_now() -
+ frc971::controls::kLoopFrequency * 10) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(255, 255, 255);
+ }
+ return;
+ }
+
+ // Pi disconnected
+ if ((server_statistics_fetcher_.get() &&
+ DisconnectedPiServer(*server_statistics_fetcher_)) ||
+ (client_statistics_fetcher_.get() &&
+ DisconnectedPiClient(*client_statistics_fetcher_))) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 0, 0);
+ } else {
+ DisplayLed(0, 255, 0);
+ }
+
+ return;
+ }
+
+ if (superstructure_status_fetcher_.get()) {
+ // Check if end effector is intaking.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::INTAKING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(255, 165, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+ // Check if end effector is spitting.
+ if (superstructure_status_fetcher_->end_effector_state() ==
+ EndEffectorState::SPITTING) {
+ if (flash_counter_.Flash()) {
+ DisplayLed(0, 255, 0);
+ } else {
+ DisplayLed(0, 0, 0);
+ }
+
+ return;
+ }
+
+ // Check the if there is a cone in the end effector.
+ if (superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_UP ||
+ superstructure_status_fetcher_->game_piece() ==
+ vision::Class::CONE_DOWN) {
+ DisplayLed(255, 255, 0);
+ return;
+ }
+ // Check if the cube beam break is triggered.
+ if (superstructure_status_fetcher_->game_piece() == vision::Class::CUBE) {
+ DisplayLed(138, 43, 226);
+ return;
+ }
+
+ // Check if there is a target that is in sight
+ if (drivetrain_status_fetcher_->line_follow_logging()->have_target()) {
+ DisplayLed(255, 165, 0);
+ return;
+ }
+
+ if (event_loop_->monotonic_now() <
+ last_accepted_time_ + std::chrono::milliseconds(500)) {
+ DisplayLed(0, 0, 255);
+ return;
+ }
+
+ return;
+ }
+}
+
+} // namespace y2023::control_loops::superstructure
diff --git a/y2023/control_loops/superstructure/led_indicator.h b/y2023/control_loops/superstructure/led_indicator.h
new file mode 100644
index 0000000..d88650d
--- /dev/null
+++ b/y2023/control_loops/superstructure/led_indicator.h
@@ -0,0 +1,97 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/network/message_bridge_client_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "ctre/phoenix/led/CANdle.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2023/vision/game_pieces_generated.h"
+
+namespace y2023::control_loops::superstructure {
+
+class FlashCounter {
+ public:
+ FlashCounter(size_t flash_iterations) : flash_iterations_(flash_iterations) {}
+
+ bool Flash() {
+ if (counter_ % flash_iterations_ == 0) {
+ flash_ = !flash_;
+ }
+ counter_++;
+ return flash_;
+ }
+
+ private:
+ size_t flash_iterations_;
+ size_t counter_ = 0;
+ bool flash_ = false;
+};
+
+class LedIndicator {
+ public:
+ LedIndicator(aos::EventLoop *event_loop);
+
+ // Colors in order of priority:
+ //
+ // Red: estopped
+ // Pink: not zeroed
+ // Flash red/white: imu disconnected
+ // Flash red/green: pi disconnected
+ //
+ // Statemachine:
+ // END EFFECTOR INTAKING:
+ // Flash orange/off
+ // END EFFECTOR SPITTING:
+ // Flash green/off
+ // CONE LOADED:
+ // Yellow
+ // CUBE LOADED:
+ // Purple
+ // HAS A TARGET
+ // Gold
+ // VISION:
+ // Blue
+
+ void DecideColor();
+
+ private:
+ static constexpr size_t kFlashIterations = 5;
+
+ void DisplayLed(uint8_t r, uint8_t g, uint8_t b);
+
+ ctre::phoenix::led::CANdle candle_{0, ""};
+
+ aos::EventLoop *event_loop_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output>
+ drivetrain_output_fetcher_;
+ aos::Fetcher<Status> superstructure_status_fetcher_;
+ aos::Fetcher<Position> superstructure_position_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics>
+ server_statistics_fetcher_;
+ aos::Fetcher<aos::message_bridge::ClientStatistics>
+ client_statistics_fetcher_;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<frc971::sensors::GyroReading> gyro_reading_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+
+ size_t last_accepted_count_ = 0;
+ aos::monotonic_clock::time_point last_accepted_time_ =
+ aos::monotonic_clock::min_time;
+
+ FlashCounter flash_counter_{kFlashIterations};
+};
+
+} // namespace y2023::control_loops::superstructure
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_LED_INDICATOR_H_
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 0f19a1e..ece0790 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -88,7 +88,8 @@
? position->roller_falcon()->torque_current()
: 0.0,
position->cone_position(), position->end_effector_cube_beam_break(),
- &output_struct.roller_voltage);
+ &output_struct.roller_voltage,
+ unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cone() : false);
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
index 670351a..7ac7000 100644
--- a/y2023/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -22,6 +22,9 @@
wrist:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
roller_goal:RollerGoal (id: 3);
+
+ // If true, we started with the cone loaded and should proceed to that state.
+ preloaded_with_cone:bool (id: 4);
}
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index c1c20a3..80e41f9 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -446,6 +446,26 @@
const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
};
+// Test that we are able to signal that the ball was preloaded
+TEST_F(SuperstructureTest, Preloaded) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_preloaded_with_cone(true);
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(dt());
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
+ EndEffectorState::LOADED);
+}
+
// Tests that the superstructure does nothing when the goal is to remain
// still.
TEST_F(SuperstructureTest, DoesNothing) {
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 3951dfa..ce3cbbe 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -42,6 +42,9 @@
namespace input {
namespace joysticks {
+constexpr double kConeWrist = 0.4;
+constexpr double kCubeWrist = 1.0;
+
// TODO(milind): add correct locations
const ButtonLocation kDriverSpit(2, 1);
const ButtonLocation kSpit(4, 13);
@@ -60,7 +63,7 @@
const ButtonLocation kLowCube(4, 3);
const ButtonLocation kGroundPickupConeUp(4, 7);
-const ButtonLocation kGroundPickupConeDownBase(4, 8);
+const ButtonLocation kGroundPickupConeDown(4, 8);
const ButtonLocation kGroundPickupCube(4, 10);
const ButtonLocation kHPConePickup(4, 6);
@@ -70,6 +73,9 @@
const ButtonLocation kWrist(4, 10);
const ButtonLocation kStayIn(3, 4);
+const ButtonLocation kConeDownTip(4, 4);
+const ButtonLocation kConeDownBase(4, 5);
+
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
@@ -77,6 +83,7 @@
CONE_UP = 0,
CONE_DOWN = 1,
CUBE = 2,
+ CONE_TIP = 4,
};
struct ButtonData {
@@ -86,6 +93,7 @@
struct ArmSetpoint {
uint32_t index;
+ std::optional<uint32_t> place_index = std::nullopt;
double wrist_goal;
std::optional<double> score_wrist_goal = std::nullopt;
GamePiece game_piece;
@@ -97,35 +105,88 @@
const std::vector<ArmSetpoint> setpoints = {
{
.index = arm::GroundPickupBackConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = 0.7,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kGroundPickupConeUp}},
.side = Side::BACK,
},
{
.index = arm::GroundPickupFrontConeUpIndex(),
- .wrist_goal = 0.2,
+ .wrist_goal = 0.6,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kGroundPickupConeUp}},
.side = Side::FRONT,
},
{
.index = arm::GroundPickupBackConeDownBaseIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {{kGroundPickupConeDownBase}},
+ .buttons = {{kGroundPickupConeDown}},
.side = Side::BACK,
},
{
.index = arm::GroundPickupFrontConeDownBaseIndex(),
- .wrist_goal = 0.2,
+ .wrist_goal = 0.6,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {{kGroundPickupConeDownBase}},
+ .buttons = {{kGroundPickupConeDown}},
.side = Side::FRONT,
},
{
- .index = arm::ScoreBackMidConeUpIndex(),
+ .index = arm::ScoreBackLowConeDownTipIndex(),
+ .wrist_goal = 0.7,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kLowConeScoreRight, SpotSelectionHint::RIGHT},
+ {kLowCube, SpotSelectionHint::MIDDLE},
+ {kLowConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::BOTTOM,
+ },
+ {
+ .index = arm::ScoreBackMidConeDownTipIndex(),
+ .place_index = arm::ScoreBackMidConeDownTipPlacedIndex(),
+ .wrist_goal = 0.8,
+ .score_wrist_goal = 2.0,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
+ {kMidConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::MIDDLE,
+ },
+ {
+ .index = arm::ScoreFrontMidConeDownTipIndex(),
+ .place_index = arm::ScoreFrontMidConeDownTipPlacedIndex(),
.wrist_goal = 0.0,
+ .score_wrist_goal = 1.4,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
+ {kMidConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::MIDDLE,
+ },
+ {
+ .index = arm::ScoreFrontHighConeDownTipIndex(),
+ .place_index = arm::ScoreFrontHighConeDownTipPlacedIndex(),
+ .wrist_goal = 0.4,
+ .score_wrist_goal = 1.4,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kHighConeScoreRight, SpotSelectionHint::RIGHT},
+ {kHighConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::TOP,
+ },
+ {
+ .index = arm::ScoreFrontLowConeDownTipIndex(),
+ .wrist_goal = 2.8,
+ .game_piece = GamePiece::CONE_TIP,
+ .buttons = {{kLowConeScoreRight, SpotSelectionHint::RIGHT},
+ {kLowCube, SpotSelectionHint::MIDDLE},
+ {kLowConeScoreLeft, SpotSelectionHint::LEFT}},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::TOP,
+ },
+ {
+ .index = arm::ScoreBackMidConeUpIndex(),
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kMidConeScoreRight, SpotSelectionHint::RIGHT},
{kMidConeScoreLeft, SpotSelectionHint::LEFT}},
@@ -134,7 +195,7 @@
},
{
.index = arm::ScoreBackLowConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -143,7 +204,7 @@
},
{
.index = arm::ScoreFrontLowConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -152,8 +213,8 @@
},
{
.index = arm::ScoreBackMidConeDownBaseIndex(),
- .wrist_goal = 2.2,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.5,
+ .score_wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -162,8 +223,7 @@
},
{
.index = arm::ScoreBackLowConeDownBaseIndex(),
- .wrist_goal = 0.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -172,7 +232,7 @@
},
{
.index = arm::ScoreFrontLowConeDownBaseIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kLowConeScoreLeft, SpotSelectionHint::LEFT},
{kLowConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -181,8 +241,8 @@
},
{
.index = arm::ScoreFrontMidConeDownBaseIndex(),
- .wrist_goal = 2.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.6,
+ .score_wrist_goal = 0.2,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -191,8 +251,8 @@
},
{
.index = arm::ScoreFrontHighConeDownBaseIndex(),
- .wrist_goal = 2.0,
- .score_wrist_goal = 0.0,
+ .wrist_goal = 2.6,
+ .score_wrist_goal = 0.2,
.game_piece = GamePiece::CONE_DOWN,
.buttons = {{kHighConeScoreLeft, SpotSelectionHint::LEFT},
{kHighConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -201,21 +261,21 @@
},
{
.index = arm::HPPickupFrontConeUpIndex(),
- .wrist_goal = 0.0,
+ .wrist_goal = kConeWrist,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHPConePickup}},
.side = Side::FRONT,
},
{
.index = arm::HPPickupBackConeUpIndex(),
- .wrist_goal = 0.4,
+ .wrist_goal = 0.5,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHPConePickup}},
.side = Side::BACK,
},
{
.index = arm::ScoreFrontHighConeUpIndex(),
- .wrist_goal = 0.05,
+ .wrist_goal = kConeWrist + 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kHighConeScoreLeft, SpotSelectionHint::LEFT},
{kHighConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -224,7 +284,7 @@
},
{
.index = arm::ScoreFrontMidConeUpIndex(),
- .wrist_goal = 0.05,
+ .wrist_goal = kConeWrist + 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {{kMidConeScoreLeft, SpotSelectionHint::LEFT},
{kMidConeScoreRight, SpotSelectionHint::RIGHT}},
@@ -233,14 +293,14 @@
},
{
.index = arm::GroundPickupBackCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kGroundPickupCube}},
.side = Side::BACK,
},
{
.index = arm::ScoreFrontMidCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kMidCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -248,8 +308,7 @@
},
{
.index = arm::ScoreBackMidCubeIndex(),
- .wrist_goal = 0.6,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kMidCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -257,7 +316,7 @@
},
{
.index = arm::ScoreFrontLowCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kLowCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -265,7 +324,7 @@
},
{
.index = arm::ScoreBackLowCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kLowCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -273,7 +332,7 @@
},
{
.index = arm::ScoreFrontHighCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kHighCube, SpotSelectionHint::MIDDLE}},
.side = Side::FRONT,
@@ -281,8 +340,7 @@
},
{
.index = arm::ScoreBackHighCubeIndex(),
- .wrist_goal = 0.6,
- .score_wrist_goal = 0.0,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kHighCube, SpotSelectionHint::MIDDLE}},
.side = Side::BACK,
@@ -290,7 +348,7 @@
},
{
.index = arm::GroundPickupFrontCubeIndex(),
- .wrist_goal = 0.6,
+ .wrist_goal = kCubeWrist,
.game_piece = GamePiece::CUBE,
.buttons = {{kGroundPickupCube}},
.side = Side::FRONT,
@@ -333,11 +391,12 @@
RollerGoal roller_goal = RollerGoal::IDLE;
arm_goal_position_ = arm::NeutralIndex();
std::optional<double> score_wrist_goal = std::nullopt;
+ std::optional<double> place_index = std::nullopt;
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
roller_goal = RollerGoal::INTAKE_CONE_UP;
current_game_piece_ = GamePiece::CONE_UP;
- } else if (data.IsPressed(kGroundPickupConeDownBase)) {
+ } else if (data.IsPressed(kGroundPickupConeDown)) {
roller_goal = RollerGoal::INTAKE_CONE_DOWN;
current_game_piece_ = GamePiece::CONE_DOWN;
} else if (data.IsPressed(kGroundPickupCube)) {
@@ -345,8 +404,17 @@
current_game_piece_ = GamePiece::CUBE;
}
+ if (current_game_piece_ == GamePiece::CONE_DOWN ||
+ current_game_piece_ == GamePiece::CONE_TIP) {
+ if (data.IsPressed(kConeDownTip)) {
+ current_game_piece_ = GamePiece::CONE_TIP;
+ } else if (data.IsPressed(kConeDownBase)) {
+ current_game_piece_ = GamePiece::CONE_DOWN;
+ }
+ }
+
if (current_game_piece_ == GamePiece::CUBE) {
- wrist_goal = 0.6;
+ wrist_goal = kCubeWrist;
}
std::optional<RowSelectionHint> placing_row;
@@ -391,6 +459,7 @@
wrist_goal = current_setpoint_->wrist_goal;
arm_goal_position_ = current_setpoint_->index;
score_wrist_goal = current_setpoint_->score_wrist_goal;
+ place_index = current_setpoint_->place_index;
}
placing_row = current_setpoint_->row_hint;
@@ -407,9 +476,19 @@
// If we are supposed to dunk it, wait until we are close enough to
// spit.
if (std::abs(score_wrist_goal.value() -
- superstructure_status_fetcher_->wrist()->position()) <
+ superstructure_status_fetcher_->wrist()->goal_position()) <
0.1) {
- roller_goal = RollerGoal::SPIT;
+ if (place_index.has_value()) {
+ arm_goal_position_ = place_index.value();
+ if (arm_goal_position_ ==
+ superstructure_status_fetcher_->arm()->current_node() &&
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
+ 0.01) {
+ roller_goal = RollerGoal::SPIT;
+ }
+ } else {
+ roller_goal = RollerGoal::SPIT;
+ }
}
} else {
roller_goal = RollerGoal::SPIT;