Pre-plan auto splines
(a) Make it so that the drivetrain automatically evicts old splines
(b) Set up auto to preplan splines at construction and after every auto.
Change-Id: I96ddb3a38947da02ad9ddc6fe933b7e85727dc18
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index b22ab36..7bdb44b 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -15,16 +15,16 @@
namespace y2020 {
namespace actors {
-using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
namespace superstructure = y2020::control_loops::superstructure;
-class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+class AutonomousActor : public frc971::autonomous::BaseAutonomousActor {
public:
- explicit AutonomousActor(::aos::EventLoop *event_loop);
+ explicit AutonomousActor(aos::EventLoop *event_loop);
bool RunAction(
- const ::frc971::autonomous::AutonomousActionParams *params) override;
+ const frc971::autonomous::AutonomousActionParams *params) override;
private:
void Reset();
@@ -37,24 +37,42 @@
void SendSuperstructureGoal();
void RetractIntake();
void SplineAuto();
- void SendStartingPosition(double x, double y, double theta);
- void SendStartingPosition(const frc971::MultiSpline *const spline);
+ void SendStartingPosition(const Eigen::Vector3d &start);
void GalacticSearch();
void AutoNavBounce();
void AutoNavBarrel();
void AutoNavSlalom();
bool DriveFwd();
+ void Replan();
+
double intake_goal_ = 0.0;
double roller_voltage_ = 0.0;
const float kRollerSpeedCompensation = 2.0;
- ::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
- ::aos::Sender<::y2020::control_loops::superstructure::Goal>
+ aos::Sender<y2020::control_loops::superstructure::Goal>
superstructure_goal_sender_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<y2020::vision::GalacticSearchPath> path_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+
+ std::optional<std::array<SplineHandle, 4>> bounce_splines_;
+
+ struct GalacticSearchSplines {
+ SplineHandle red_a;
+ SplineHandle red_b;
+ SplineHandle blue_a;
+ SplineHandle blue_b;
+ };
+ std::optional<GalacticSearchSplines> galactic_search_splines_;
+
+ std::optional<SplineHandle> barrel_spline_;
+ std::optional<SplineHandle> slalom_spline_;
+ std::optional<SplineHandle> test_spline_;
+
aos::Alliance alliance_ = aos::Alliance::kInvalid;
AutonomousSplines auto_splines_;
};