3 files changed, 33 insertions, 20 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
index 0a7452c..1d61448 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -8,12 +8,12 @@ public class Autonomous {
 	}
 
 	public Control run(Control c) {
-		//if (!robot.armL.get()) {
-		//	c.arm = -0.2;
-		//} else {
-		//	c.arm = 0;
-		//	robot.armE.reset();
-		//}
+		if (robot.armL.get()) {
+			c.arm = -0.2;
+		} else {
+			c.arm = 0;
+			robot.armE.reset();
+		}
 
 		return c;
 	}
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index ebc4efc..7d63f86 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -29,27 +29,25 @@ public class HwRobot {
 
 	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
 
-	//public final Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
-	//public final Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
 	private final PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0);
 	private final PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4);
 	private final PIDOutput armM    = new Talon(/*PWM*/8);
-	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2);
 	private final PIDOutput fBallM  = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3));
 	private final PIDOutput bBallM  = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7));
+
+	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
 	public final DigitalInput armL = new DigitalInput(/*DIO*/3);
 	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
+	public final Encoder   rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
+	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
 
 	public HwRobot() {
-		//lDriveE.setPIDSourceType(PIDSourceType.kRate);
-		//rDriveE.setPIDSourceType(PIDSourceType.kRate);
 		armE.setPIDSourceType(PIDSourceType.kDisplacement);
+		lDriveE.setPIDSourceType(PIDSourceType.kRate);
+		rDriveE.setPIDSourceType(PIDSourceType.kRate);
 	}
 
 	public void run(Control c) {
-		if (armE.pidGet() < -295)
-			c.arm = 0;
-
 		lDriveM1.pidWrite( c.lDrive1);
 		lDriveM2.pidWrite( c.lDrive2);
 		rDriveM1.pidWrite(-c.rDrive1);
@@ -61,6 +59,11 @@ public class HwRobot {
 		SmartDashboard.putNumber("fBall", c.fBall);
 		SmartDashboard.putNumber("bBall", c.bBall);
 		SmartDashboard.putNumber("armM", c.arm);
+
 		SmartDashboard.putNumber("armE", armE.pidGet());
+		SmartDashboard.putNumber("lDriveE", lDriveE.pidGet());
+		SmartDashboard.putNumber("rDriveE", rDriveE.pidGet());
+		SmartDashboard.putBoolean("ballL", ballL.get());
+		SmartDashboard.putBoolean("armL", armL.get());
 	}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index cafbca1..19dfc6e 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj.Joystick;
 
 public class Teleop {
 	private final HwRobot robot;
+	private final double armSoftStop = 95;//295;
 
 	public Teleop(HwRobot robot) {
 		this.robot = robot;
+		robot.armE.reset();
 	}
 
 	private static double jsScale(Joystick j) {
@@ -36,14 +38,22 @@ public class Teleop {
 		double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
 		double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B));
 
-		boolean ballL = false; //robot.ballL.get();
-		control.fBall = bound(-oneIf(!ballL), fBall, 1);
-		control.bBall = bound(-oneIf(!ballL), bBall < 0 ? 0.75*bBall : bBall, 1);
+		boolean ballL = robot.ballL.get();
+		control.fBall = bound(-oneIf(ballL), fBall, 1);
+		control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.75*bBall : bBall, 1);
 
 		/* I'm eyeballing 10 degrees ≈ 50 clicks */
-		//if (robot.armE.pidGet() < 50 && robot.armL.get()) {
-		//	robot.armE.reset();
-		//}
+		if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
+			robot.armE.reset();
+		}
+
+		if (oi.xbox.getButton(Button.LBumper))
+			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
+		else if (oi.xbox.getButton(Button.RBumper))
+			control.arm = robot.armE.pidGet()/armSoftStop;
+
+		//if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
+		//	control.arm = 0;
 
 		/* Take pictures */
 		/*