Add superstructure_queue to joystick_reader
Created temporary button mappings and reciprocal actions. Correct
actions and button mappings will need to be filled in.
Change-Id: Ie3f9dd3cc9570379ca5976979fbf636169fe6fbf
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 5e4adf7..9d00ac6 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -13,8 +13,10 @@
#include "aos/linux_code/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
using ::frc971::control_loops::drivetrain_queue;
+using ::y2018::control_loops::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
@@ -26,6 +28,22 @@
namespace input {
namespace joysticks {
+//TODO(Neil): Change button locations to real ones on driverstation.
+const ButtonLocation kIntakeDown(3, 9);
+const POVLocation kIntakeUp(3, 90);
+const ButtonLocation kIntakeIn(3, 12);
+const ButtonLocation kIntakeOut(3, 8);
+
+const ButtonLocation kArmDown(3, 3);
+const ButtonLocation kArmSwitch(3, 7);
+const ButtonLocation kArmScale(3, 6);
+
+const ButtonLocation kClawOpen(3, 5);
+const ButtonLocation kClawClose(3, 4);
+
+const ButtonLocation kForkDeploy(3, 11);
+const ButtonLocation kForkStow(3, 10);
+
std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
class Reader : public ::aos::input::JoystickInput {
@@ -68,6 +86,75 @@
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
}
+
+ superstructure_queue.status.FetchLatest();
+ if (!superstructure_queue.status.get()) {
+ LOG(ERROR, "Got no superstructure status packet.\n");
+ return;
+ }
+
+ if (data.IsPressed(kIntakeUp)) {
+ // Bring in the intake.
+ intake_goal_ = -M_PI; //TODO(Neil): Add real value here once we have one.
+ }
+ if (data.IsPressed(kIntakeDown)) {
+ // Deploy the intake.
+ intake_goal_ = 0; //TODO(Neil): Add real value here once we have one.
+ }
+
+ auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
+
+ new_superstructure_goal->intake.intake_angle = intake_goal_;
+
+ if (data.IsPressed(kIntakeIn)) {
+ // Turn on the rollers.
+ new_superstructure_goal->intake.roller_voltage =
+ 9.0; //TODO(Neil): Add real value once we have one.
+ } else if (data.IsPressed(kIntakeOut)) {
+ // Turn off the rollers.
+ new_superstructure_goal->intake.roller_voltage =
+ -9.0; //TODO(Neil): Add real value once we have one.
+ } else {
+ // We don't want the rollers on.
+ new_superstructure_goal->intake.roller_voltage = 0.0;
+ }
+
+ if (data.IsPressed(kArmDown)) {
+ // Put the arm down to the intake level.
+ new_superstructure_goal->arm.proximal_position =
+ 1.0; // TODO(Neil): Add real value once we have it.
+ new_superstructure_goal->arm.distal_position =
+ 0.0; // TODO(Neil): Add real value once we have it.
+ } else if (data.IsPressed(kArmSwitch)) {
+ // Put the arm up to the level of the switch.
+ new_superstructure_goal->arm.proximal_position =
+ 1.5; // TODO(Neil): Add real value once we have it.
+ new_superstructure_goal->arm.distal_position =
+ 0.5; // TODO(Neil): Add real value once we have it.
+ } else if (data.IsPressed(kArmScale)) {
+ // Put the arm up to the level of the switch.
+ new_superstructure_goal->arm.proximal_position =
+ 1.5; // TODO(Neil): Add real value once we have it.
+ new_superstructure_goal->arm.distal_position =
+ 2.0; // TODO(Neil): Add real value once we have it.
+ }
+
+ if (data.IsPressed(kClawOpen)) {
+ new_superstructure_goal->open_claw = true;
+ } else if (data.IsPressed(kClawClose)) {
+ new_superstructure_goal->open_claw = false;
+ }
+
+ if (data.IsPressed(kForkDeploy)) {
+ new_superstructure_goal->deploy_fork = true;
+ } else if (data.IsPressed(kForkStow)) {
+ new_superstructure_goal->deploy_fork = false;
+ }
+
+ LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ if (!new_superstructure_goal.Send()) {
+ LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
}
private:
@@ -78,6 +165,9 @@
action_queue_.CancelAllActions();
}
+ // Current goals to send to the robot.
+ double intake_goal_ = 0.0;
+
bool was_running_ = false;
bool auto_running_ = false;