Add champs autos
This adds the cargo ship auto as well as a rocket auto.
Change-Id: I81bd90de4ca0b1615f7f163c93d2854f4f5dd0c5
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 6a38967..1679a7d 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -58,6 +58,7 @@
LOG(INFO, "Stopping auto mode\n");
action_queue_.CancelAllActions();
auto_action_running_ = false;
+ AutoEnded();
}
} // namespace input
diff --git a/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index 097196d..d040b11 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -423,7 +423,7 @@
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
drivetrain_queue.status.FetchLatest();
- LOG_STRUCT(INFO, "dts", *drivetrain_queue.status.get());
+ LOG_STRUCT(DEBUG, "dts", *drivetrain_queue.status.get());
if (drivetrain_queue.status.get() &&
((drivetrain_queue.status->trajectory_logging.planning_spline_idx ==
spline_handle_ &&
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index 57aba4e..4e8e60d 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -5,7 +5,16 @@
namespace y2019 {
namespace actors {
-::frc971::MultiSpline AutonomousSplines::HabToFarRocket() {
+void MaybeFlipSpline(::frc971::MultiSpline *spline, bool is_left) {
+ if (!is_left) {
+ for (size_t i = 0; i < spline->spline_y.size(); i++) {
+ spline->spline_y[i] *= -1.0;
+ }
+ }
+}
+
+// Path off of level 2 to the far side of the rocket with a panel
+::frc971::MultiSpline AutonomousSplines::HABToFarRocket(bool is_left) {
::frc971::MultiSpline spline;
::frc971::Constraint longitudinal_constraint;
::frc971::Constraint lateral_constraint;
@@ -13,30 +22,272 @@
::frc971::Constraint velocity_constraint;
longitudinal_constraint.constraint_type = 1;
- longitudinal_constraint.value = 1.5;
+ longitudinal_constraint.value = 2.0;
lateral_constraint.constraint_type = 2;
- lateral_constraint.value = 1.0;
+ lateral_constraint.value = 2.0;
voltage_constraint.constraint_type = 3;
voltage_constraint.value = 11.0;
- // Note: This velocity constraint is currently too late in the spline to
- // actually do anything.
velocity_constraint.constraint_type = 4;
- velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 7.5;
+ velocity_constraint.value = 4.0;
+ velocity_constraint.start_distance = 0.0;
velocity_constraint.end_distance = 10.0;
- spline.spline_count = 1;
- spline.spline_x = {{1.0, 2.0, 4.0, 7.8, 7.8, 6.53}};
- spline.spline_y = {{1.5, 1.5, 1.5, 1.5, 3.0, 3.47}};
+ spline.spline_count = 2;
+ spline.spline_x = {{1.14763818102, 1.66, 3.10, 4.05, 4.45, 5.11, 5.77, 6.71,
+ 7.27, 7.19, 6.57}};
+ spline.spline_y = {{1.30261224364, 1.30217320136, 1.39, 1.47, 1.56346705393,
+ 1.69, 1.81, 1.97, 2.18, 2.84, 3.33}};
+
spline.constraints = {{longitudinal_constraint, lateral_constraint,
voltage_constraint, velocity_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
return spline;
}
-::frc971::MultiSpline AutonomousSplines::FarRockettoHP() {
+// Path from the far side of the rocket to the loading station to pickup
+::frc971::MultiSpline AutonomousSplines::FarRocketToHP(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 3.0;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 2.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 4.5;
+ velocity_constraint.start_distance = 0.0;
+ velocity_constraint.end_distance = 10.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{6.6, 7.511, 6.332, 4.590, 1.561, 0.179}};
+ spline.spline_y = {{3.391, 2.826, 1.384, 3.395 - 0.20, 3.429 - 0.20, 3.434 - 0.20}};
+
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from the human player station to the far side of the rocket with a panel
+::frc971::MultiSpline AutonomousSplines::HPToFarRocket(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint velocity_constraint;
+ ::frc971::Constraint voltage_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 3.0;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 3.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 2.0;
+ velocity_constraint.start_distance = 7.0;
+ velocity_constraint.end_distance = 15.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{0.895115737979, 2.9155615909, 5.02361983866,
+ 6.40346237218, 7.1260656844, 7.83907559509}};
+ spline.spline_y = {{3.43030859063, 3.44230565037, 2.8824369646, 2.81000389973,
+ 3.08853311072, 2.6933085577}};
+
+ spline.constraints = {{lateral_constraint, velocity_constraint,
+ voltage_constraint, longitudinal_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from the far side of the rocket to close to the loading station
+::frc971::MultiSpline AutonomousSplines::FarRocketToNearHP(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint constraints;
+
+ // TODO(theo): Add some real constraints.
+ constraints.constraint_type = 0;
+ constraints.value = 0;
+ constraints.start_distance = 0;
+ constraints.end_distance = 0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{6.51652191988, 6.83156293562, 5.74513904409, 2.2337653586,
+ 1.94766705864, 0.727526876557}};
+ spline.spline_y = {{3.2465107468, 2.88277456846, 1.93458779243, 3.44064777429,
+ 3.44377880106, 3.43326367284}};
+
+ spline.constraints = {{constraints}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from level 2 to 2nd cargo ship bay with a hatch panel
+::frc971::MultiSpline AutonomousSplines::HABToSecondCargoShipBay(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint constraints;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint voltage_constraint;
+
+ constraints.constraint_type = 4;
+ constraints.value = 1.6;
+ constraints.start_distance = 4.0;
+ constraints.end_distance = 10.0;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 2.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 10.0;
+
+ spline.spline_count = 1;
+ constexpr double kLess = 0.06;
+ spline.spline_x = {{1.0, 2.53944573074, 5.75526086906, 6.52583747973 - kLess,
+ 7.12318661548 - kLess, 7.22595029399 - kLess}};
+ spline.spline_y = {{1.5, 1.48, 2.05178220103,
+ 2.56666687655, 1.79340280288, 1.16170693058}};
+
+ spline.constraints = {{constraints, longitudinal_constraint, voltage_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from 2nd cargo ship bay to loading station
+::frc971::MultiSpline AutonomousSplines::SecondCargoShipBayToHP(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint constraints;
+ ::frc971::Constraint voltage_constraint;
+
+ constraints.constraint_type = 4;
+ constraints.value = 4.0;
+ constraints.start_distance = 0;
+ constraints.end_distance = 10;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{7.22595029399, 7.1892447864, 6.5373977907, 5.55997590982,
+ 1.22953437637, 0.32521840905}};
+ constexpr double kYShift = 0.1;
+ spline.spline_y = {{1.2, 1.44543230529, 2.00646674662,
+ 3.43762336271 - kYShift, 3.44125430793 - kYShift,
+ 3.4360348159 - kYShift}};
+
+ spline.constraints = {{constraints, voltage_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from loading station to 3rd cargo ship bay with a hatch panel
+::frc971::MultiSpline AutonomousSplines::HPToThirdCargoShipBay(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint velocity_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint2;
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 3.5;
+ velocity_constraint.start_distance = 0;
+ velocity_constraint.end_distance = 10;
+
+ velocity_constraint2.constraint_type = 4;
+ velocity_constraint2.value = 2.0;
+ velocity_constraint2.start_distance = 6;
+ velocity_constraint2.end_distance = 10;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 10.0;
+
+ spline.spline_count = 1;
+ constexpr double kEndMove = 0.25;
+ spline.spline_x = {{0.75, 1.112, 5.576, 7.497 - kEndMove, 7.675 - kEndMove,
+ 7.768 - kEndMove}};
+ spline.spline_y = {{3.431, 3.434, 2.712, 2.874, 1.786, 1.168}};
+
+ spline.constraints = {
+ {velocity_constraint, voltage_constraint, velocity_constraint2}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+// Path from 3rd cargo ship bay to near the loading station
+::frc971::MultiSpline AutonomousSplines::ThirdCargoShipBayToNearHP(
+ bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint velocity_constraint;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 0;
+ velocity_constraint.end_distance = 10;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{7.75823205276, 7.58356294646, 5.95536035287,
+ 2.12377989323, 1.29347361128, 0.598613577531}};
+ spline.spline_y = {{1.16791407107, 1.94564064915, 2.54565614767,
+ 3.43728005786, 3.43775494434, 3.43119598027}};
+
+ spline.constraints = {{velocity_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::HabToFarRocketTest(bool is_left) {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 2.0;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 2.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 1.7;
+ velocity_constraint.start_distance = 0.0;
+ velocity_constraint.end_distance = 0.8;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{1.14763818102, 2.53944573074, 3.74586892131,
+ 5.22352745444, 6.70255737219, 7.35784750785}};
+ spline.spline_y = {{1.30261224364, 1.28295363394, 1.27450357714,
+ 2.89953366429, 3.10734391012, 2.90125929705}};
+
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+
+ MaybeFlipSpline(&spline, is_left);
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::FarRocketToHPTest() {
::frc971::MultiSpline spline;
::frc971::Constraint longitudinal_constraint;
::frc971::Constraint lateral_constraint;
@@ -54,18 +305,19 @@
velocity_constraint.constraint_type = 4;
velocity_constraint.value = 0.5;
- velocity_constraint.start_distance = 7.5;
+ velocity_constraint.start_distance = 9.5;
velocity_constraint.end_distance = 10.0;
spline.spline_count = 1;
spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
+
spline.constraints = {{longitudinal_constraint, lateral_constraint,
voltage_constraint, velocity_constraint}};
return spline;
}
-::frc971::MultiSpline AutonomousSplines::HPToNearRocket() {
+::frc971::MultiSpline AutonomousSplines::HPToNearRocketTest() {
::frc971::MultiSpline spline;
::frc971::Constraint longitudinal_constraint;
::frc971::Constraint lateral_constraint;
@@ -89,6 +341,7 @@
spline.spline_count = 1;
spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
+
spline.constraints = {{longitudinal_constraint, lateral_constraint,
voltage_constraint, velocity_constraint}};
return spline;
@@ -116,8 +369,10 @@
0.4f + startx, 0.4f + startx, 1.0f + startx}};
spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f,
starty - 0.7f, starty - 1.0f, starty - 1.0f}};
+
spline.constraints = {
{longitudinal_constraint, lateral_constraint, voltage_constraint}};
+
return spline;
}
@@ -133,7 +388,9 @@
spline.spline_count = 1;
spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}};
spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}};
+
spline.constraints = {{contraints}};
+
return spline;
}
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index d093b99..a55c2f1 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -14,20 +14,44 @@
class AutonomousSplines {
public:
- // A spline that does an 's' cause that's what he wanted.
+ // Splines for 2 Panels on the far side of the Rocket
+
+ // Path off of level 2 to the far side of the rocket with a panel
+ static ::frc971::MultiSpline HABToFarRocket(bool is_left);
+
+ // Path from the far side of the rocket to the loading station to pickup
+ static ::frc971::MultiSpline FarRocketToHP(bool is_left);
+
+ // Path from the far side of the rocket to the loading station to pickup
+ static ::frc971::MultiSpline HPToFarRocket(bool is_left);
+
+ // Path from the far side of the rocket to close to the loading station
+ static ::frc971::MultiSpline FarRocketToNearHP(bool is_left);
+
+ // Splines for 2 Panels on the far reaches of the cargo ship
+
+ // Path from level 2 to 2nd cargo ship bay with a hatch panel
+ static ::frc971::MultiSpline HABToSecondCargoShipBay(bool is_left);
+
+ // Path from 2nd cargo ship bay to loading station
+ static ::frc971::MultiSpline SecondCargoShipBayToHP(bool is_left);
+
+ // Path from loading station to 3rd cargo ship bay with a hatch panel
+ static ::frc971::MultiSpline HPToThirdCargoShipBay(bool is_left);
+
+ // Path from 3rd cargo ship bay to near the loading station
+ static ::frc971::MultiSpline ThirdCargoShipBayToNearHP(bool is_left);
+
+ // Testing Splines
+ static ::frc971::MultiSpline HPToNearRocketTest();
+ static ::frc971::MultiSpline HabToFarRocketTest(bool is_left);
+ static ::frc971::MultiSpline FarRocketToHPTest();
+
static ::frc971::MultiSpline BasicSSpline();
-
- // Straight
static ::frc971::MultiSpline StraightLine();
-
- // HP to near side rocket
- static ::frc971::MultiSpline HPToNearRocket();
-
- static ::frc971::MultiSpline HabToFarRocket();
- static ::frc971::MultiSpline FarRockettoHP();
};
} // namespace actors
} // namespace y2019
-#endif // Y2019_ACTORS_AUTO_SPLINES_H_
+#endif // Y2019_ACTORS_AUTO_SPLINES_H_
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 41a70fd..a565764 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -34,6 +34,42 @@
: frc971::autonomous::BaseAutonomousActor(
s, control_loops::drivetrain::GetDrivetrainConfig()) {}
+bool AutonomousActor::WaitForDriveXGreater(double x) {
+ LOG(INFO, "Waiting until x > %f\n", x);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (drivetrain_queue.status->x > x) {
+ LOG(INFO, "X at %f\n", drivetrain_queue.status->x);
+ return true;
+ }
+ }
+}
+
+bool AutonomousActor::WaitForDriveYCloseToZero(double y) {
+ LOG(INFO, "Waiting until |y| < %f\n", y);
+ ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
+ ::std::chrono::milliseconds(5) / 2);
+
+ while (true) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ phased_loop.SleepUntilNext();
+ drivetrain_queue.status.FetchLatest();
+ if (::std::abs(drivetrain_queue.status->y) < y) {
+ LOG(INFO, "Y at %f\n", drivetrain_queue.status->y);
+ return true;
+ }
+ }
+}
+
void AutonomousActor::Reset(bool is_left) {
const double turn_scalar = is_left ? 1.0 : -1.0;
elevator_goal_ = 0.01;
@@ -55,9 +91,9 @@
auto localizer_resetter = localizer_control.MakeMessage();
// Start on the left l2.
localizer_resetter->x = 1.0;
- localizer_resetter->y = 1.5 * turn_scalar;
+ localizer_resetter->y = 1.35 * turn_scalar;
localizer_resetter->theta = M_PI;
- localizer_resetter->theta_uncertainty = 0.0000001;
+ localizer_resetter->theta_uncertainty = 0.00001;
if (!localizer_resetter.Send()) {
LOG(ERROR, "Failed to reset localizer.\n");
}
@@ -80,54 +116,240 @@
const ProfileParameters kDrive = {4.0, 3.0};
const ProfileParameters kTurn = {5.0, 15.0};
+const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.01, M_PI / 2.0};
+const ElevatorWristPosition kPanelHPIntakeBackwardPos{0.015, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};
+
+const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
+
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
- monotonic_clock::time_point start_time = monotonic_clock::now();
+ const monotonic_clock::time_point start_time = monotonic_clock::now();
const bool is_left = params.mode == 0;
{
LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
params.mode, is_left ? "left" : "right");
}
+
const double turn_scalar = is_left ? 1.0 : -1.0;
Reset(is_left);
+ enum class Mode { kTesting, kRocket, kCargoship };
+ Mode mode = Mode::kCargoship;
+ if (mode == Mode::kRocket) {
+ SplineHandle spline1 =
+ PlanSpline(AutonomousSplines::HabToFarRocketTest(is_left),
+ SplineDirection::kBackward);
- // Grab the disk, wait until we have vacuum, then jump
- set_elevator_goal(0.01);
- set_wrist_goal(-M_PI / 2.0);
- set_intake_goal(-1.2);
- set_suction_goal(true, 1);
- SendSuperstructureGoal();
+ // Grab the disk, jump, wait until we have vacuum, then raise the elevator
+ set_elevator_goal(0.010);
+ set_wrist_goal(-M_PI / 2.0);
+ set_intake_goal(-1.2);
+ set_suction_goal(true, 1);
+ SendSuperstructureGoal();
- if (!WaitForGamePiece()) return true;
- LOG(INFO, "Has game piece\n");
+ // if planned start the spline and plan the next
+ if (!spline1.WaitForPlan()) return true;
+ LOG(INFO, "Planned\n");
+ spline1.Start();
- StartDrive(-4.0, 0.0, kJumpDrive, kTurn);
- if (!WaitForDriveNear(3.3, 10.0)) return true;
- LOG(INFO, "Lifting\n");
- set_elevator_goal(0.30);
- SendSuperstructureGoal();
+ // If suction, move the superstructure to score
+ if (!WaitForGamePiece()) return true;
+ LOG(INFO, "Has game piece\n");
+ if (!spline1.WaitForSplineDistanceRemaining(3.5)) return true;
+ set_elevator_wrist_goal(kPanelBackwardMiddlePos);
+ SendSuperstructureGoal();
- if (!WaitForDriveNear(2.8, 10.0)) return true;
- LOG(INFO, "Off the platform\n");
+ if (!spline1.WaitForSplineDistanceRemaining(2.0)) return true;
+ set_elevator_wrist_goal(kPanelForwardMiddlePos);
+ SendSuperstructureGoal();
- StartDrive(0.0, 1.00 * turn_scalar, kDrive, kTurn);
- LOG(INFO, "Turn started\n");
- if (!WaitForSuperstructureDone()) return true;
- LOG(INFO, "Superstructure done\n");
+ // END SPLINE 1
- if (!WaitForDriveNear(0.7, 10.0)) return true;
- StartDrive(0.0, -0.35 * turn_scalar, kDrive, kTurn);
+ if (!spline1.WaitForSplineDistanceRemaining(0.2)) return true;
+ LineFollowAtVelocity(1.3, 4);
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1200))) return true;
- LOG(INFO, "Elevator up\n");
- set_elevator_goal(0.78);
- SendSuperstructureGoal();
+ set_suction_goal(false, 1);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
+ LineFollowAtVelocity(-1.0, 4);
+ SplineHandle spline2 = PlanSpline(AutonomousSplines::FarRocketToHP(is_left),
+ SplineDirection::kBackward);
- if (!WaitForDriveDone()) return true;
- LOG(INFO, "Done driving\n");
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
+ if (!spline2.WaitForPlan()) return true;
+ LOG(INFO, "Planned\n");
+ // Drive back to hp and set the superstructure accordingly
+ spline2.Start();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
+ set_elevator_wrist_goal(kPanelHPIntakeBackwardPos);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
+ set_suction_goal(true, 1);
+ SendSuperstructureGoal();
- if (!WaitForSuperstructureDone()) return true;
+ if (!spline2.WaitForSplineDistanceRemaining(1.6)) return true;
+ LineFollowAtVelocity(-1.6);
+
+ // As soon as we pick up Panel 2 go score on the back rocket
+ if (!WaitForGamePiece()) return true;
+ LineFollowAtVelocity(1.5);
+ SplineHandle spline3 = PlanSpline(AutonomousSplines::HPToFarRocket(is_left),
+ SplineDirection::kForward);
+ if (!WaitForDriveXGreater(0.50)) return true;
+ if (!spline3.WaitForPlan()) return true;
+ spline3.Start();
+ LOG(INFO, "Has game piece\n");
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
+ set_elevator_wrist_goal(kPanelBackwardMiddlePos);
+ SendSuperstructureGoal();
+ if (!WaitForDriveXGreater(7.1)) return true;
+ LineFollowAtVelocity(-1.5, 4);
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
+ set_elevator_wrist_goal(kPanelBackwardUpperPos);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1500))) return true;
+ set_suction_goal(false, 1);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
+ LineFollowAtVelocity(1.0, 4);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
+ } else if (mode == Mode::kCargoship) {
+ SplineHandle spline1 =
+ PlanSpline(AutonomousSplines::HABToSecondCargoShipBay(is_left),
+ SplineDirection::kBackward);
+ set_elevator_goal(0.01);
+ set_wrist_goal(-M_PI / 2.0);
+ set_intake_goal(-1.2);
+ set_suction_goal(true, 1);
+ SendSuperstructureGoal();
+
+ // if planned start the spline and plan the next
+ if (!spline1.WaitForPlan()) return true;
+ LOG(INFO, "Planned\n");
+ spline1.Start();
+
+ // If suction, move the superstructure to score
+ if (!WaitForGamePiece()) return true;
+ LOG(INFO, "Has game piece\n");
+ // unstick the hatch panel
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
+ set_elevator_goal(0.5);
+ set_wrist_goal(-M_PI / 2.0);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
+ set_elevator_wrist_goal(kPanelCargoBackwardPos);
+ SendSuperstructureGoal();
+
+ if (!spline1.WaitForSplineDistanceRemaining(0.8)) return true;
+ // Line follow in to the first disc.
+ LineFollowAtVelocity(-0.9, 2);
+ if (!WaitForDriveYCloseToZero(1.2)) return true;
+
+ set_suction_goal(false, 1);
+ SendSuperstructureGoal();
+ LOG(INFO, "Dropping disc 1 %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ if (!WaitForDriveYCloseToZero(1.13)) return true;
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
+
+ LineFollowAtVelocity(0.9, 2);
+ SplineHandle spline2 =
+ PlanSpline(AutonomousSplines::SecondCargoShipBayToHP(is_left),
+ SplineDirection::kForward);
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
+ if (!spline2.WaitForPlan()) return true;
+ LOG(INFO, "Planned\n");
+ // Drive back to hp and set the superstructure accordingly
+ spline2.Start();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
+ set_elevator_wrist_goal(kPanelHPIntakeForwrdPos);
+ SendSuperstructureGoal();
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
+ set_suction_goal(true, 1);
+ SendSuperstructureGoal();
+
+ if (!spline2.WaitForSplineDistanceRemaining(1.75)) return true;
+ LineFollowAtVelocity(1.5);
+ // As soon as we pick up Panel 2 go score on the rocket
+ if (!WaitForGamePiece()) return true;
+ LOG(INFO, "Got gamepiece %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ LineFollowAtVelocity(-4.0);
+ SplineHandle spline3 =
+ PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
+ SplineDirection::kBackward);
+ if (!WaitForDriveXGreater(0.55)) return true;
+ if (!spline3.WaitForPlan()) return true;
+ spline3.Start();
+ // Wait until we are a bit out to lift.
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
+ set_elevator_wrist_goal(kPanelCargoBackwardPos);
+ SendSuperstructureGoal();
+
+ if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
+ // Line follow in to the second disc.
+ LineFollowAtVelocity(-0.7, 3);
+ LOG(INFO, "Drawing in disc 2 %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ if (!WaitForDriveYCloseToZero(1.2)) return true;
+
+ set_suction_goal(false, 1);
+ SendSuperstructureGoal();
+ LOG(INFO, "Dropping disc 2 %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+
+ if (!WaitForDriveYCloseToZero(1.13)) return true;
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
+ LOG(INFO, "Backing up %f\n",
+ DoubleSeconds(monotonic_clock::now() - start_time));
+ LineFollowAtVelocity(0.9, 3);
+ if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
+ } else {
+ // Grab the disk, wait until we have vacuum, then jump
+ set_elevator_goal(0.01);
+ set_wrist_goal(-M_PI / 2.0);
+ set_intake_goal(-1.2);
+ set_suction_goal(true, 1);
+ SendSuperstructureGoal();
+
+ if (!WaitForGamePiece()) return true;
+ LOG(INFO, "Has game piece\n");
+
+ StartDrive(-4.0, 0.0, kJumpDrive, kTurn);
+ if (!WaitForDriveNear(3.3, 10.0)) return true;
+ LOG(INFO, "Lifting\n");
+ set_elevator_goal(0.30);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveNear(2.8, 10.0)) return true;
+ LOG(INFO, "Off the platform\n");
+
+ StartDrive(0.0, 1.00 * turn_scalar, kDrive, kTurn);
+ LOG(INFO, "Turn started\n");
+ if (!WaitForSuperstructureDone()) return true;
+ LOG(INFO, "Superstructure done\n");
+
+ if (!WaitForDriveNear(0.7, 10.0)) return true;
+ StartDrive(0.0, -0.35 * turn_scalar, kDrive, kTurn);
+
+ LOG(INFO, "Elevator up\n");
+ set_elevator_goal(0.78);
+ SendSuperstructureGoal();
+
+ if (!WaitForDriveDone()) return true;
+ LOG(INFO, "Done driving\n");
+
+ if (!WaitForSuperstructureDone()) return true;
+ }
LOG(INFO, "Done %f\n", DoubleSeconds(monotonic_clock::now() - start_time));
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 38db070..e25085a 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -18,6 +18,11 @@
using ::frc971::ProfileParameters;
using ::y2019::control_loops::superstructure::superstructure_queue;
+struct ElevatorWristPosition {
+ double elevator;
+ double wrist;
+};
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::frc971::autonomous::AutonomousActionQueueGroup *s);
@@ -64,6 +69,11 @@
wrist_max_acceleration_ = wrist_max_acceleration;
}
+ void set_elevator_wrist_goal(ElevatorWristPosition goal) {
+ set_elevator_goal(goal.elevator);
+ set_wrist_goal(goal.wrist);
+ }
+
void SendSuperstructureGoal() {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->elevator.unsafe_goal = elevator_goal_;
@@ -112,6 +122,20 @@
}
}
+ bool WaitForMilliseconds(std::chrono::milliseconds wait) {
+ ::aos::monotonic_clock::time_point end_time =
+ ::aos::monotonic_clock::now() + wait;
+
+ while (::aos::monotonic_clock::now() < end_time) {
+ if (ShouldCancel()) {
+ return false;
+ }
+ // TODO(james): Allow non-multiples of 5.
+ ::std::this_thread::sleep_for(::std::chrono::milliseconds(5));
+ }
+ return true;
+ }
+
bool IsSuperstructureDone() {
superstructure_queue.status.FetchLatest();
@@ -150,6 +174,12 @@
}
}
}
+
+ // Waits until the robot's x > x.
+ bool WaitForDriveXGreater(double x);
+
+ // Waits until y is within y of zero.
+ bool WaitForDriveYCloseToZero(double y);
};
} // namespace actors