side peg auto

1 files changed, 35 insertions, 20 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 60dcd24..8e5846a 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -95,16 +95,30 @@ public class Autonomous {
 			});
 		modeChooser.addObject("Left Peg", robot->new Command[]{
 				init,
-				driveDistance(robot, 0.5, 8.0),
-				turnDegrees(robot, 0.4, 60),
+				driveDistance(robot, 0.4, 8.4),
+				turnDegrees(robot, 0.4, 70),
 				driveDistance(robot, 0.4, 2.16),
+				timeout(1.0, c->false),
+				c->{c.gedOut = true; return true;},
+				timeout(1.0, c->false),
+				driveDistance(robot, 0.4, -3),
+				c->{c.gedOut = false; return true;},
+				turnDegrees(robot, 0.4, -70),
+				driveDistance(robot, 1.0, 15),
 				stop,
 			});
 		modeChooser.addObject("Right Peg", robot->new Command[]{
 				init,
-				driveDistance(robot, 0.4, 3.5),
-				turnDegrees(robot, 0.4, -60),
-				driveDistance(robot, 0.4, 8.3),
+				driveDistance(robot, 0.4, 8.4),
+				turnDegrees(robot, 0.4, -70),
+				driveDistance(robot, 0.4, 2.16),
+				timeout(1.0, c->false),
+				c->{c.gedOut = true; return true;},
+				timeout(1.0, c->false),
+				driveDistance(robot, 0.4, -3),
+				c->{c.gedOut = false; return true;},
+				turnDegrees(robot, 0.4, 70),
+				driveDistance(robot, 1.0, 15),
 				stop,
 			});
 		modeChooser.addObject("Stand Still", robot->new Command[]{
@@ -229,16 +243,16 @@ public class Autonomous {
 			}
 			c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
 
-			/* Make sure the left and right sides are even */
 			cntT++;
-			if (Math.abs(lPct - rPct) > pctTolerance) {
-				if (lPct > rPct) {
-					cntL++;
-					c.lDrive *= pctThrottle;
-				} else {
-					cntR++;
-					c.rDrive *= pctThrottle;
-				}
+			double throttle = Math.min(Math.abs(lPct - rPct), pctTolerance)/pctTolerance;
+			if (lPct > rPct) {
+				c.lDrive *= 1.0-(pctThrottle*throttle);
+				//c.rDrive /= 1.0-(pctThrottle*throttle);
+				cntL += throttle;
+			} else {
+				//c.lDrive /= 1.0-(pctThrottle*throttle);
+				c.rDrive *= 1.0-(pctThrottle*throttle);
+				cntR += throttle;
 			}
 
 			SmartDashboard.putNumber("lPct", lPct);
@@ -247,8 +261,8 @@ public class Autonomous {
 			SmartDashboard.putNumber("rMin", rMin);
 			SmartDashboard.putNumber("lDrive", c.lDrive);
 			SmartDashboard.putNumber("rDrive", c.rDrive);
-			SmartDashboard.putNumber("lCnt", cntL/(double)cntT);
-			SmartDashboard.putNumber("rCnt", cntR/(double)cntT);
+			SmartDashboard.putNumber("lCnt", cntL/cntT);
+			SmartDashboard.putNumber("rCnt", cntR/cntT);
 			return lPct >= 1 && rPct >= 1;
 		}
 	}
@@ -276,14 +290,15 @@ public class Autonomous {
 	}
 
 	private static Command driveDistance(HwRobot robot, double speed, double dist) {
-		return new DriveDistance(robot, 0.05, 0.001,
+		return new DriveDistance(robot, 1.0, (1/36.0)/dist,
 		                         speed, dist,
 		                         speed, dist);
 	}
 	private static Command turnRadians(HwRobot robot, double speed, double rad) {
-		return new DriveDistance(robot, 0.6, 0.002,
-		                         speed, Math.copySign(rad*robot.axleWidth/2, rad),
-		                         speed, Math.copySign(rad*robot.axleWidth/2, -rad));
+		double dist = rad*robot.axleWidth/2;
+		return new DriveDistance(robot, 0.33, (1/36.0)/dist,
+		                         speed, Math.copySign(dist, rad),
+		                         speed, Math.copySign(dist, -rad));
 	}
 	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
 		return turnRadians(robot, speed, deg * Math.PI/180);