Stop resetting position after profiles

blair-robot-project · Oct 26, 2017 · 1ad60f0 · 1ad60f0
1 parent aa418fb
commit 1ad60f0
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 2 deletions.
diff --git a/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java b/Pathgen/src/main/java/org/usfirst/frc/team449/pathgen/Pathgen.java
@@ -138,7 +138,6 @@ public static void main(String[] args) throws IOException {
 		profiles.put("RedBackup", backupRed);
 		profiles.put("BlueBackup", backupBlue);
 		profiles.put("forward100In", points);
-		profiles.put("BlueBackup", backupBlue);
 		profiles.put("BlueLoadingToLoading", blueLoadingToLoading);
 		profiles.put("BlueBoilerToLoading", blueBoilerToLoading);
 		profiles.put("RedLoadingToLoading", redLoadingToLoading);

diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/jacksonWrappers/FPSTalon.java
@@ -682,6 +682,9 @@ public void loadProfile(MotionProfileData data) {
 					currentGearSettings.getClosedLoopRampRate(), 1);
 		}
 
+		//Only call position getter once
+		double startPosition = data.resetPosition() ? 0 : getPositionFeet();
+
 		//Load in profiles
 		for (int i = 0; i < data.getData().length; ++i) {
 			CANTalon.TrajectoryPoint point = new CANTalon.TrajectoryPoint();
@@ -690,7 +693,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]) * (data.isInverted() ? -1 : 1);
+			point.position = startPosition + feetToEncoder(data.getData()[i][0]) * (data.isInverted() ? -1 : 1);
 
 			//Calculate vel based off inversion
 			if (data.isInverted()) {