| Austin Schuh | 6bcc230 | 2019-03-23 22:28:06 -0700 | [diff] [blame^] | 1 | #include "y2019/actors/auto_splines.h" |
| 2 | |
| 3 | #include "frc971/control_loops/control_loops.q.h" |
| 4 | |
| 5 | namespace y2019 { |
| 6 | namespace actors { |
| 7 | |
| 8 | ::frc971::MultiSpline AutonomousSplines::HPToNearRocket() { |
| 9 | ::frc971::MultiSpline spline; |
| 10 | ::frc971::Constraint longitudinal_constraint; |
| 11 | ::frc971::Constraint lateral_constraint; |
| 12 | ::frc971::Constraint voltage_constraint; |
| 13 | |
| 14 | longitudinal_constraint.constraint_type = 1; |
| 15 | longitudinal_constraint.value = 1.0; |
| 16 | |
| 17 | lateral_constraint.constraint_type = 2; |
| 18 | lateral_constraint.value = 1.0; |
| 19 | |
| 20 | voltage_constraint.constraint_type = 3; |
| 21 | voltage_constraint.value = 12.0; |
| 22 | |
| 23 | spline.spline_count = 1; |
| 24 | spline.spline_x = {{0.4, 1.0, 3.0, 4.0, 4.5, 5.05}}; |
| 25 | spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.5}}; |
| 26 | spline.constraints = { |
| 27 | {longitudinal_constraint, lateral_constraint, voltage_constraint}}; |
| 28 | return spline; |
| 29 | } |
| 30 | |
| 31 | ::frc971::MultiSpline AutonomousSplines::BasicSSpline() { |
| 32 | ::frc971::MultiSpline spline; |
| 33 | ::frc971::Constraint longitudinal_constraint; |
| 34 | ::frc971::Constraint lateral_constraint; |
| 35 | ::frc971::Constraint voltage_constraint; |
| 36 | |
| 37 | longitudinal_constraint.constraint_type = 1; |
| 38 | longitudinal_constraint.value = 1.0; |
| 39 | |
| 40 | lateral_constraint.constraint_type = 2; |
| 41 | lateral_constraint.value = 1.0; |
| 42 | |
| 43 | voltage_constraint.constraint_type = 3; |
| 44 | voltage_constraint.value = 6.0; |
| 45 | |
| 46 | spline.spline_count = 1; |
| 47 | const float startx = 0.4; |
| 48 | const float starty = 3.4; |
| 49 | spline.spline_x = {{0.0f + startx, 0.6f + startx, 0.6f + startx, |
| 50 | 0.4f + startx, 0.4f + startx, 1.0f + startx}}; |
| 51 | spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f, |
| 52 | starty - 0.7f, starty - 1.0f, starty - 1.0f}}; |
| 53 | spline.constraints = { |
| 54 | {longitudinal_constraint, lateral_constraint, voltage_constraint}}; |
| 55 | return spline; |
| 56 | } |
| 57 | |
| 58 | ::frc971::MultiSpline AutonomousSplines::StraightLine() { |
| 59 | ::frc971::MultiSpline spline; |
| 60 | ::frc971::Constraint contraints; |
| 61 | |
| 62 | contraints.constraint_type = 0; |
| 63 | contraints.value = 0.0; |
| 64 | contraints.start_distance = 0.0; |
| 65 | contraints.end_distance = 0.0; |
| 66 | |
| 67 | spline.spline_count = 1; |
| 68 | spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}}; |
| 69 | spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}}; |
| 70 | spline.constraints = {{contraints}}; |
| 71 | return spline; |
| 72 | } |
| 73 | |
| 74 | } // namespace actors |
| 75 | } // namespace y2019 |