Add 3 box auto.
Change-Id: Iba294717f526ebfb1d8f3a61942afb31a86859b4
diff --git a/y2018/actors/autonomous_actor.cc b/y2018/actors/autonomous_actor.cc
index a31d2f3..edbe406 100644
--- a/y2018/actors/autonomous_actor.cc
+++ b/y2018/actors/autonomous_actor.cc
@@ -29,11 +29,21 @@
const ProfileParameters kFinalSwitchDrive = {0.5, 0.5};
const ProfileParameters kDrive = {5.0, 2.5};
+const ProfileParameters kThirdBoxDrive = {3.0, 2.5};
const ProfileParameters kSlowDrive = {1.5, 2.5};
+const ProfileParameters kScaleTurnDrive = {3.0, 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};
+const ProfileParameters kReallyFastTurn = {1.5, 9.0};
+
+const ProfileParameters kThirdBoxSlowBackup = {0.35, 1.5};
+const ProfileParameters kThirdBoxSlowTurn = {1.5, 4.0};
+
+const ProfileParameters kThirdBoxPlaceDrive = {4.0, 2.3};
+
+const ProfileParameters kFinalFrontFarSwitchDrive = {2.0, 2.0};
} // namespace
@@ -80,74 +90,284 @@
} 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);
+ StartDrive(-3.2, 0.0, kDrive, kTurn);
if (!WaitForDriveProfileDone()) return true;
+ } break;
+ case 200:
+ {
+ constexpr bool kDriveBehind = true;
+ if (kDriveBehind) {
+ // Start on the left. Hit the switch.
+ 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);
- // Now, close so let's move the arm up.
- set_arm_goal_position(arm::FrontSwitchAutoIndex());
- SendSuperstructureGoal();
+ 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;
- StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
- if (!WaitForTurnProfileDone()) return true;
+ // Now, close so let's move the arm up.
+ set_arm_goal_position(arm::FrontSwitchAutoIndex());
+ SendSuperstructureGoal();
- 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));
+ StartDrive(0.0, M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
- set_open_claw(true);
- SendSuperstructureGoal();
+ 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));
- ::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;
+ 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;
+ } else {
+ // Start on the left. Hit the switch.
+ constexpr double kFullDriveLength = 5.55;
+ constexpr double kTurnDistance = 0.35;
+ StartDrive(-kFullDriveLength, 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::FrontSwitchIndex());
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, -M_PI / 2.0, kDrive, kFastTurn);
+ if (!WaitForTurnProfileDone()) return true;
+
+ set_max_drivetrain_voltage(10.0);
+ StartDrive(1.1, 0.0, kDrive, kTurn);
+ if (!WaitForArmTrajectoryClose(0.001)) return true;
+ if (!WaitForDriveNear(0.6, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ if (!WaitForDriveNear(0.3, M_PI / 2.0)) return true;
+ set_max_drivetrain_voltage(6.0);
+ StartDrive(0.0, 0.0, kFinalFrontFarSwitchDrive, kTurn);
+
+ // if (!WaitForDriveNear(0.2, M_PI / 2.0)) return true;
+ ::std::this_thread::sleep_for(chrono::milliseconds(300));
+
+ set_open_claw(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(1000));
+ 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;
+ constexpr double kDriveDistance = 6.95;
+ // Distance and angle to do the big drive to the third cube.
+ constexpr double kThirdCubeDrive = 1.57;
+ constexpr double kThirdCubeTurn = M_PI / 4.0 - 0.1;
+ // Angle to do the slow pickup turn on the third box.
+ constexpr double kThirdBoxEndTurnAngle = 0.20;
+
+ // Distance to drive back to the scale with the third cube.
+ constexpr double kThirdCubeDropDrive = kThirdCubeDrive + 0.30;
+
// 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());
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
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);
+ if (!WaitForDriveNear(4.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, 0.0, kScaleTurnDrive, kFastTurn);
// 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);
+ if (!WaitForDriveNear(3.25, M_PI / 2.0)) return true;
+ StartDrive(0.0, -M_PI / 6.0, kScaleTurnDrive, kFastTurn);
+
+ if (!WaitForDriveNear(1.0, M_PI / 2.0)) return true;
+ StartDrive(0.0, M_PI / 6.0, kScaleTurnDrive, kFastTurn);
// Get close and open the claw.
- if (!WaitForDriveNear(0.25, 0.25)) return true;
+ if (!WaitForDriveNear(0.15, 0.25)) return true;
set_open_claw(true);
SendSuperstructureGoal();
+ set_intake_angle(-0.40);
+ LOG(INFO, "Dropped first box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
::std::this_thread::sleep_for(chrono::milliseconds(500));
- StartDrive(0.25, 0.0, kDrive, kTurn);
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(200));
+
+ LOG(INFO, "Starting second box drive %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ constexpr double kSecondBoxSwerveAngle = 0.35;
+ constexpr double kSecondBoxDrive = 1.43;
+ StartDrive(kSecondBoxDrive, 0.0, kDrive, kFastTurn);
+ if (!WaitForDriveNear(kSecondBoxDrive - 0.2, M_PI / 2.0)) return true;
+
+ StartDrive(0.0, kSecondBoxSwerveAngle, kDrive, kFastTurn);
+ if (!WaitForDriveNear(0.5, M_PI / 2.0)) return true;
+
+ set_open_claw(true);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+
+ StartDrive(0.0, -kSecondBoxSwerveAngle, kDrive, kFastTurn);
+
if (!WaitForDriveProfileDone()) return true;
+ set_intake_angle(0.10);
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ set_open_claw(false);
+
+ set_roller_voltage(10.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Grabbing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForBoxGrabed()) return true;
+
+ LOG(INFO, "Got second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ ::std::this_thread::sleep_for(chrono::milliseconds(600));
+
+ set_grab_box(false);
+ set_arm_goal_position(arm::UpIndex());
+ set_roller_voltage(0.0);
+ set_disable_box_correct(false);
+ SendSuperstructureGoal();
+ LOG(INFO, "Driving to place second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ StartDrive(-kSecondBoxDrive + 0.2, kSecondBoxSwerveAngle, kDrive,
+ kFastTurn);
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+
+ constexpr double kSecondBoxEndExtraAngle = 0.3;
+ StartDrive(0.0, -kSecondBoxSwerveAngle - kSecondBoxEndExtraAngle, kDrive,
+ kFastTurn);
+
+ LOG(INFO, "Starting throw %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForDriveNear(0.4, M_PI / 2.0)) return true;
+ set_arm_goal_position(arm::BackHighBoxIndex());
+ SendSuperstructureGoal();
+
+ // Throw the box.
+ if (!WaitForArmTrajectoryClose(0.03)) return true;
+
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ LOG(INFO, "Releasing second box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(700));
+ set_open_claw(false);
+ set_grab_box(true);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Driving to third box %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ StartDrive(kThirdCubeDrive, kSecondBoxEndExtraAngle, kThirdBoxDrive,
+ kFastTurn);
+ if (!WaitForDriveNear(kThirdCubeDrive - 0.1, M_PI / 4.0)) return true;
+
+ StartDrive(0.0, kThirdCubeTurn, kThirdBoxDrive, kFastTurn);
+ if (!WaitForDriveNear(0.3, M_PI / 4.0)) return true;
+
+ set_intake_angle(0.05);
+ set_roller_voltage(9.0);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+ StartDrive(0.30, kThirdBoxEndTurnAngle, kThirdBoxSlowBackup, kThirdBoxSlowTurn);
+ if (!WaitForDriveProfileDone()) return true;
+
+ if (!WaitForBoxGrabed()) return true;
+ LOG(INFO, "Third box grabbed %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ const bool too_late =
+ monotonic_clock::now() > start_time + chrono::milliseconds(12000);
+ if (too_late) {
+ LOG(INFO, "Third box too long, going up. %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ set_grab_box(false);
+ set_arm_goal_position(arm::UpIndex());
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+ }
+ ::std::this_thread::sleep_for(chrono::milliseconds(600));
+
+ set_grab_box(false);
+ if (!too_late) {
+ set_arm_goal_position(arm::BackMiddle2BoxIndex());
+ }
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ StartDrive(-kThirdCubeDropDrive, 0.0,
+ kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForDriveNear(1.40, M_PI / 4.0)) return true;
+ StartDrive(0.0, -kThirdCubeTurn - 0.2 - kThirdBoxEndTurnAngle - 0.3,
+ kThirdBoxPlaceDrive, kReallyFastTurn);
+
+ if (!WaitForArmTrajectoryClose(0.005)) return true;
+ if (!WaitForDriveProfileDone()) return true;
+ if (!WaitForTurnProfileDone()) return true;
+
+ if (!too_late) {
+ set_open_claw(true);
+ set_intake_angle(-M_PI / 4.0);
+ set_roller_voltage(0.0);
+ SendSuperstructureGoal();
+
+ LOG(INFO, "Final open %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ }
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(14750) -
+ (monotonic_clock::now() - start_time));
+
+ set_arm_goal_position(arm::UpIndex());
+ SendSuperstructureGoal();
+
+ ::std::this_thread::sleep_for(chrono::milliseconds(15000) -
+ (monotonic_clock::now() - start_time));
+
+ set_close_claw(true);
+ SendSuperstructureGoal();
+
} break;
}
diff --git a/y2018/actors/autonomous_actor.h b/y2018/actors/autonomous_actor.h
index cf5766c..a292531 100644
--- a/y2018/actors/autonomous_actor.h
+++ b/y2018/actors/autonomous_actor.h
@@ -15,6 +15,7 @@
namespace y2018 {
namespace actors {
using ::y2018::control_loops::superstructure_queue;
+using ::frc971::control_loops::drivetrain_queue;
namespace arm = ::y2018::control_loops::superstructure::arm;
@@ -32,7 +33,9 @@
arm_goal_position_ = arm::NeutralIndex();
grab_box_ = false;
open_claw_ = false;
+ close_claw_ = false;
deploy_fork_ = false;
+ disable_box_correct_ = false;
InitializeEncoders();
ResetDrivetrain();
SendSuperstructureGoal();
@@ -44,11 +47,17 @@
uint32_t arm_goal_position_ = arm::NeutralIndex();
bool grab_box_ = false;
bool open_claw_ = false;
+ bool close_claw_ = false;
bool deploy_fork_ = false;
+ bool disable_box_correct_ = false;
void set_roller_voltage(double roller_voltage) {
roller_voltage_ = roller_voltage;
}
+ void set_intake_angle(double intake_angle) {
+ set_left_intake_angle(intake_angle);
+ set_right_intake_angle(intake_angle);
+ }
void set_left_intake_angle(double left_intake_angle) {
left_intake_angle_ = left_intake_angle;
}
@@ -60,8 +69,13 @@
}
void set_grab_box(bool grab_box) { grab_box_ = grab_box; }
void set_open_claw(bool open_claw) { open_claw_ = open_claw; }
+ void set_close_claw(bool close_claw) { close_claw_ = close_claw; }
void set_deploy_fork(bool deploy_fork) { deploy_fork_ = deploy_fork; }
+ void set_disable_box_correct(bool disable_box_correct) {
+ disable_box_correct_ = disable_box_correct;
+ }
+
void SendSuperstructureGoal() {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->intake.roller_voltage = roller_voltage_;
@@ -71,6 +85,7 @@
new_superstructure_goal->arm_goal_position = arm_goal_position_;
new_superstructure_goal->grab_box = grab_box_;
new_superstructure_goal->open_claw = open_claw_;
+ new_superstructure_goal->close_claw = close_claw_;
new_superstructure_goal->deploy_fork = deploy_fork_;
if (!new_superstructure_goal.Send()) {
@@ -78,6 +93,67 @@
}
}
+ bool WaitForArmTrajectoryOrDriveClose(double drive_threshold,
+ double arm_threshold) {
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+
+ constexpr double kPositionTolerance = 0.02;
+ constexpr double kProfileTolerance = 0.001;
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+
+ superstructure_queue.status.FetchLatest();
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status.get() && superstructure_queue.status.get()) {
+ const double left_profile_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->profiled_left_position_goal);
+ const double right_profile_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->profiled_right_position_goal);
+
+ const double left_error =
+ (initial_drivetrain_.left -
+ drivetrain_queue.status->estimated_left_position);
+ const double right_error =
+ (initial_drivetrain_.right -
+ drivetrain_queue.status->estimated_right_position);
+
+ const double profile_distance_to_go =
+ (left_profile_error + right_profile_error) / 2.0;
+
+ const double distance_to_go = (left_error + right_error) / 2.0;
+
+ // Check superstructure first.
+ if (superstructure_queue.status->arm.current_node ==
+ arm_goal_position_ &&
+ superstructure_queue.status->arm.path_distance_to_go <
+ arm_threshold) {
+ LOG(INFO, "Arm finished first: %f, drivetrain %f distance\n",
+ superstructure_queue.status->arm.path_distance_to_go,
+ ::std::abs(distance_to_go));
+ return true;
+ }
+
+ // Now check drivetrain.
+ if (::std::abs(profile_distance_to_go) <
+ drive_threshold + kProfileTolerance &&
+ ::std::abs(distance_to_go) < drive_threshold + kPositionTolerance) {
+ LOG(INFO,
+ "Drivetrain finished first: arm %f, drivetrain %f distance\n",
+ superstructure_queue.status->arm.path_distance_to_go,
+ ::std::abs(distance_to_go));
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
+
bool WaitForArmTrajectoryClose(double threshold) {
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
::std::chrono::milliseconds(5) / 2);
@@ -88,7 +164,8 @@
superstructure_queue.status.FetchLatest();
if (superstructure_queue.status.get()) {
- if (superstructure_queue.status->arm.current_node == arm_goal_position_ &&
+ if (superstructure_queue.status->arm.current_node ==
+ arm_goal_position_ &&
superstructure_queue.status->arm.path_distance_to_go < threshold) {
return true;
}
@@ -96,6 +173,24 @@
phased_loop.SleepUntilNext();
}
}
+
+ bool WaitForBoxGrabed() {
+ ::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.grab_state == 4) {
+ return true;
+ }
+ }
+ phased_loop.SleepUntilNext();
+ }
+ }
};
} // namespace actors