Add a couple more splines and tweak auto spline
Change-Id: I726d318a9b05693d1e6f9ecaa5d9cdce5ed69a53
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index f316037..57aba4e 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -5,11 +5,72 @@
namespace y2019 {
namespace actors {
+::frc971::MultiSpline AutonomousSplines::HabToFarRocket() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.5;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ // Note: This velocity constraint is currently too late in the spline to
+ // actually do anything.
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 7.5;
+ velocity_constraint.end_distance = 10.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{1.0, 2.0, 4.0, 7.8, 7.8, 6.53}};
+ spline.spline_y = {{1.5, 1.5, 1.5, 1.5, 3.0, 3.47}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::FarRockettoHP() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.5;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 7.5;
+ velocity_constraint.end_distance = 10.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
+ spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+ return spline;
+}
+
::frc971::MultiSpline AutonomousSplines::HPToNearRocket() {
::frc971::MultiSpline spline;
::frc971::Constraint longitudinal_constraint;
::frc971::Constraint lateral_constraint;
::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
longitudinal_constraint.constraint_type = 1;
longitudinal_constraint.value = 1.0;
@@ -18,13 +79,18 @@
lateral_constraint.value = 1.0;
voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 12.0;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 2.7;
+ velocity_constraint.end_distance = 10.0;
spline.spline_count = 1;
- spline.spline_x = {{0.4, 1.0, 3.0, 4.0, 4.5, 5.05}};
- spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.5}};
- spline.constraints = {
- {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
+ spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
return spline;
}
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index 7e79d1f..d093b99 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -22,6 +22,9 @@
// HP to near side rocket
static ::frc971::MultiSpline HPToNearRocket();
+
+ static ::frc971::MultiSpline HabToFarRocket();
+ static ::frc971::MultiSpline FarRockettoHP();
};
} // namespace actors