Added more goals to joystick reader
Change-Id: Ie6d409f7b65f5ba5c7dccaa609edf1f97bdba6a7
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 06b8e40..a9c0f8c 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -14,6 +14,7 @@
#include "aos/util/log_interval.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+#include "y2020/constants.h"
#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
@@ -22,12 +23,24 @@
using aos::input::driver_station::JoystickAxis;
using aos::input::driver_station::POVLocation;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+
namespace y2020 {
namespace input {
namespace joysticks {
namespace superstructure = y2020::control_loops::superstructure;
+// TODO(sabina): fix button locations.
+
+const ButtonLocation kShootFast(4, 1);
+const ButtonLocation kShootSlow(4, 2);
+const ButtonLocation kIntakeExtend(4, 3);
+const ButtonLocation kIntakeIn(4, 4);
+const ButtonLocation kSpit(4, 5);
+
class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
@@ -45,13 +58,72 @@
AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
}
- void HandleTeleop(
- const ::aos::input::driver_station::Data & /*data*/) override {
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get()) {
AOS_LOG(ERROR, "Got no superstructure status message.\n");
return;
}
+
+ double hood_pos = constants::Values::kHoodRange().upper;
+ double intake_pos = constants::Values::kIntakeRange().lower;
+ double turret_pos = 0.0;
+ float roller_speed = 0.0f;
+ double accelerator_speed = 0.0;
+ double finisher_speed = 0.0;
+
+ if (data.IsPressed(kShootFast)) {
+ accelerator_speed = 300.0;
+ finisher_speed = 300.0;
+ }
+
+ else if (data.IsPressed(kShootSlow)) {
+ accelerator_speed = 30.0;
+ finisher_speed = 30.0;
+ }
+
+ if (data.IsPressed(kIntakeExtend)) {
+ intake_pos = constants::Values::kIntakeRange().middle();
+ }
+
+ if (data.IsPressed(kIntakeIn)) {
+ roller_speed = 6.0f;
+ } else if (data.IsPressed(kSpit)) {
+ roller_speed = -6.0f;
+ }
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<superstructure::Goal> superstructure_goal_offset;
+ {
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), hood_pos);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), intake_pos);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), turret_pos);
+
+ flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
+ superstructure::CreateShooterGoal(*builder.fbb(), accelerator_speed, finisher_speed);
+
+ superstructure::Goal::Builder superstructure_goal_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_goal_builder.add_hood(hood_offset);
+ superstructure_goal_builder.add_intake(intake_offset);
+ superstructure_goal_builder.add_turret(turret_offset);
+ superstructure_goal_builder.add_roller_voltage(roller_speed);
+ superstructure_goal_builder.add_shooter(shooter_offset);
+
+ if (!builder.Send(superstructure_goal_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+ }
}
private: