commit ddd2ba65e584ac2529f1546b01f3d28420b63284
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Mar 08 22:17:13 2020 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Mar 15 15:53:02 2020 -0700
tree 65f224c3e8fe15fe2d61b698bb37489fbe8a8273
parent 387b7de2a3f21654d7f8211b771ac24031b722ba

Add alliance-switching to 2020 auto

Add code to automatically switch the side of the field we initialize to
based on the alliance reported by the driver's station (this can be set
from the driver's station when not connected to the FMS).

Change-Id: I8c7576e451a0f0762b29f2fe8c407791900c4727

y2020/actors/auto_splines.cc

diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 3498a3e..b6d97c4 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -20,7 +20,8 @@
 }
 
 flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
-    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+    aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
+    aos::Alliance alliance) {
   flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
   flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
   flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -30,7 +31,7 @@
         builder->MakeBuilder<frc971::Constraint>();
     longitudinal_constraint_builder.add_constraint_type(
         frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
-    longitudinal_constraint_builder.add_value(1.0);
+    longitudinal_constraint_builder.add_value(2.0);
     longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
   }
 
@@ -39,7 +40,7 @@
         builder->MakeBuilder<frc971::Constraint>();
     lateral_constraint_builder.add_constraint_type(
         frc971::ConstraintType::LATERAL_ACCELERATION);
-    lateral_constraint_builder.add_value(1.0);
+    lateral_constraint_builder.add_value(2.0);
     lateral_constraint_offset = lateral_constraint_builder.Finish();
   }
 
@@ -48,7 +49,7 @@
         builder->MakeBuilder<frc971::Constraint>();
     voltage_constraint_builder.add_constraint_type(
         frc971::ConstraintType::VOLTAGE);
-    voltage_constraint_builder.add_value(6.0);
+    voltage_constraint_builder.add_value(8.0);
     voltage_constraint_offset = voltage_constraint_builder.Finish();
   }
 
@@ -59,16 +60,22 @@
               {longitudinal_constraint_offset, lateral_constraint_offset,
                voltage_constraint_offset});
 
-  const float startx = 0.0;
-  const float starty = 0.05;
+  const float startx = 3.3;
+  const float starty = -.7;
+  std::vector<float> x_pos{0.0f + startx, 0.8f + startx, 0.8f + startx,
+                           1.2f + startx, 1.2f + startx, 2.0f + startx};
+  std::vector<float> y_pos{starty - 0.0f, starty - 0.0f, starty - 0.1f,
+                           starty - 0.2f, starty - 0.3f, starty - 0.3f};
+  if (alliance == aos::Alliance::kRed) {
+    for (size_t ii = 0; ii < x_pos.size(); ++ii) {
+      x_pos[ii] *= -1;
+      y_pos[ii] *= -1;
+    }
+  }
   flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
-      builder->fbb()->CreateVector<float>({0.0f + startx, 0.8f + startx,
-                                           0.8f + startx, 1.2f + startx,
-                                           1.2f + startx, 2.0f + startx});
+      builder->fbb()->CreateVector<float>(x_pos);
   flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
-      builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
-                                           starty - 0.1f, starty - 0.2f,
-                                           starty - 0.3f, starty - 0.3f});
+      builder->fbb()->CreateVector<float>(y_pos);
 
   frc971::MultiSpline::Builder multispline_builder =
       builder->MakeBuilder<frc971::MultiSpline>();