Add champs autos
This adds the cargo ship auto as well as a rocket auto.
Change-Id: I81bd90de4ca0b1615f7f163c93d2854f4f5dd0c5
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