Modify joystick reader for controller and define more setpoints
Change-Id: Id838374c4274042c5d63d639c61aba1031a0262b
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 310f41f..746feab 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -29,57 +29,83 @@
namespace input {
namespace joysticks {
-// TODO(sabina): update button locations when the board is done
-const ButtonLocation kIntakeOut(3, 3);
-const ButtonLocation kElevatorDown(0, 0);
-const ButtonLocation kElevatorFront1(4, 1);
-const ButtonLocation kElevatorFront2(4, 11);
-const ButtonLocation kElevatorFront3(4, 9);
-const ButtonLocation kElevatorFront4(4, 7);
-const ButtonLocation kElevatorFront5(4, 5);
-
-const ButtonLocation kElevatorBack1(3, 14);
-const ButtonLocation kElevatorBack2(4, 12);
-const ButtonLocation kElevatorBack3(4, 10);
-const ButtonLocation kElevatorBack4(4, 8);
-const ButtonLocation kElevatorBack5(4, 6);
-
-const ButtonLocation kElevatorIntaking(3, 4);
-const ButtonLocation kElevatorOuttake(3, 6);
-const ButtonLocation kRelease(4, 4);
-
const ButtonLocation kSuctionBall(3, 13);
const ButtonLocation kSuctionHatch(3, 12);
const ButtonLocation kDeployStilt(3, 8);
const ButtonLocation kFallOver(3, 9);
-const ButtonLocation kDiskLoad(0, 0);
-const ButtonLocation kDiskRocketMiddle(0, 0);
-const ButtonLocation kDiskRocketTop(0, 0);
-const ButtonLocation kCargoLoad(0, 0);
-const ButtonLocation kCargoBay(0, 0);
-const ButtonLocation kCargoRocketBase(0, 0);
-const ButtonLocation kCargoRocketMiddle(0, 0);
-const ButtonLocation kCargoRocketTop(0, 0);
-const ButtonLocation kStow(0, 0);
-const ButtonLocation kIntakeExtend(0, 0);
-const ButtonLocation kIntake(0, 0);
-const ButtonLocation kSpit(0, 0);
-const ButtonLocation kCargoSuction(0, 0);
-const ButtonLocation kDiskSuction(0, 0);
-const ButtonLocation kSuctionOut(0, 0);
-const ButtonLocation kRetractStilt(0, 0);
-const ButtonLocation kBackwards(0, 0);
+struct ElevatorWristPosition {
+ double elevator;
+ double wrist;
+};
-const ButtonLocation kWristBackwards(3, 10);
-const ButtonLocation kWristForwards(3, 7);
+const ButtonLocation kRocketForwardLower(5, 1);
+const ButtonLocation kRocketForwardMiddle(5, 2);
+const ButtonLocation kRocketForwardUpper(5, 4);
+const ButtonLocation kCargoForward(5, 3);
+
+const POVLocation kRocketBackwardUnpressed(5, -1);
+const POVLocation kRocketBackwardLower(5, 180);
+const POVLocation kRocketBackwardMiddle(5, 90);
+const POVLocation kRocketBackwardUpper(5, 0);
+const POVLocation kCargoBackward(5, 270);
+
+const ButtonLocation kPanelSwitch(5, 7);
+const ButtonLocation kCargoSwitch(5, 8);
+
+const ButtonLocation kBallHPIntakeForward(5, 6);
+const ButtonLocation kBallHPIntakeBackward(5, 5);
+const JoystickAxis kBallOutake(5, 3);
+const JoystickAxis kBallIntake(5, 4);
+
+const ButtonLocation kPanelHPIntakeForward(5, 6);
+const ButtonLocation kPanelHPIntakeBackward(5, 5);
+
+const ButtonLocation kRelease(2, 4);
+
+const ElevatorWristPosition kStowPos{0.36, 0.0};
+
+const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.04, M_PI / 2.0};
+const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.04, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelForwardMiddlePos{0.7412, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardMiddlePos{0.7412, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelForwardUpperPos{1.4524, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardUpperPos{1.4524, -M_PI / 2.0};
+
+const ElevatorWristPosition kBallForwardLowerPos{0.0, 0.98};
+const ElevatorWristPosition kBallBackwardLowerPos{0.607, -2.281};
+
+const ElevatorWristPosition kBallForwardMiddlePos{0.67945, 1.22};
+const ElevatorWristPosition kBallBackwardMiddlePos{0.93345, -1.83};
+
+const ElevatorWristPosition kBallForwardUpperPos{1.41605, 1.22};
+const ElevatorWristPosition kBallBackwardUpperPos{1.41605, -1.36};
+
+const ElevatorWristPosition kBallCargoForwardPos{0.73025, M_PI / 2};
+const ElevatorWristPosition kBallCargoBackwardPos{0.80645, -1.92};
+
+const ElevatorWristPosition kBallHPIntakeForwardPos{0.340, 0.737};
+const ElevatorWristPosition kBallHPIntakeBackwardPos{0.52, -1.1};
+
+const ElevatorWristPosition kBallIntakePos{0.29, 2.14};
class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
: ::aos::input::ActionJoystickInput(
event_loop,
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {}
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
+ superstructure_queue.goal.FetchLatest();
+ if (superstructure_queue.goal.get()) {
+ top_ = superstructure_queue.goal->suction.top;
+ bottom_ = superstructure_queue.goal->suction.bottom;
+ }
+ }
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
superstructure_queue.position.FetchLatest();
@@ -92,203 +118,134 @@
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
- /*
- if (data.IsPressed(kElevatorUp)) {
- elevator_height_ += 0.1;
- } else if (data.IsPressed(kElevatorDown)) {
- elevator_height_ -= 0.1;
- } else if (data.IsPressed(kDiskLoad)) {
- elevator_height_ = 0.48;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kDiskRocketMiddle)) {
- elevator_height_ = 1.19;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kDiskRocketTop)) {
- elevator_height_ = 1.90;
- wrist_angle_ = M_PI;
- }
-
- // TODO(sabina): do we need an angle here?
- else if (data.IsPressed(kCargoLoad)) {
- elevator_height_ = 1.12;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kCargoBay)) {
- elevator_height_ = 0.0;
- wrist_angle_ = M_PI / 3;
- } else if (data.IsPressed(kCargoRocketBase)) {
- elevator_height_ = 0.7;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kCargoRocketMiddle)) {
- elevator_height_ = 1.41;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kCargoRocketTop)) {
- elevator_height_ = 2.12;
- wrist_angle_ = M_PI;
- } else if (data.IsPressed(kStow)) {
- elevator_height_ = 0.5;
- wrist_angle_ = 0.0;
- } else {
- }
- */
-
- /*
- // TODO(sabina): get accurate angle.
- if (data.IsPressed(kIntakeExtend)) {
- new_superstructure_goal->intake.unsafe_goal = 0.5;
- } else {
- new_superstructure_goal->intake.unsafe_goal = 0.0;
- }
-
- if (data.IsPressed(kIntake)) {
- new_superstructure_goal->suction.bottom = true;
- if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->roller_voltage = 12.0;
- } else {
- new_superstructure_goal->roller_voltage = 0.0;
- }
- } else if (data.IsPressed(kSpit)) {
- new_superstructure_goal->suction.bottom = false;
- if (superstructure_queue.status->has_piece == false) {
- new_superstructure_goal->roller_voltage = 12.0;
- } else {
- new_superstructure_goal->roller_voltage = 0.0;
- }
- } else {
- new_superstructure_goal->roller_voltage = 0.0;
- }
- */
-
- // TODO(sabina): decide if we should really have disk suction as its own
- // button
- /*
- if (data.IsPressed(kCargoSuction)) {
- new_superstructure_goal->suction.top = false;
- new_superstructure_goal->suction.bottom = true;
- } else if (data.IsPressed(kDiskSuction)) {
- new_superstructure_goal->suction.top = true;
- new_superstructure_goal->suction.bottom = true;
- } else if (data.IsPressed(kSuctionOut)) {
- new_superstructure_goal->suction.top = true;
- new_superstructure_goal->suction.bottom = true;
- }
- */
-
if (data.IsPressed(kSuctionBall)) {
- top_ = false;
- bottom_ = true;
+ Ball();
} else if (data.IsPressed(kSuctionHatch)) {
- top_ = true;
- bottom_ = true;
+ Disc();
} else if (data.IsPressed(kRelease) ||
!superstructure_queue.status->has_piece) {
top_ = false;
bottom_ = false;
}
+ if (data.IsPressed(kRocketBackwardUnpressed)) {
+ elevator_wrist_pos_ = kStowPos;
+ }
+ new_superstructure_goal->intake.unsafe_goal = -1.2;
+ new_superstructure_goal->roller_voltage = 0.0;
+
+ const bool kDoBallIntake = data.GetAxis(kBallIntake) > 0.9;
+ const bool kDoBallOutake = data.GetAxis(kBallOutake) > 0.9;
+
+ if (data.IsPressed(kPanelSwitch)) {
+ switch_ball_ = false;
+ } else if (data.IsPressed(kCargoSwitch)) {
+ switch_ball_ = true;
+ }
+
// TODO(sabina): max height please?
if (data.IsPressed(kFallOver)) {
new_superstructure_goal->stilts.unsafe_goal = 0.71;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.45;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 0.5;
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
} else if (data.IsPressed(kDeployStilt)) {
new_superstructure_goal->stilts.unsafe_goal = 0.50;
new_superstructure_goal->stilts.profile_params.max_velocity = 0.45;
- new_superstructure_goal->stilts.profile_params.max_acceleration = 0.5;
+ if (stilts_was_above_) {
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 0.75;
+ } else {
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
+ }
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.01;
+ new_superstructure_goal->stilts.profile_params.max_velocity = 0.25;
+ new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
}
- if (data.IsPressed(kElevatorFront1)) {
- elevator_height_ = 1.5;
- } else if (data.IsPressed(kElevatorFront2)) {
- elevator_height_ = 1.2;
- } else if (data.IsPressed(kElevatorFront3)) {
- elevator_height_ = 0.8;
- } else if (data.IsPressed(kElevatorFront4)) {
- elevator_height_ = 0.3;
- } else if (data.IsPressed(kElevatorFront5)) {
- elevator_height_ = 0.01;
+ if (superstructure_queue.status->stilts.position > 0.65) {
+ stilts_was_above_ = true;
+ } else if (superstructure_queue.status->stilts.position < 0.1) {
+ stilts_was_above_ = false;
}
- /*
- if (data.IsPressed(kWristDown)) {
- wrist_angle_ = -M_PI / 3.0;
- } else {
- wrist_angle_ = M_PI / 3.0;
- }
- */
- if (data.IsPressed(kWristBackwards)) {
- // Hatch pannel back
- elevator_height_ = 0.03;
- wrist_angle_ = -M_PI / 2.0;
- Disc();
- } else if (data.IsPressed(kWristForwards)) {
- // Hatch pannel front
- elevator_height_ = 0.03;
- wrist_angle_ = M_PI / 2.0;
- Disc();
- } else if (data.IsPressed(kElevatorFront5)) {
- // Ball front
- Ball();
- elevator_height_ = 0.52;
- wrist_angle_ = 1.1;
- } else if (data.IsPressed(kElevatorBack5)) {
- // Ball back
- elevator_height_ = 0.52;
- wrist_angle_ = -1.1;
- } else if (data.IsPressed(kElevatorFront2)) {
- elevator_height_ = 1.5;
- wrist_angle_ = 0.0;
- } else {
- wrist_angle_ = 0.0;
- elevator_height_ = 0.36;
- }
-
- if (data.IsPressed(kElevatorOuttake) ||
- (data.IsPressed(kIntakeOut) &&
- !superstructure_queue.status->has_piece)) {
- new_superstructure_goal->intake.unsafe_goal = 0.959327;
- }
-
- if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
- elevator_height_ = 0.29;
- wrist_angle_ = 2.14;
- if (data.IsPressed(kElevatorIntaking)) {
- new_superstructure_goal->roller_voltage = 9.0;
- } else {
- new_superstructure_goal->roller_voltage = 0.0;
+ if (switch_ball_) {
+ if (superstructure_queue.status->has_piece) {
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
}
- Ball();
+
+ // Go to intake position and apply vacuum
+ if (data.IsPressed(kBallHPIntakeForward)) {
+ Ball();
+ elevator_wrist_pos_ = kBallHPIntakeForwardPos;
+ } else if (data.IsPressed(kBallHPIntakeBackward)) {
+ Ball();
+ elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
+ }
+
+ // Go to elevator/wrist position. Overrides intake position if pressed so
+ // we can re-grab the ball.
+ if (data.IsPressed(kRocketForwardLower)) {
+ elevator_wrist_pos_ = kBallForwardLowerPos;
+ } else if (data.IsPressed(kRocketBackwardLower)) {
+ elevator_wrist_pos_ = kBallBackwardLowerPos;
+ } else if (data.IsPressed(kRocketForwardMiddle)) {
+ elevator_wrist_pos_ = kBallForwardMiddlePos;
+ } else if (data.IsPressed(kRocketBackwardMiddle)) {
+ elevator_wrist_pos_ = kBallBackwardMiddlePos;
+ } else if (data.IsPressed(kRocketForwardUpper)) {
+ elevator_wrist_pos_ = kBallForwardUpperPos;
+ } else if (data.IsPressed(kRocketBackwardUpper)) {
+ elevator_wrist_pos_ = kBallBackwardUpperPos;
+ } else if (data.IsPressed(kCargoForward)) {
+ elevator_wrist_pos_ = kBallCargoForwardPos;
+ } else if (data.IsPressed(kCargoBackward)) {
+ elevator_wrist_pos_ = kBallCargoBackwardPos;
+ }
} else {
- new_superstructure_goal->intake.unsafe_goal = -1.2;
- new_superstructure_goal->roller_voltage = 0.0;
+ if (data.IsPressed(kPanelHPIntakeForward)) {
+ Disc();
+ elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
+ } else if (data.IsPressed(kPanelHPIntakeBackward)) {
+ Disc();
+ elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
+ }
+
+ // Go to elevator/wrist position. Overrides intake position if pressed so
+ // we can re-grab the panel.
+ if (data.IsPressed(kRocketForwardLower)) {
+ elevator_wrist_pos_ = kPanelForwardLowerPos;
+ } else if (data.IsPressed(kRocketBackwardLower)) {
+ elevator_wrist_pos_ = kPanelBackwardLowerPos;
+ } else if (data.IsPressed(kRocketForwardMiddle)) {
+ elevator_wrist_pos_ = kPanelForwardMiddlePos;
+ } else if (data.IsPressed(kRocketBackwardMiddle)) {
+ elevator_wrist_pos_ = kPanelBackwardMiddlePos;
+ } else if (data.IsPressed(kRocketForwardUpper)) {
+ elevator_wrist_pos_ = kPanelForwardUpperPos;
+ } else if (data.IsPressed(kRocketBackwardUpper)) {
+ elevator_wrist_pos_ = kPanelBackwardUpperPos;
+ }
}
- if (data.IsPressed(kElevatorOuttake)) {
- new_superstructure_goal->roller_voltage = -6.0;
- }
+ if (switch_ball_) {
+ if (kDoBallOutake ||
+ (kDoBallIntake && !superstructure_queue.status->has_piece)) {
+ new_superstructure_goal->intake.unsafe_goal = 0.959327;
+ }
- if (data.IsPressed(kElevatorBack1)) {
- wrist_angle_ = -2.451824;
- elevator_height_ = 0.430478;
- //new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
- //new_superstructure_goal->wrist.profile_params.max_acceleration = 20.0;
+ if (kDoBallIntake && !superstructure_queue.status->has_piece) {
+ elevator_wrist_pos_ = kBallIntakePos;
+ new_superstructure_goal->roller_voltage = 9.0;
+ Ball();
+ } else {
+ if (kDoBallOutake) {
+ new_superstructure_goal->roller_voltage = -6.0;
+ } else {
+ new_superstructure_goal->intake.unsafe_goal = -1.2;
+ new_superstructure_goal->roller_voltage = 0.0;
+ }
+ }
}
- if (data.IsPressed(kElevatorBack2)) {
- wrist_angle_ = -2.400;
- elevator_height_ = 0.364108;
- new_superstructure_goal->elevator.profile_params.max_velocity = 2.0;
- new_superstructure_goal->elevator.profile_params.max_acceleration = 5.0;
- new_superstructure_goal->wrist.profile_params.max_velocity = 0.35;
- new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
- }
- if (data.IsPressed(kElevatorBack3)) {
- wrist_angle_ = -2.211173;
- elevator_height_ = 0.25;
- new_superstructure_goal->wrist.profile_params.max_velocity = 2.0;
- new_superstructure_goal->wrist.profile_params.max_acceleration = 10.0;
- }
-
if (data.IsPressed(kRelease)) {
top_ = false;
@@ -298,8 +255,9 @@
new_superstructure_goal->suction.top = top_;
new_superstructure_goal->suction.bottom = bottom_;
- new_superstructure_goal->elevator.unsafe_goal = elevator_height_;
- new_superstructure_goal->wrist.unsafe_goal = wrist_angle_;
+ new_superstructure_goal->elevator.unsafe_goal =
+ elevator_wrist_pos_.elevator;
+ new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist;
LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
if (!new_superstructure_goal.Send()) {
@@ -318,11 +276,12 @@
private:
// Current goals here.
- double elevator_height_ = 0.0;
- double wrist_angle_ = 0.0;
-
+ ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool top_ = false;
bool bottom_ = false;
+
+ bool switch_ball_ = false;
+ bool stilts_was_above_ = false;
};
} // namespace joysticks