Add testing mode for joystick reader.
Setting test mode on the DS puts it in a state where there are
buttons that individually move each subsystem up and down very
slowly. Useful when we are first bringing up the robot, to check
that everything works.
Change-Id: Ia2009d0478d3f6c9c7b44a03e7b7ca5955e9101d
diff --git a/frc971/constants.h b/frc971/constants.h
index 5012817..4e3acfa 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -51,7 +51,6 @@
double drivetrain_done_distance;
double drivetrain_max_speed;
-
// Superstructure Values /////
struct ZeroingConstants {
diff --git a/frc971/frc971.gyp b/frc971/frc971.gyp
index f6006d0..8f7d8a5 100644
--- a/frc971/frc971.gyp
+++ b/frc971/frc971.gyp
@@ -31,7 +31,9 @@
'<(AOS)/common/util/util.gyp:log_interval',
'<(DEPTH)/frc971/queues/queues.gyp:gyro',
+ '<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_queue',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_queue',
+ '<(DEPTH)/frc971/control_loops/fridge/fridge.gyp:fridge_queue',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
'<(AOS)/common/actions/actions.gyp:action_lib',
diff --git a/frc971/joystick_reader.cc b/frc971/joystick_reader.cc
index 8329220..b67c160 100644
--- a/frc971/joystick_reader.cc
+++ b/frc971/joystick_reader.cc
@@ -11,12 +11,16 @@
#include "aos/common/time.h"
#include "aos/common/actions/actions.h"
+#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/constants.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
+using ::frc971::control_loops::claw_queue;
using ::frc971::control_loops::drivetrain_queue;
+using ::frc971::control_loops::fridge_queue;
using ::frc971::sensors::gyro_reading;
using ::aos::input::driver_station::ButtonLocation;
@@ -31,6 +35,20 @@
const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
const ButtonLocation kQuickTurn(1, 5);
+// TODO(danielp): Real buttons for all of these.
+const ButtonLocation kElevatorUp(99, 99);
+const ButtonLocation kElevatorDown(99, 99);
+const ButtonLocation kArmUp(99, 99);
+const ButtonLocation kArmDown(99, 99);
+const ButtonLocation kClawUp(99, 99);
+const ButtonLocation kClawDown(99, 99);
+
+// Testing mode.
+const double kElevatorVelocity = 0.1;
+const double kArmVelocity = 0.2;
+const double kClawVelocity = 0.2;
+// TODO(danielp): Verify.
+const double kJoystickDt = 0.01;
class Reader : public ::aos::input::JoystickInput {
public:
@@ -49,7 +67,12 @@
}
if (!data.GetControlBit(ControlBit::kAutonomous)) {
- HandleTeleop(data);
+ HandleDrivetrain(data);
+ if (data.GetControlBit(ControlBit::kTestMode)) {
+ HandleTest(data);
+ } else {
+ HandleTeleop(data);
+ }
}
}
@@ -68,7 +91,6 @@
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- HandleDrivetrain(data);
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
LOG(DEBUG, "Canceling\n");
@@ -78,6 +100,80 @@
was_running_ = action_queue_.Running();
}
+ void HandleTest(const ::aos::input::driver_station::Data &data) {
+ if (action_queue_.Running()) {
+ // We don't really want any actions running.
+ LOG(DEBUG, "Cancelling actions for test mode.\n");
+ action_queue_.CancelAllActions();
+ }
+
+ if (data.GetControlBit(ControlBit::kEnabled)) {
+ if (data.PosEdge(ControlBit::kEnabled)) {
+ // If we got enabled, wait for everything to zero.
+ LOG(INFO, "Waiting for zero.\n");
+ waiting_for_zero_ = true;
+ }
+ if (waiting_for_zero_) {
+ claw_queue.status.FetchLatest();
+ fridge_queue.status.FetchLatest();
+ if (!claw_queue.status.get()) {
+ LOG(ERROR, "Got no claw status packet.\n");
+ // Not safe to continue.
+ return;
+ }
+ if (!fridge_queue.status.get()) {
+ LOG(ERROR, "Got no fridge status packet.\n");
+ return;
+ }
+
+ if (claw_queue.status->zeroed && fridge_queue.status->zeroed) {
+ LOG(INFO, "Zeroed! Starting test mode.\n");
+ waiting_for_zero_ = false;
+
+ // Set the initial goals to where we are now.
+ elevator_goal_ = fridge_queue.status->height;
+ arm_goal_ = fridge_queue.status->angle;
+ claw_goal_ = claw_queue.status->angle;
+ } else {
+ return;
+ }
+ }
+
+ // These buttons move a subsystem up or down for as long as they are
+ // pressed, at low velocity.
+ if (data.IsPressed(kElevatorUp)) {
+ elevator_goal_ += kElevatorVelocity * kJoystickDt;
+ }
+ if (data.IsPressed(kElevatorDown)) {
+ elevator_goal_ -= kElevatorVelocity * kJoystickDt;
+ }
+ if (data.IsPressed(kArmUp)) {
+ arm_goal_ += kArmVelocity * kJoystickDt;
+ }
+ if (data.IsPressed(kArmDown)) {
+ arm_goal_ -= kArmVelocity * kJoystickDt;
+ }
+ if (data.IsPressed(kClawUp)) {
+ claw_goal_ += kClawVelocity * kJoystickDt;
+ }
+ if (data.IsPressed(kClawDown)) {
+ claw_goal_ -= kClawVelocity * kJoystickDt;
+ }
+
+ if (!fridge_queue.goal.MakeWithBuilder()
+ .height(elevator_goal_)
+ .angle(arm_goal_)
+ .Send()) {
+ LOG(ERROR, "Sending fridge goal failed.\n");
+ }
+ if (!claw_queue.goal.MakeWithBuilder()
+ .angle(claw_goal_)
+ .Send()) {
+ LOG(ERROR, "Sending claw goal failed.\n");
+ }
+ }
+ }
+
private:
void StartAuto() {
LOG(INFO, "Starting auto mode\n");
@@ -90,6 +186,15 @@
}
bool was_running_;
+
+ // Previous goals for systems.
+ double elevator_goal_ = 0.0;
+ double arm_goal_ = 0.0;
+ double claw_goal_ = 0.0;
+
+ // If we're waiting for the subsystems to zero.
+ bool waiting_for_zero_ = false;
+
bool auto_running_ = false;
::aos::common::actions::ActionQueue action_queue_;