work on auto

2 files changed, 12 insertions, 22 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 038dd5a..da7b8f8 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -37,24 +37,22 @@ public class Autonomous {
 	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 pctThrottle = 0.2;
 		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;
-		public DriveDistance(HwRobot robot,
+		public DriveDistance(HwRobot robot, double pctThrottle,
 		                     double lSpeed, double lDistTarget,
 		                     double rSpeed, double rDistTarget) {
 			this.robot = robot;
+			this.pctThrottle = pctThrottle;
 			this.lSpeed = lSpeed;
 			this.lDistTarget = lDistTarget;
 			this.rSpeed = rSpeed;
 			this.rDistTarget = rDistTarget;
 		}
-		public DriveDistance(HwRobot robot, double speed, double target) {
-			this(robot, speed, target, speed, target);
-		}
 		private boolean initialized = false;
 		public boolean execute(Control c) {
 			if (!initialized) {
@@ -79,27 +77,23 @@ public class Autonomous {
 
 			if (Math.abs(lPct - rPct) > pctTolerance) {
 				if (lPct > rPct) {
-					c.lDrive *= 0.6;
-					//c.rDrive *= 2;
+					c.lDrive *= pctThrottle;
 				} else {
-					//c.lDrive *= 2;
-					c.rDrive *= 0.6;
+					c.rDrive *= pctThrottle;
 				}
 			}
-			SmartDashboard.putNumber("lPct", lPct);
-			SmartDashboard.putNumber("rPct", rPct);
-			SmartDashboard.putNumber("lTarget", lDistTarget);
-			SmartDashboard.putNumber("rTarget", rDistTarget);
 			return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished;
 		}
 	}
 
 	private static final double axleWidth = 2; // in feet
 	private static Command driveDistance(HwRobot robot, double speed, double dist) {
-		return new DriveDistance(robot, speed, dist);
+		return new DriveDistance(robot, 0.2,
+		                         speed, dist,
+		                         speed, dist);
 	}
 	private static Command turnRadians(HwRobot robot, double speed, double rad) {
-		return new DriveDistance(robot,
+		return new DriveDistance(robot, 0.6,
 		                         speed, Math.copySign(rad*axleWidth/2, rad),
 		                         speed, Math.copySign(rad*axleWidth/2, -rad));
 	}
@@ -123,10 +117,9 @@ public class Autonomous {
 
 		// Left peg
 		commands = new Command[]{
-			//driveDistance(robot, 0.3, 3.5),
-			//turnDegrees(robot, 0.3, 60),
-			//driveDistance(robot, 0.3, 8.3),
-			turnDegrees(robot, 0.5, 90),
+			driveDistance(robot, 0.3, 3.5),
+			turnDegrees(robot, 0.3, 60),
+			driveDistance(robot, 0.3, 8.3),
 		};
 	}
 
diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
index 7608376..091b68b 100644
--- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
@@ -119,9 +119,6 @@ public class HwRobot {
 		SmartDashboard.putBoolean("highGear", c.highGear);
 		SmartDashboard.putBoolean("gedOut", c.gedOut);
 
-		SmartDashboard.putNumber("lDrive", c.lDrive);
-		SmartDashboard.putNumber("rDrive", c.rDrive);
-
 		//PDP.updateTable();
 		//SmartDashboard.putData("PDP", PDP);
 	}