Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / bc837879ce688a1eca69ff316f3ce3a6445ade4e / . / y2019 / actors / auto_splines.cc
blob: 57aba4e99f073a7eba5ab2953a79c4b11bcd6e36 [file] [log] [blame]
Austin Schuh6bcc2302019-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
5namespace y2019 {
6namespace actors {
7
James Kuszmaulbc837872019-04-07 12:01:47 -0700[diff] [blame^]8::frc971::MultiSpline AutonomousSplines::HabToFarRocket() {
9 ::frc971::MultiSpline spline;
10 ::frc971::Constraint longitudinal_constraint;
11 ::frc971::Constraint lateral_constraint;
12 ::frc971::Constraint voltage_constraint;
13 ::frc971::Constraint velocity_constraint;
14
15 longitudinal_constraint.constraint_type = 1;
16 longitudinal_constraint.value = 1.5;
17
18 lateral_constraint.constraint_type = 2;
19 lateral_constraint.value = 1.0;
20
21 voltage_constraint.constraint_type = 3;
22 voltage_constraint.value = 11.0;
23
24 // Note: This velocity constraint is currently too late in the spline to
25 // actually do anything.
26 velocity_constraint.constraint_type = 4;
27 velocity_constraint.value = 0.5;
28 velocity_constraint.start_distance = 7.5;
29 velocity_constraint.end_distance = 10.0;
30
31 spline.spline_count = 1;
32 spline.spline_x = {{1.0, 2.0, 4.0, 7.8, 7.8, 6.53}};
33 spline.spline_y = {{1.5, 1.5, 1.5, 1.5, 3.0, 3.47}};
34 spline.constraints = {{longitudinal_constraint, lateral_constraint,
35 voltage_constraint, velocity_constraint}};
36 return spline;
37}
38
39::frc971::MultiSpline AutonomousSplines::FarRockettoHP() {
40 ::frc971::MultiSpline spline;
41 ::frc971::Constraint longitudinal_constraint;
42 ::frc971::Constraint lateral_constraint;
43 ::frc971::Constraint voltage_constraint;
44 ::frc971::Constraint velocity_constraint;
45
46 longitudinal_constraint.constraint_type = 1;
47 longitudinal_constraint.value = 1.5;
48
49 lateral_constraint.constraint_type = 2;
50 lateral_constraint.value = 1.0;
51
52 voltage_constraint.constraint_type = 3;
53 voltage_constraint.value = 11.0;
54
55 velocity_constraint.constraint_type = 4;
56 velocity_constraint.value = 0.5;
57 velocity_constraint.start_distance = 7.5;
58 velocity_constraint.end_distance = 10.0;
59
60 spline.spline_count = 1;
61 spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
62 spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
63 spline.constraints = {{longitudinal_constraint, lateral_constraint,
64 voltage_constraint, velocity_constraint}};
65 return spline;
66}
67
Austin Schuh6bcc2302019-03-23 22:28:06 -0700[diff] [blame]68::frc971::MultiSpline AutonomousSplines::HPToNearRocket() {
69 ::frc971::MultiSpline spline;
70 ::frc971::Constraint longitudinal_constraint;
71 ::frc971::Constraint lateral_constraint;
72 ::frc971::Constraint voltage_constraint;
James Kuszmaulbc837872019-04-07 12:01:47 -0700[diff] [blame^]73 ::frc971::Constraint velocity_constraint;
Austin Schuh6bcc2302019-03-23 22:28:06 -0700[diff] [blame]74
75 longitudinal_constraint.constraint_type = 1;
76 longitudinal_constraint.value = 1.0;
77
78 lateral_constraint.constraint_type = 2;
79 lateral_constraint.value = 1.0;
80
81 voltage_constraint.constraint_type = 3;
James Kuszmaulbc837872019-04-07 12:01:47 -0700[diff] [blame^]82 voltage_constraint.value = 11.0;
83
84 velocity_constraint.constraint_type = 4;
85 velocity_constraint.value = 0.5;
86 velocity_constraint.start_distance = 2.7;
87 velocity_constraint.end_distance = 10.0;
Austin Schuh6bcc2302019-03-23 22:28:06 -0700[diff] [blame]88
89 spline.spline_count = 1;
James Kuszmaulbc837872019-04-07 12:01:47 -0700[diff] [blame^]90 spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
91 spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
92 spline.constraints = {{longitudinal_constraint, lateral_constraint,
93 voltage_constraint, velocity_constraint}};
Austin Schuh6bcc2302019-03-23 22:28:06 -0700[diff] [blame]94 return spline;
95}
96
97::frc971::MultiSpline AutonomousSplines::BasicSSpline() {
98 ::frc971::MultiSpline spline;
99 ::frc971::Constraint longitudinal_constraint;
100 ::frc971::Constraint lateral_constraint;
101 ::frc971::Constraint voltage_constraint;
102
103 longitudinal_constraint.constraint_type = 1;
104 longitudinal_constraint.value = 1.0;
105
106 lateral_constraint.constraint_type = 2;
107 lateral_constraint.value = 1.0;
108
109 voltage_constraint.constraint_type = 3;
110 voltage_constraint.value = 6.0;
111
112 spline.spline_count = 1;
113 const float startx = 0.4;
114 const float starty = 3.4;
115 spline.spline_x = {{0.0f + startx, 0.6f + startx, 0.6f + startx,
116 0.4f + startx, 0.4f + startx, 1.0f + startx}};
117 spline.spline_y = {{starty - 0.0f, starty - 0.0f, starty - 0.3f,
118 starty - 0.7f, starty - 1.0f, starty - 1.0f}};
119 spline.constraints = {
120 {longitudinal_constraint, lateral_constraint, voltage_constraint}};
121 return spline;
122}
123
124::frc971::MultiSpline AutonomousSplines::StraightLine() {
125 ::frc971::MultiSpline spline;
126 ::frc971::Constraint contraints;
127
128 contraints.constraint_type = 0;
129 contraints.value = 0.0;
130 contraints.start_distance = 0.0;
131 contraints.end_distance = 0.0;
132
133 spline.spline_count = 1;
134 spline.spline_x = {{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0}};
135 spline.spline_y = {{1.25, 1.25, 1.25, 1.25, 1.25, 1.25}};
136 spline.constraints = {{contraints}};
137 return spline;
138}
139
140} // namespace actors
141} // namespace y2019