Merge "Add Barrel and Slalom autonav splines"
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 4223332..c2fddcc 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -179,5 +179,40 @@
StartDrive(1.0, 0.0, kDrive, kTurn);
return WaitForDriveDone();
}
+
+void AutonomousActor::SendSuperstructureGoal() {
+
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ intake_offset;
+
+ {
+ StaticZeroingSingleDOFProfiledSubsystemGoal::Builder intake_builder =
+ builder.MakeBuilder<StaticZeroingSingleDOFProfiledSubsystemGoal>();
+
+ frc971::ProfileParameters::Builder profile_params_builder =
+ builder.MakeBuilder<frc971::ProfileParameters>();
+ profile_params_builder.add_max_velocity(0.0);
+ profile_params_builder.add_max_acceleration(0.0);
+ flatbuffers::Offset<frc971::ProfileParameters> profile_params_offset =
+ profile_params_builder.Finish();
+ intake_builder.add_unsafe_goal(intake_goal_);
+ intake_builder.add_profile_params(profile_params_offset);
+ intake_offset = intake_builder.Finish();
+ }
+
+ superstructure::Goal::Builder superstructure_builder =
+ builder.MakeBuilder<superstructure::Goal>();
+
+ superstructure_builder.add_intake(intake_offset);
+ superstructure_builder.add_roller_voltage(roller_voltage_);
+ superstructure_builder.add_roller_speed_compensation(kRollerSpeedCompensation);
+
+ if (!builder.Send(superstructure_builder.Finish())) {
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
+ }
+
+}
} // namespace actors
} // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index 12b8e77..feba84d 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -9,10 +9,16 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2020/actors/auto_splines.h"
#include "y2020/vision/galactic_search_path_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
namespace y2020 {
namespace actors {
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+
+namespace superstructure = y2020::control_loops::superstructure;
+
class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
public:
explicit AutonomousActor(::aos::EventLoop *event_loop);
@@ -22,14 +28,27 @@
private:
void Reset();
+
+ void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
+ void set_roller_voltage(double roller_voltage) {
+ roller_voltage_ = roller_voltage;
+ }
+
+ void SendSuperstructureGoal();
void SplineAuto();
void SendStartingPosition(double x, double y, double theta);
void SendStartingPosition(const frc971::MultiSpline *const spline);
void GalacticSearch();
bool DriveFwd();
+ double intake_goal_ = 0.0;
+ double roller_voltage_ = 0.0;
+ const float kRollerSpeedCompensation = 2.0;
+
::aos::Sender<::frc971::control_loops::drivetrain::LocalizerControl>
localizer_control_sender_;
+ ::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::Alliance alliance_ = aos::Alliance::kInvalid;