Skip to content

Commit

Permalink
Fix backwards profiles
Browse files Browse the repository at this point in the history
  • Loading branch information
Noah Gleason committed Oct 21, 2017
1 parent b682af4 commit 137dfb7
Show file tree
Hide file tree
Showing 33 changed files with 2,730 additions and 2,873 deletions.
41 changes: 21 additions & 20 deletions Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,17 +22,18 @@ public static void main(String[] args) throws IOException {
final double BACK_FROM_PEG = -5;
//DO NOT TOUCH THE ONES BELOW
final double CARRIAGE_LEN = 3.63;
final double BLUE_WALL_TO_CENTER_PEG = 113.75;
final double BLUE_WALL_TO_SIDE_PEG = 130.75;
final double BLUE_BACK_CORNER_TO_SIDE_PEG = 91.;
final double BLUE_HALF_KEY_LENGTH = 150.5/2.;
final double BLUE_KEY_CORNER_TO_SIDE_PEG = 15.;
final double RED_WALL_TO_CENTER_PEG = 114.;
final double RED_WALL_TO_SIDE_PEG = 130.;
final double RED_BACK_CORNER_TO_SIDE_PEG = 93.5;
final double RED_HALF_KEY_LENGTH = 152.5/2.;
final double RED_KEY_CORNER_TO_SIDE_PEG = 18.75;
final double AIRSHIP_PARALLEL_OFFSET = 6.;
final double BLUE_WALL_TO_CENTER_PEG = 114.;
final double BLUE_WALL_TO_SIDE_PEG = 130.5;
final double BLUE_BACK_CORNER_TO_SIDE_PEG = 89.;
final double BLUE_HALF_KEY_LENGTH = 152./2.;
final double BLUE_KEY_CORNER_TO_SIDE_PEG = 16.;
final double RED_WALL_TO_CENTER_PEG = 113.5;
final double RED_WALL_TO_SIDE_PEG = 131.;
final double RED_BACK_CORNER_TO_SIDE_PEG = 97.;
final double RED_HALF_KEY_LENGTH = 152./2.;
final double RED_KEY_CORNER_TO_SIDE_PEG = 21.;
final double AIRSHIP_PARALLEL_OFFSET_BLUE = 0.;
final double AIRSHIP_PARALLEL_OFFSET_RED = 0.;

final double PEG_BASE_TO_CENTER = CENTER_TO_FRONT + CARRIAGE_LEN + BACK_FROM_PEG;

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

Waypoint[] blueLeft = new Waypoint[]{
new Waypoint(0, 0, 0),
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.
,-(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.)
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.
,-(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.)
};

Waypoint[] blueRight = new Waypoint[]{
new Waypoint(0, 0, 0),
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.
,(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.)
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.
,(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.)
};

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

Waypoint[] redLeft = new Waypoint[]{
new Waypoint(0, 0, 0),
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.
,-(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.)
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.
,-(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.)
};

Waypoint[] redRight = new Waypoint[]{
new Waypoint(0, 0, 0),
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.
,(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.)
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.
,(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.)
};

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

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

for (String profile : profiles.keySet()) {
Trajectory trajectory = Pathfinder.generate(profiles.get(profile), config);
Expand Down
2 changes: 1 addition & 1 deletion RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -690,7 +690,7 @@ public void loadProfile(MotionProfileData data) {
point.velocityOnly = velocityOnly; // true => no position servo just velocity feedforward

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

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

0 comments on commit 137dfb7

Please sign in to comment.