fix auto

3 files changed, 33 insertions, 10 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; 
 		}
 	}
 
diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
index 091b68b..e3b2bd5 100644
--- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
@@ -34,6 +34,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 import edu.wpi.first.wpilibj.PowerDistributionPanel;
 import edu.wpi.first.wpilibj.Talon;
@@ -42,6 +43,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
 import org.usfirst.frc.team4272.robotlib.PIDServo;
+import org.usfirst.frc.team4272.robotlib.RollingAvg;
 
 public class HwRobot {
 	/* Relay == a Spike */
@@ -71,6 +73,7 @@ public class HwRobot {
 	    camTilt = new PIDServo(9);
 	public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
 	public final Encoder   rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
+	public final PIDSource lRate, rRate;
 	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
 	public final DriverStation ds = DriverStation.getInstance();
 
@@ -88,6 +91,9 @@ public class HwRobot {
 		lDriveE.setPIDSourceType(PIDSourceType.kRate);
 		rDriveE.setPIDSourceType(PIDSourceType.kRate);
 		//PDP.initTable(NetworkTable.getTable("PDP"));
+
+		lRate = new RollingAvg(5, lDriveE);
+		rRate = new RollingAvg(5, rDriveE);
 	}
 
 	public void run(Control c) {
diff --git a/src/org/usfirst/frc/team4272/robot2017/Teleop.java b/src/org/usfirst/frc/team4272/robot2017/Teleop.java
index 254c530..103bd66 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Teleop.java
@@ -31,17 +31,14 @@ package org.usfirst.frc.team4272.robot2017;
 
 import edu.wpi.first.wpilibj.GenericHID.Hand;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import org.usfirst.frc.team4272.robotlib.PushButton;
-import org.usfirst.frc.team4272.robotlib.RollingAvg;
 
 public class Teleop {
 	private final HwRobot robot;
-	private final PIDSource lRate, rRate;
 	private final Timer timer = new Timer();
 	private final Preferences prefs = Preferences.getInstance();
 
@@ -51,8 +48,6 @@ public class Teleop {
 
 	public Teleop(HwRobot robot) {
 		this.robot = robot;
-		this.lRate = new RollingAvg(5, robot.lDriveE);
-		this.rRate = new RollingAvg(5, robot.rDriveE);
 		timer.start();
 	}
 
@@ -83,7 +78,7 @@ public class Teleop {
 		} else if (oi.rStick.getTrigger()) {
 			control.highGear = true;
 		} else {
-			double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2);
+			double speed = Math.abs((robot.lRate.pidGet() + robot.rRate.pidGet()) / 2);
 			if (!control.highGear) {
 				if (speed > shiftUp)
 					control.highGear = true;