fix auto

1 files changed, 26 insertions, 4 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index da7b8f8..5ce3aa5 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -31,18 +31,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Autonomous {
 	private static interface Command {
+		/** Mutate the @{code Control} (possible because it is
+		 * passed by reference), and return whether or not
+		 * this command has finished yet.
+		 *
+		 * @param c The structure to control the robot
+		 * @return Whether the command has finished.
+		 */
 		public boolean execute(Control c);
 	}
 
 	private static class DriveDistance implements Command {
 		private static final double pctTaper = 1.0/3;
 		private static final double pctTolerance = 0.005;
-		private static final double distFinished = 0.1;
 
 		private final HwRobot robot;
 		private final double pctThrottle;
 		private final double lSpeed, lDistTarget;
 		private final double rSpeed, rDistTarget;
+		private double lMult = 1 , rMult = 1;
 		public DriveDistance(HwRobot robot, double pctThrottle,
 		                     double lSpeed, double lDistTarget,
 		                     double rSpeed, double rDistTarget) {
@@ -67,12 +74,26 @@ public class Autonomous {
 
 			c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct);
 			if (Math.abs(1-lPct) < pctTaper) {
-				c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper));
+				c.lDrive *= Math.abs((1-lPct)/pctTaper);
+				if ( Math.abs(robot.lRate.pidGet()) < 0.05) {
+					lMult += 0.05;
+				}
+				c.lDrive *= lMult;
+				if (c.lDrive > lSpeed) {
+					c.lDrive = lSpeed;
+				}
 			}
 
 			c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
 			if (Math.abs(1-rPct) < pctTaper) {
-				c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper));
+				c.rDrive *= Math.abs((1-rPct)/pctTaper);
+				if ( Math.abs(robot.rRate.pidGet()) < 0.05) {
+					rMult += 0.05;
+				}
+				c.rDrive *= rMult;
+				if (c.rDrive > rSpeed) {
+					c.rDrive = rSpeed;
+				}
 			}
 
 			if (Math.abs(lPct - rPct) > pctTolerance) {
@@ -82,7 +103,8 @@ public class Autonomous {
 					c.rDrive *= pctThrottle;
 				}
 			}
-			return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished;
+			
+			return lPct >= 1 && rPct >= 1; 
 		}
 	}