Simple auto-stack state machine.
Change-Id: I199be90257cebd612af51a9a7080ed4e8ce4c8b5
diff --git a/bot3/joystick_reader.cc b/bot3/joystick_reader.cc
index 826c5f0..29d0f75 100644
--- a/bot3/joystick_reader.cc
+++ b/bot3/joystick_reader.cc
@@ -39,6 +39,12 @@
// Preset motion limits.
constexpr ProfileParams kElevatorMove{0.3, 1.0};
+constexpr ProfileParams kFastElevatorMove{1.0, 5.0};
+
+// Preset goals for autostacking
+constexpr double kOneToteHeight{0.45};
+constexpr double kCarryHeight{0.180};
+constexpr double kGroundHeight{0.030};
// Joystick & button addresses.
const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
@@ -54,8 +60,8 @@
const POVLocation kOpenPassiveSupport(4, 0);
const POVLocation kClosePassiveSupport(4, 180);
-const ButtonLocation kIntakeIn(3, 7);
-const ButtonLocation kIntakeOut(4, 12);
+const ButtonLocation kIntakeOut(3, 7);
+const ButtonLocation kIntakeIn(4, 12);
const ButtonLocation kAutoStack(4, 7);
const ButtonLocation kManualStack(4, 6);
@@ -107,15 +113,86 @@
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
intake_closed_ = false;
can_restraint_open_ = true;
passive_support_open_ = true;
+ LOG(DEBUG, "Canceling\n");
+ }
+
+ elevator_queue.status.FetchLatest();
+ if (!elevator_queue.status.get()) {
+ LOG(ERROR, "Got no elevator status packet.\n");
+ }
+
+ if (elevator_queue.status.get() && elevator_queue.status->zeroed) {
+ if (waiting_for_zero_) {
+ LOG(INFO, "Zeroed! Starting teleop mode.\n");
+ waiting_for_zero_ = false;
+
+ // Set the initial goals to the bottom, since otherwise the driver seems
+ // to get confused and think that the end of the zeroing sequence is the
+ // same location and then wonders why it doesn't work...
+ elevator_goal_ = kGroundHeight;
+ }
+ } else {
+ waiting_for_zero_ = true;
+ }
+
+ if (data.PosEdge(kAutoStack)) {
+ action_queue_.CancelAllActions();
+ if (tote_count_ == 6) {
+ stacking_state_machine_ = FULL;
+ } else {
+ stacking_state_machine_ = WAITING;
+ }
+ }
+ if (data.IsPressed(kAutoStack)) {
+ switch (stacking_state_machine_) {
+ case WAITING:
+ elevator_params_ = kFastElevatorMove;
+ elevator_goal_ = kOneToteHeight;
+ if (elevator_queue.status->has_tote) {
+ stacking_state_machine_ = GOING_DOWN;
+ }
+ break;
+ case GOING_DOWN:
+ elevator_params_ = kFastElevatorMove;
+ elevator_goal_ = kGroundHeight;
+ if (elevator_queue.status->height < 0.05) {
+ ++tote_count_;
+ if (tote_count_ == 6) {
+ stacking_state_machine_ = FULL;
+ } else {
+ stacking_state_machine_ = GOING_UP;
+ }
+ }
+ break;
+ case GOING_UP:
+ elevator_params_ = kFastElevatorMove;
+ elevator_goal_ = kOneToteHeight;
+ if (elevator_queue.status->height > kOneToteHeight - 0.05) {
+ stacking_state_machine_ = WAITING;
+ }
+ break;
+ case FULL:
+ elevator_goal_ = kCarryHeight;
+ elevator_params_ = kFastElevatorMove;
+ break;
+ case OFF:
+ stacking_state_machine_ = WAITING;
+ break;
+ }
+ } else {
+ stacking_state_machine_ = OFF;
}
// Buttons for intaking.
if (data.IsPressed(kIntakeIn)) {
intake_goal = 10.0;
+ if (stacking_state_machine_ != OFF &&
+ elevator_queue.status->height < 0.43) {
+ intake_goal = 0.0;
+ }
} else if (data.IsPressed(kIntakeOut)) {
intake_goal = -10.0;
@@ -123,7 +200,6 @@
}
// TODO(Adam): Implement essential actors/goals.
if (data.PosEdge(kMoveToteHeight)) {
- // TODO(adams): Find correct values for height, velocity, and acceleration.
elevator_goal_ = 0.45;
elevator_params_ = {1.0, 5.0};
action_queue_.CancelAllActions();
@@ -153,7 +229,6 @@
passive_support_open_ = false;
}
-
// Buttons for elevator.
if (data.PosEdge(kCarry)) {
@@ -165,7 +240,7 @@
if (data.PosEdge(kSetDown)) {
// TODO(comran): Get actual height/velocity/acceleration values.
- elevator_goal_ = 0.030;
+ elevator_goal_ = 0.005;
elevator_params_ = {1.0, 5.0};
action_queue_.CancelAllActions();
}
@@ -178,31 +253,12 @@
if (data.PosEdge(kScoreBegin)) {
// TODO(comran): Get actual height/velocity/acceleration values.
- elevator_goal_ = 0.0;
- elevator_params_ = {1.0, 1.5};
- }
-
- if (data.PosEdge(ControlBit::kEnabled)) {
- // If we got enabled, wait for everything to zero.
- LOG(INFO, "Waiting for zero.\n");
- waiting_for_zero_ = true;
- }
-
- elevator_queue.status.FetchLatest();
- if (!elevator_queue.status.get()) {
- LOG(ERROR, "Got no elevator status packet.\n");
- }
-
- if (elevator_queue.status.get() && elevator_queue.status->zeroed) {
- if (waiting_for_zero_) {
- LOG(INFO, "Zeroed! Starting teleop mode.\n");
- waiting_for_zero_ = false;
-
- // Set the initial goals to where we are now.
- elevator_goal_ = elevator_queue.status->goal_height;
- }
- } else {
- waiting_for_zero_ = true;
+ elevator_goal_ = 0.030;
+ elevator_params_ = {1.0, 5.0};
+ intake_closed_ = false;
+ can_restraint_open_ = true;
+ passive_support_open_ = true;
+ tote_count_ = 0;
}
// Send our goals if everything looks OK.
@@ -269,12 +325,24 @@
bool intake_closed_ = false;
- bool can_restraint_open_ = false;
+ bool can_restraint_open_ = true;
- bool passive_support_open_ = false;
+ bool passive_support_open_ = true;
+
+ int tote_count_ = 0;
::aos::common::actions::ActionQueue action_queue_;
+ enum StackingStateMachine {
+ WAITING,
+ GOING_DOWN,
+ GOING_UP,
+ FULL,
+ OFF
+ };
+
+ StackingStateMachine stacking_state_machine_;
+
::aos::util::SimpleLogInterval no_drivetrain_status_ =
::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.2), WARNING,
"no drivetrain status");