Work on autonomous on comp bot.

1 files changed, 16 insertions, 13 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 5ce3aa5..5fa7257 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -42,14 +42,14 @@ public class Autonomous {
 	}
 
 	private static class DriveDistance implements Command {
-		private static final double pctTaper = 1.0/3;
+		private static final double pctTaper = 1.0/6;
 		private static final double pctTolerance = 0.005;
 
 		private final HwRobot robot;
 		private final double pctThrottle;
 		private final double lSpeed, lDistTarget;
 		private final double rSpeed, rDistTarget;
-		private double lMult = 1 , rMult = 1;
+		private double lMin = 0, rMin = 0;
 		public DriveDistance(HwRobot robot, double pctThrottle,
 		                     double lSpeed, double lDistTarget,
 		                     double rSpeed, double rDistTarget) {
@@ -75,10 +75,9 @@ public class Autonomous {
 			c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct);
 			if (Math.abs(1-lPct) < pctTaper) {
 				c.lDrive *= Math.abs((1-lPct)/pctTaper);
-				if ( Math.abs(robot.lRate.pidGet()) < 0.05) {
-					lMult += 0.05;
+				if (Math.abs(robot.lRate.pidGet()) < 0.5) {
+					lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05;
 				}
-				c.lDrive *= lMult;
 				if (c.lDrive > lSpeed) {
 					c.lDrive = lSpeed;
 				}
@@ -87,10 +86,9 @@ public class Autonomous {
 			c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
 			if (Math.abs(1-rPct) < pctTaper) {
 				c.rDrive *= Math.abs((1-rPct)/pctTaper);
-				if ( Math.abs(robot.rRate.pidGet()) < 0.05) {
-					rMult += 0.05;
+				if (Math.abs(robot.rRate.pidGet()) < 0.5) {
+					rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05;
 				}
-				c.rDrive *= rMult;
 				if (c.rDrive > rSpeed) {
 					c.rDrive = rSpeed;
 				}
@@ -103,7 +101,12 @@ public class Autonomous {
 					c.rDrive *= pctThrottle;
 				}
 			}
-			
+			SmartDashboard.putNumber("lPct", lPct);
+			SmartDashboard.putNumber("rPct", rPct);
+			SmartDashboard.putNumber("lMin", lMin);
+			SmartDashboard.putNumber("rMin", rMin);
+			SmartDashboard.putNumber("lDrive", c.lDrive);
+			SmartDashboard.putNumber("rDrive", c.rDrive);
 			return lPct >= 1 && rPct >= 1; 
 		}
 	}
@@ -133,15 +136,15 @@ public class Autonomous {
 		// Center peg
 		/*
 		commands = new Command[]{
-			driveDistance(robot, 0.3, 8.75),
+			driveDistance(robot, 0.4, 8.75),
 		};
 		*/
 
 		// Left peg
 		commands = new Command[]{
-			driveDistance(robot, 0.3, 3.5),
-			turnDegrees(robot, 0.3, 60),
-			driveDistance(robot, 0.3, 8.3),
+			driveDistance(robot, 0.4, 3.5),
+			turnDegrees(robot, 0.4, 60),
+			driveDistance(robot, 0.4, 8.3),
 		};
 	}