Add game piece auto holding to joysticks and intaking
This is closer to production but not all the way there.
Change-Id: I540d0f7913fc0cd4894a2056b6266660b1841ac0
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index f718c3d..17cc45f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -30,14 +30,28 @@
namespace joysticks {
// TODO(sabina): update button locations when the board is done
-const ButtonLocation kElevatorUp(4, 4);
const ButtonLocation kIntakeOut(3, 3);
const ButtonLocation kElevatorDown(0, 0);
-const ButtonLocation kElevator1(4, 1);
-const ButtonLocation kElevator2(4, 11);
-const ButtonLocation kElevator3(4, 9);
-const ButtonLocation kElevator4(4, 7);
-const ButtonLocation kElevator5(4, 5);
+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 kElevatorIntakingUp(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);
@@ -54,11 +68,11 @@
const ButtonLocation kCargoSuction(0, 0);
const ButtonLocation kDiskSuction(0, 0);
const ButtonLocation kSuctionOut(0, 0);
-const ButtonLocation kDeployStilt(3, 8);
const ButtonLocation kRetractStilt(0, 0);
const ButtonLocation kBackwards(0, 0);
-const ButtonLocation kWristDown(3, 1);
+const ButtonLocation kWristBackwards(3, 10);
+const ButtonLocation kWristForwards(3, 7);
class Reader : public ::aos::input::ActionJoystickInput {
public:
@@ -146,6 +160,7 @@
// 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;
@@ -156,39 +171,104 @@
new_superstructure_goal->suction.top = true;
new_superstructure_goal->suction.bottom = true;
}
+ */
+
+ if (data.IsPressed(kSuctionBall)) {
+ top_ = false;
+ bottom_ = true;
+ } else if (data.IsPressed(kSuctionHatch)) {
+ top_ = true;
+ bottom_ = true;
+ } else if (data.IsPressed(kRelease) ||
+ !superstructure_queue.status->has_piece) {
+ top_ = false;
+ bottom_ = false;
+ }
// TODO(sabina): max height please?
if (data.IsPressed(kDeployStilt)) {
- new_superstructure_goal->stilts.unsafe_goal = 0.3;
+ new_superstructure_goal->stilts.unsafe_goal = 0.50;
+ } else if (data.IsPressed(kFallOver)) {
+ new_superstructure_goal->stilts.unsafe_goal = 0.71;
} else {
new_superstructure_goal->stilts.unsafe_goal = 0.01;
}
- if (data.IsPressed(kIntakeOut)) {
- new_superstructure_goal->intake.unsafe_goal = 0.8;
- new_superstructure_goal->roller_voltage = 5.0;
- } else {
- new_superstructure_goal->intake.unsafe_goal = -1.2;
- new_superstructure_goal->roller_voltage = 0.0;
- }
-
- if (data.IsPressed(kElevator1)) {
+ if (data.IsPressed(kElevatorFront1)) {
elevator_height_ = 1.5;
- } else if (data.IsPressed(kElevator2)) {
+ } else if (data.IsPressed(kElevatorFront2)) {
elevator_height_ = 1.2;
- } else if (data.IsPressed(kElevator3)) {
+ } else if (data.IsPressed(kElevatorFront3)) {
elevator_height_ = 0.8;
- } else if (data.IsPressed(kElevator4)) {
+ } else if (data.IsPressed(kElevatorFront4)) {
elevator_height_ = 0.3;
- } else if (data.IsPressed(kElevator5)) {
+ } else if (data.IsPressed(kElevatorFront5)) {
elevator_height_ = 0.01;
}
+ /*
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(kElevatorIntaking)) {
+ //}
+ if (data.IsPressed(kIntakeOut) && !superstructure_queue.status->has_piece) {
+ elevator_height_ = 0.29;
+ wrist_angle_ = 2.14;
+ new_superstructure_goal->intake.unsafe_goal = 0.52;
+ if (data.IsPressed(kElevatorIntaking)) {
+ new_superstructure_goal->roller_voltage = 6.0;
+ } else {
+ new_superstructure_goal->roller_voltage = 0.0;
+ }
+ Ball();
+ } else {
+ new_superstructure_goal->intake.unsafe_goal = -1.2;
+ new_superstructure_goal->roller_voltage = 0.0;
+ }
+
+ if (data.IsPressed(kElevatorIntakingUp)) {
+ elevator_height_ = 0.29 + 0.3;
+ wrist_angle_ = 2.14;
+ }
+
+
+ if (data.IsPressed(kRelease)) {
+ top_ = false;
+ bottom_ = false;
+ }
+
+ 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_;
@@ -199,10 +279,22 @@
}
}
+ void Disc() {
+ top_ = true;
+ bottom_ = true;
+ }
+ void Ball() {
+ top_ = false;
+ bottom_ = true;
+ }
+
private:
// Current goals here.
double elevator_height_ = 0.0;
double wrist_angle_ = 0.0;
+
+ bool top_ = false;
+ bool bottom_ = false;
};
} // namespace joysticks