Skip to content

Commit 137dfb7

Browse files
author
Noah Gleason
committed
Fix backwards profiles
1 parent b682af4 commit 137dfb7

33 files changed

+2730
-2873
lines changed

‎Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java‎

Lines changed: 21 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -22,17 +22,18 @@ public static void main(String[] args) throws IOException {
2222
final double BACK_FROM_PEG = -5;
2323
//DO NOT TOUCH THE ONES BELOW
2424
final double CARRIAGE_LEN = 3.63;
25-
final double BLUE_WALL_TO_CENTER_PEG = 113.75;
26-
final double BLUE_WALL_TO_SIDE_PEG = 130.75;
27-
final double BLUE_BACK_CORNER_TO_SIDE_PEG = 91.;
28-
final double BLUE_HALF_KEY_LENGTH = 150.5/2.;
29-
final double BLUE_KEY_CORNER_TO_SIDE_PEG = 15.;
30-
final double RED_WALL_TO_CENTER_PEG = 114.;
31-
final double RED_WALL_TO_SIDE_PEG = 130.;
32-
final double RED_BACK_CORNER_TO_SIDE_PEG = 93.5;
33-
final double RED_HALF_KEY_LENGTH = 152.5/2.;
34-
final double RED_KEY_CORNER_TO_SIDE_PEG = 18.75;
35-
final double AIRSHIP_PARALLEL_OFFSET = 6.;
25+
final double BLUE_WALL_TO_CENTER_PEG = 114.;
26+
final double BLUE_WALL_TO_SIDE_PEG = 130.5;
27+
final double BLUE_BACK_CORNER_TO_SIDE_PEG = 89.;
28+
final double BLUE_HALF_KEY_LENGTH = 152./2.;
29+
final double BLUE_KEY_CORNER_TO_SIDE_PEG = 16.;
30+
final double RED_WALL_TO_CENTER_PEG = 113.5;
31+
final double RED_WALL_TO_SIDE_PEG = 131.;
32+
final double RED_BACK_CORNER_TO_SIDE_PEG = 97.;
33+
final double RED_HALF_KEY_LENGTH = 152./2.;
34+
final double RED_KEY_CORNER_TO_SIDE_PEG = 21.;
35+
final double AIRSHIP_PARALLEL_OFFSET_BLUE = 0.;
36+
final double AIRSHIP_PARALLEL_OFFSET_RED = 0.;
3637

3738
final double PEG_BASE_TO_CENTER = CENTER_TO_FRONT + CARRIAGE_LEN + BACK_FROM_PEG;
3839

@@ -43,14 +44,14 @@ public static void main(String[] args) throws IOException {
4344

4445
Waypoint[] blueLeft = new Waypoint[]{
4546
new Waypoint(0, 0, 0),
46-
new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
47-
,-(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
47+
new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.cos(5.*Math.PI/6.))/12.
48+
,-(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
4849
};
4950

5051
Waypoint[] blueRight = new Waypoint[]{
5152
new Waypoint(0, 0, 0),
52-
new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
53-
,(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
53+
new Waypoint((BLUE_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.cos(5.*Math.PI/6.))/12.
54+
,(BLUE_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_BLUE*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
5455
};
5556

5657
Waypoint[] blueCenter = new Waypoint[]{
@@ -60,14 +61,14 @@ public static void main(String[] args) throws IOException {
6061

6162
Waypoint[] redLeft = new Waypoint[]{
6263
new Waypoint(0, 0, 0),
63-
new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
64-
,-(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
64+
new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.cos(5.*Math.PI/6.))/12.
65+
,-(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.sin(5.*Math.PI/6.))/12.,-Math.PI/3.)
6566
};
6667

6768
Waypoint[] redRight = new Waypoint[]{
6869
new Waypoint(0, 0, 0),
69-
new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.cos(5.*Math.PI/6.))/12.
70-
,(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
70+
new Waypoint((RED_WALL_TO_SIDE_PEG-CENTER_TO_BACK - 0.5*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.cos(5.*Math.PI/6.))/12.
71+
,(RED_BACK_CORNER_TO_SIDE_PEG - CENTER_TO_SIDE - (Math.sqrt(3.)/2.)*PEG_BASE_TO_CENTER + AIRSHIP_PARALLEL_OFFSET_RED*Math.sin(5.*Math.PI/6.))/12.,Math.PI/3.)
7172
};
7273

7374
Waypoint[] redCenter = new Waypoint[]{
@@ -157,7 +158,7 @@ public static void main(String[] args) throws IOException {
157158
double calciferWheelbase = 26./12.;
158159

159160
Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_HIGH,
160-
0.05, 5., 3., 6.); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)
161+
0.05, 5., 4.5, 9.); //Units are seconds, feet/second, feet/(second^2), and feet/(second^3)
161162

162163
for (String profile : profiles.keySet()) {
163164
Trajectory trajectory = Pathfinder.generate(profiles.get(profile), config);

‎RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -690,7 +690,7 @@ public void loadProfile(MotionProfileData data) {
690690
point.velocityOnly = velocityOnly; // true => no position servo just velocity feedforward
691691

692692
// Set all the fields of the profile point
693-
point.position = feetToEncoder(data.getData()[i][0]);
693+
point.position = feetToEncoder(data.getData()[i][0]) * (data.isInverted() ? -1 : 1);
694694

695695
//Calculate vel based off inversion
696696
if (data.isInverted()) {

‎RoboRIO/src/main/resources/calciferLeftBlueBackupProfile.csv‎

Lines changed: 49 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -1,55 +1,49 @@
1-
54
2-
3.6998202195815923E-4, 0.014799280878326368, 0.29598561756652736, 0.05
3-
0.0016598585659637298, 0.02579753088011141, 0.21996500003570083, 0.05
4-
0.00456453440812901, 0.058093516843305604, 0.6459197192638838, 0.05
5-
0.009736780007313877, 0.10344491198369735, 0.9070279028078349, 0.05
6-
0.017839832946141745, 0.16206105877655733, 1.1723229358571996, 0.05
7-
0.029553962908322517, 0.23428259924361547, 1.4444308093411629, 0.05
8-
0.04558452438091224, 0.32061122945179443, 1.7265726041635792, 0.05
9-
0.06667136828171022, 0.4217368780159597, 2.0225129712833056, 0.05
10-
0.09359946125912086, 0.5385618595482127, 2.336499630645059, 0.05
11-
0.1272105710811522, 0.6722221964406268, 2.6732067378482816, 0.05
12-
0.16807301705612493, 0.8172489194994549, 2.9005344611765627, 0.05
13-
0.21646743551455633, 0.9678883691686276, 3.012788993383455, 0.05
14-
0.2727166853993793, 1.1249849976964599, 3.1419325705566448, 0.05
15-
0.337186048202079, 1.2893872560539932, 3.288045167150666, 0.05
16-
0.41028513081610507, 1.4619816522805213, 3.4518879245305634, 0.05
17-
0.49247214172896103, 1.6437402182571188, 3.635171319531949, 0.05
18-
0.5842612721242723, 1.8357826079062252, 3.840847792982127, 0.05
19-
0.6858366380214413, 2.0315073179433822, 3.9144942007431416, 0.05
20-
0.7970122085133569, 2.22351140983831, 3.840081837898559, 0.05
21-
0.9176009722860492, 2.4117752754538464, 3.765277312310724, 0.05
22-
1.0474140419043465, 2.596261392365944, 3.6897223382419497, 0.05
23-
1.1862558344826994, 2.7768358515670593, 3.6114891840223073, 0.05
24-
1.3339112137862812, 2.9531075860716363, 3.52543469009154, 0.05
25-
1.490119631756835, 3.124168359411078, 3.4212154667888317, 0.05
26-
1.65453101456359, 3.2882276561350996, 3.281185934480435, 0.05
27-
1.8266394777861632, 3.44216926445146, 3.07883216632721, 0.05
28-
2.005573645763164, 3.578683359540009, 2.730281901770981, 0.05
29-
2.190102794812028, 3.6905829809772848, 2.237992428745512, 0.05
30-
2.378684170630626, 3.771627516371953, 1.620890707893361, 0.05
31-
2.5693176875725445, 3.81267033883837, 0.8208564493283443, 0.05
32-
2.759599402518523, 3.8056342989195713, -0.14072079837597506, 0.05
33-
2.946872367560439, 3.7454593008383132, -1.2034999616251607, 0.05
34-
3.1284463650146055, 3.6314799490833294, -2.279587035099677, 0.05
35-
3.3018235617964797, 3.4675439356374826, -3.278720268916935, 0.05
36-
3.464863868073202, 3.260806125534449, -4.13475620206067, 0.05
37-
3.6158542077459597, 3.0198067934551562, -4.819986641585858, 0.05
38-
3.7536192744114567, 2.755301333309941, -5.290109202904301, 0.05
39-
3.8778743299406693, 2.4851011105842513, -5.404004454513798, 0.05
40-
3.988955314752372, 2.221619696234056, -5.269628287003902, 0.05
41-
4.0872719633166055, 1.966332971284676, -5.105734498987604, 0.05
42-
4.17324899764151, 1.719540686498095, -4.935845695731618, 0.05
43-
4.247289766422093, 1.4808153756116704, -4.774506217728494, 0.05
44-
4.3097559164647645, 1.2493230008534204, -4.629847495165, 0.05
45-
4.36107902606379, 1.0264621919805206, -4.457216177457997, 0.05
46-
4.402163348463961, 0.8216864480034127, -4.095514879542157, 0.05
47-
4.434254593872503, 0.6418249081708596, -3.597230796651063, 0.05
48-
4.4585365423441035, 0.48563896943199575, -3.1237187747772763, 0.05
49-
4.476145765944183, 0.35218447200157993, -2.6690899486083164, 0.05
50-
4.488184183890034, 0.2407683589170284, -2.22832226169103, 0.05
51-
4.495729469015408, 0.15090570250747054, -1.7972531281911575, 0.05
52-
4.4998434292031835, 0.08227920375551274, -1.3725299750391557, 0.05
53-
4.501578501017331, 0.03470143628294848, -0.9515553494512853, 0.05
54-
4.501982459636722, 0.008079172387830838, -0.5324452779023527, 0.05
55-
4.501982459636722, 0.0, -0.16158344775661673, 0.05
1+
48
2+
5.227250022608844E-4, 0.020909000090435375, 0.41818000180870746, 0.05
3+
0.0023454064609125854, 0.03645362917303402, 0.31089258165197287, 0.05
4+
0.006451335591911353, 0.08211858261997534, 0.9132990689388265, 0.05
5+
0.013767472257941222, 0.14632273332059736, 1.2840830140124404, 0.05
6+
0.025241613574952037, 0.2294828263402163, 1.6632018603923786, 0.05
7+
0.0418553796467967, 0.33227532143689326, 2.055849901933539, 0.05
8+
0.06464003174297048, 0.45569304192347565, 2.468354409731648, 0.05
9+
0.09469480172644233, 0.601095399669437, 2.9080471549192275, 0.05
10+
0.1332074081600263, 0.7702521286716791, 3.383134580044842, 0.05
11+
0.18147657729001396, 0.965383382599753, 3.902625078561477, 0.05
12+
0.2404406855302663, 1.1792821648050467, 4.277975644105873, 0.05
13+
0.31065069171082, 1.4042001236110742, 4.49835917612055, 0.05
14+
0.3927437715777452, 1.6418615973385038, 4.753229474548593, 0.05
15+
0.4874499975425362, 1.8941245192958196, 5.045258439146316, 0.05
16+
0.5950581139051417, 2.1521623272521113, 5.160756159125834, 0.05
17+
0.7153540535348383, 2.40591879259393, 5.07512930683637, 0.05
18+
0.8481273667100422, 2.655466263504078, 4.990949418202968, 0.05
19+
0.9931735331474119, 2.900923328747392, 4.909141304866278, 0.05
20+
1.1502924337050309, 3.1423780111523802, 4.829093648099763, 0.05
21+
1.3192736926504234, 3.379625178907851, 4.744943355109417, 0.05
22+
1.4998565316137857, 3.6116567792672476, 4.64063200718793, 0.05
23+
1.6916494090923582, 3.835857549571447, 4.484015406083985, 0.05
24+
1.8939964852865554, 4.0469415238839455, 4.221679486249972, 0.05
25+
2.1050265019648475, 4.220600333565846, 3.4731761936380146, 0.05
26+
2.3220908512364353, 4.341286985431754, 2.41373303731816, 0.05
27+
2.542464445259929, 4.407471880469876, 1.3236979007624328, 0.05
28+
2.762688113645886, 4.404473367719135, -0.05997025501482511, 0.05
29+
2.9788317905830777, 4.322873538743837, -1.6319965795059588, 0.05
30+
3.1869227096726442, 4.161818381791327, -3.2211031390501965, 0.05
31+
3.3833823557902156, 3.929192922351427, -4.652509188798, 0.05
32+
3.565309805743528, 3.638548999066251, -5.812878465703513, 0.05
33+
3.7305454088039185, 3.304712061207814, -6.6767387571687475, 0.05
34+
3.8775596078692334, 2.9402839813062984, -7.288561598030308, 0.05
35+
4.0060475444395145, 2.569758731405628, -7.410504998013412, 0.05
36+
4.116681535994646, 2.212679831102628, -7.141578006059994, 0.05
37+
4.2101534706927835, 1.8694386939627594, -6.864822742797374, 0.05
38+
4.287094483589678, 1.538820257937898, -6.612368720497228, 0.05
39+
4.348789153451391, 1.2338933972342632, -6.098537214072697, 0.05
40+
4.397143368340726, 0.9670842977866958, -5.336181988951347, 0.05
41+
4.433938830184443, 0.7359092368743492, -4.623501218246933, 0.05
42+
4.46085518758187, 0.5383271479485249, -3.951641778516486, 0.05
43+
4.479496530208541, 0.3728268525334408, -3.3100059083016817, 0.05
44+
4.491413605561772, 0.23834150706461005, -2.6897069093766146, 0.05
45+
4.4981217735861785, 0.13416336048813954, -2.08356293152941, 0.05
46+
4.501114983083293, 0.05986418994229283, -1.4859834109169339, 0.05
47+
4.501876056532617, 0.015221468986477088, -0.8928544191163148, 0.05
48+
4.501883477123802, 1.4841182369525153E-4, -0.3014611432556367, 0.05
49+
4.501883477123802, 0.0, -0.0029682364739050306, 0.05

0 commit comments

Comments
 (0)