Add profile to 2022 aimer turret goal
Change-Id: I7a6b76c05e3c14e22e0fb90ea5019491227de85a
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 4c0309c..2a600d8 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -22,13 +22,22 @@
// straight out the back of the robot.
constexpr double kTurretZeroOffset = M_PI;
+constexpr double kMaxProfiledVelocity = 10.0;
+constexpr double kMaxProfiledAccel = 20.0;
+
flatbuffers::DetachedBuffer MakePrefilledGoal() {
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
+ frc971::ProfileParameters::Builder profile_builder(fbb);
+ profile_builder.add_max_velocity(kMaxProfiledVelocity);
+ profile_builder.add_max_acceleration(kMaxProfiledAccel);
+ const flatbuffers::Offset<frc971::ProfileParameters> profile_offset =
+ profile_builder.Finish();
Aimer::Goal::Builder builder(fbb);
builder.add_unsafe_goal(0);
builder.add_goal_velocity(0);
- builder.add_ignore_profile(true);
+ builder.add_ignore_profile(false);
+ builder.add_profile_params(profile_offset);
fbb.Finish(builder.Finish());
return fbb.Release();
}