Implement Joystick Reader
Reads the joystick input to set superstructure goal.
This patch also removes the HALF_EXTEND climber goal
because it is not used.
Signed-off-by: Alexander Yee <xander.yee@gmail.com>
Change-Id: I5b23ace0b255024cb1413c62c9306bc856f1fd8e
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/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index ab863eb..fece6e3 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -83,10 +83,6 @@
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();
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index b346e97..f9dfcaa 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -14,16 +14,16 @@
// 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,
}
table ShooterGoal {
catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 0);
+
+ // If true we ignore the other provided positions
auto_aim: bool (id: 1);
// Position for the turret when we aren't auto aiming
@@ -32,11 +32,11 @@
// Position for the altitude when we aren't auto aiming
altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 3);
- // If true, we started with the ball loaded and should proceed to that state.
+ // If true, we started with the ball loaded and should proceed to that state.
preloaded:bool = false (id: 4);
}
-// Represents goal for the note movement through the robot
+// 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
@@ -53,10 +53,9 @@
table Goal {
intake_goal:IntakeGoal = NONE (id: 0);
- catapult_goal:frc971.control_loops.catapult.CatapultGoal (id: 1);
- climber_goal:ClimberGoal (id: 2);
- shooter_goal:ShooterGoal (id: 3);
- note_goal:NoteGoal (id: 4);
- fire: bool (id: 5);
+ 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 d1f6641..0e6498e 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -454,13 +454,7 @@
set_point = simulated_robot_constants_->common()
->climber_set_points()
->full_extend();
- } else if (superstructure_goal_fetcher_->climber_goal() ==
- ClimberGoal::HALF_EXTEND) {
- set_point = simulated_robot_constants_->common()
- ->climber_set_points()
- ->half_extend();
}
-
EXPECT_NEAR(set_point,
superstructure_status_fetcher_->climber()->position(), 0.001);
}
@@ -859,22 +853,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);
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();