ahaha turn

2 files changed, 24 insertions, 10 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index d012a98..038dd5a 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -27,6 +27,8 @@
  */
 package org.usfirst.frc.team4272.robot2017;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 public class Autonomous {
 	private static interface Command {
 		public boolean execute(Control c);
@@ -65,23 +67,29 @@ public class Autonomous {
 			double lPct = lDistCurr/lDistTarget;
 			double rPct = rDistCurr/rDistTarget;
 
-			c.lDrive = Math.copySign(lSpeed, 1-lPct);
+			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.rDrive = Math.copySign(rSpeed, 1-rPct);
+			c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
 			if (Math.abs(1-rPct) < pctTaper) {
-				c.rDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper));
+				c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper));
 			}
 
 			if (Math.abs(lPct - rPct) > pctTolerance) {
 				if (lPct > rPct) {
-					c.lDrive *= pctThrottle;
+					c.lDrive *= 0.6;
+					//c.rDrive *= 2;
 				} else {
-					c.rDrive *= pctThrottle;
+					//c.lDrive *= 2;
+					c.rDrive *= 0.6;
 				}
 			}
+			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;
 		}
 	}
@@ -92,8 +100,8 @@ public class Autonomous {
 	}
 	private static Command turnRadians(HwRobot robot, double speed, double rad) {
 		return new DriveDistance(robot,
-		                         speed, Math.copySign(rad*axleWidth, rad),
-		                         speed, Math.copySign(rad*axleWidth, -rad));
+		                         speed, Math.copySign(rad*axleWidth/2, rad),
+		                         speed, Math.copySign(rad*axleWidth/2, -rad));
 	}
 	private static Command turnDegrees(HwRobot robot, double speed, double deg) {
 		return turnRadians(robot, speed, deg * Math.PI/180);
@@ -115,20 +123,23 @@ 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),
+			//driveDistance(robot, 0.3, 3.5),
+			//turnDegrees(robot, 0.3, 60),
+			//driveDistance(robot, 0.3, 8.3),
+			turnDegrees(robot, 0.5, 90),
 		};
 	}
 
 	public Control run(Control c) {
 		c.gedOut = false;
+		c.highGear = false;
 
 		if (step < commands.length) {
 			if (commands[step].execute(c))
 				step++;
 		}
 
+		SmartDashboard.putNumber("step", step);
 		return c;
 	}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
index 091b68b..7608376 100644
--- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
@@ -119,6 +119,9 @@ 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);
 	}