commit d0e9e06d6ded4d05f328e5ff2e186ba0ab4c3669
author Austin Schuh <austin.linux@gmail.com> Sun Oct 24 17:40:58 2021 -0700
committer Austin Schuh <austin.linux@gmail.com> Sat Nov 06 15:39:57 2021 -0700
tree 232c7e0a9c00b965014e406bda0ad8aba4d4b604
parent 9054a22dcf45b4ad5cb708812be8c59b07aa1bec

Add a 6 ball auto.

This has run 10 times in a row.

Change-Id: I6ffe32e99251d17a2e19c371132d1e2371f06dbb
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2020/actors/auto_splines.cc

diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 00e3bd0..77cfbb5 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -5,6 +5,9 @@
 namespace y2020 {
 namespace actors {
 
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+
 void MaybeFlipSpline(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder,
@@ -20,6 +23,34 @@
   }
 }
 
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+    aos::Alliance alliance) {
+  frc971::MultiSpline *spline =
+      GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+  flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+  flatbuffers::Vector<float> *spline_y = spline->mutable_spline_y();
+
+  for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+    spline_x->Mutate(ii, spline_x->Get(ii) - kFieldLength / 2.0);
+  }
+  for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+    spline_y->Mutate(ii, kFieldWidth / 2.0 - spline_y->Get(ii));
+  }
+
+  if (alliance == aos::Alliance::kBlue) {
+    for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+      spline_x->Mutate(ii, -spline_x->Get(ii));
+    }
+    for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+      spline_y->Mutate(ii, -spline_y->Get(ii));
+    }
+  }
+  return spline_offset;
+}
+
 flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder,
@@ -90,6 +121,26 @@
   return multispline_builder.Finish();
 }
 
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned1(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  return FixSpline(builder,
+                   aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_1_,
+                                                            builder->fbb()),
+                   alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned2(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  return FixSpline(builder,
+                   aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_2_,
+                                                            builder->fbb()),
+                   alliance);
+}
+
 flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder) {