Add 3 autonomous modes.
Change-Id: Ied6692009ebd5373a422881180107c1b2d2b7116
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index 1e9b936..394d574 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -20,6 +20,11 @@
bool autonomous;
uint16_t team_id;
+ // 2018 scale and switch positions.
+ // TODO(austin): Push these out to a new message?
+ bool switch_left;
+ bool scale_left;
+
// If this is true, then this message isn't actually from the control
// system and so should not be trusted as evidence that the button inputs
// etc are actually real and should be acted on.
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 7d406d0..741b49c 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -31,13 +31,18 @@
ds->WaitForData();
auto new_state = ::aos::joystick_state.MakeMessage();
-#if defined(WPILIB2017) || defined(WPILIB2018)
HAL_ControlWord control_word;
HAL_GetControlWord(&control_word);
-#else
- HALControlWord control_word;
- HALGetControlWord(&control_word);
-#endif
+ HAL_MatchInfo match_info;
+ auto status = HAL_GetMatchInfo(&match_info);
+ if (status == 0) {
+ new_state->switch_left = match_info.gameSpecificMessage[0] == 'L' ||
+ match_info.gameSpecificMessage[0] == 'l';
+ new_state->scale_left = match_info.gameSpecificMessage[1] == 'L' ||
+ match_info.gameSpecificMessage[1] == 'l';
+ }
+ HAL_FreeMatchInfo(&match_info);
+
new_state->test_mode = control_word.test;
new_state->fms_attached = control_word.fmsAttached;
new_state->enabled = control_word.enabled;
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index 83a1295..a31d2f3 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -27,8 +27,13 @@
.count();
}
-const ProfileParameters kDrive = {1.5, 2.0};
-const ProfileParameters kTurn = {3.0, 3.0};
+const ProfileParameters kFinalSwitchDrive = {0.5, 0.5};
+const ProfileParameters kDrive = {5.0, 2.5};
+const ProfileParameters kSlowDrive = {1.5, 2.5};
+const ProfileParameters kFarSwitchTurnDrive = {2.0, 2.5};
+const ProfileParameters kTurn = {4.0, 2.0};
+const ProfileParameters kSweepingTurn = {5.0, 7.0};
+const ProfileParameters kFastTurn = {5.0, 7.0};
} // namespace
@@ -43,13 +48,107 @@
LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
Reset();
- for (int i = 0; i < 4; ++i) {
- StartDrive(0.0, M_PI / 2.0, kDrive, kTurn);
- if (!WaitForTurnProfileDone()) return true;
+ switch (params.mode) {
+ case 1: {
+ constexpr double kDriveDistance = 3.2;
+ // Start on the left. Drive, arc a turn, and drop in the close switch.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(2.0, M_PI / 2.0)) return true;
- // Drive, but get within 0.3 meters
- StartDrive(1.0, 0.0, kDrive, kTurn);
- if (!WaitForDriveProfileDone()) return true;
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::BackSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, -M_PI / 4.0 - 0.2, kSlowDrive, kSweepingTurn);
+ if (!WaitForDriveNear(0.2, 0.2)) return true;
+ set_max_drivetrain_voltage(6.0);
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(0.2, 0.0, kDrive, kTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+
+ } break;
+ case 0: {
+ // Start on the left. Hit the scale.
+ constexpr double kFullDriveLength = 9.95;
+ constexpr double kTurnDistance = 4.40;
+ StartDrive(-kFullDriveLength, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileNear(kFullDriveLength - (kTurnDistance - 1.4)))
+ return true;
+ StartDrive(0.0, 0.0, kFarSwitchTurnDrive, kTurn);
+
+ if (!WaitForDriveProfileNear(kFullDriveLength - kTurnDistance))
+ return true;
+ StartDrive(0.0, -M_PI / 2.0, kFarSwitchTurnDrive, kSweepingTurn);
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.0, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchAutoIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.35, 0.0, kFinalSwitchDrive, kTurn);
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ //if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(1500));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1500));
+ set_arm_goal_position(arm::NeutralIndex());
+ SendSuperstructureGoal();
+ if (ShouldCancel()) return true;
+ set_max_drivetrain_voltage(12.0);
+ StartDrive(-0.5, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ } break;
+
+ case 3:
+ case 2: {
+ // Start on the left. Hit the scale.
+ constexpr double kDriveDistance = 7.0;
+ // Drive.
+ StartDrive(-kDriveDistance, 0.0, kDrive, kTurn);
+ if (!WaitForDriveNear(kDriveDistance - 1.0, M_PI / 2.0)) return true;
+ // Once we are away from the wall, start the arm.
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ SendSuperstructureGoal();
+
+ // We are starting to get close. Slow down for the turn.
+ if (!WaitForDriveNear(2.5, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kSlowDrive, kSweepingTurn);
+
+ // Once we've gotten slowed down a bit, start turning.
+ if (!WaitForDriveNear(1.6, M_PI / 2.0)) return true;
+ StartDrive(0.0, -M_PI / 4.0 - 0.1, kSlowDrive, kSweepingTurn);
+
+ // Get close and open the claw.
+ if (!WaitForDriveNear(0.25, 0.25)) return true;
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(500));
+
+ StartDrive(0.25, 0.0, kDrive, kTurn);
+ if (!WaitForDriveProfileDone()) return true;
+ } break;
}
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index b8d58e2..cf5766c 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -77,6 +77,25 @@
LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
+
+ bool WaitForArmTrajectoryClose(double threshold) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ superstructure_queue.status.FetchLatest();
+ if (superstructure_queue.status.get()) {
+ if (superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go < threshold) {
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
};
} // namespace actors
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 81791e5..ce46ec7 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -248,11 +248,14 @@
::frc971::autonomous::AutonomousActionParams params;
::frc971::autonomous::auto_mode.FetchLatest();
if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
+ params.mode = ::frc971::autonomous::auto_mode->mode << 2;
} else {
LOG(WARNING, "no auto mode values\n");
params.mode = 0;
}
+ // TODO(austin): use the mode later if we care. We don't care right now.
+ params.mode = static_cast<int>(::aos::joystick_state->switch_left) |
+ (static_cast<int>(::aos::joystick_state->scale_left) << 1);
action_queue_.EnqueueAction(
::frc971::autonomous::MakeAutonomousAction(params));
}