stuff

4 files changed, 35 insertions, 23 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
index 295cba9..0a7452c 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -8,6 +8,13 @@ public class Autonomous {
 	}
 
 	public Control run(Control c) {
+		//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 ec96bf4..ebc4efc 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -3,6 +3,7 @@ package org.usfirst.frc.team4272.robot2016;
 import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSourceType;
@@ -28,14 +29,16 @@ public class HwRobot {
 
 	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
 
-	//public Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
-	//public Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
-	private PIDOutput lDriveM1 = new Talon(/*PWM*/1), lDriveM2 = new Talon(/*PWM*/0);
-	private PIDOutput rDriveM1 = new Talon(/*PWM*/2), rDriveM2 = new Talon(/*PWM*/4);
-	private PIDOutput armM    = new Talon(/*PWM*/8);
-	private Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2);
-	private PIDOutput fBallM  = new PIDOutputSplitter(new Talon(/*PWM*/9), new Talon(/*PWM*/3));
-	private PIDOutput bBallM  = new PIDOutputSplitter(new Talon(/*PWM*/6), new Talon(/*PWM*/7));
+	//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 DigitalInput armL = new DigitalInput(/*DIO*/3);
+	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
 
 	public HwRobot() {
 		//lDriveE.setPIDSourceType(PIDSourceType.kRate);
@@ -44,8 +47,7 @@ public class HwRobot {
 	}
 
 	public void run(Control c) {
-		double armEval = armE.pidGet(); // avoid reading twice
-		if (armEval < -295)
+		if (armE.pidGet() < -295)
 			c.arm = 0;
 
 		lDriveM1.pidWrite( c.lDrive1);
@@ -59,6 +61,6 @@ public class HwRobot {
 		SmartDashboard.putNumber("fBall", c.fBall);
 		SmartDashboard.putNumber("bBall", c.bBall);
 		SmartDashboard.putNumber("armM", c.arm);
-		SmartDashboard.putNumber("armE", armEval);
+		SmartDashboard.putNumber("armE", armE.pidGet());
 	}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2016/Robot.java b/src/org/usfirst/frc/team4272/robot2016/Robot.java
index fc2eef6..bb15ccc 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Robot.java
@@ -37,7 +37,7 @@ public class Robot extends IterativeRobot {
 
 	public void teleopInit() {
 		try {
-			teleop = new Teleop();
+			teleop = new Teleop(robot);
 		} catch (Exception e) {}
 	}
 
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index e69c8f0..cafbca1 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -1,14 +1,14 @@
 package org.usfirst.frc.team4272.robot2016;
 
-//import org.usfirst.frc.team4272.robotlib.PushButton;
 import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
 import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
 import edu.wpi.first.wpilibj.Joystick;
 
 public class Teleop {
-	//private PushButton camButton = new PushButton();
+	private final HwRobot robot;
 
-	public Teleop() {
+	public Teleop(HwRobot robot) {
+		this.robot = robot;
 	}
 
 	private static double jsScale(Joystick j) {
@@ -32,15 +32,18 @@ public class Teleop {
 		control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0;
 		control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0;
 		control.arm = -oi.xbox.getAxis(Axis.LY);
-		control.fBall = bound(-1, oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger), 1);
-		control.bBall = bound(-1,
-		                      bound(-1, control.fBall, 0)
-		                      + oneIf(oi.xbox.getButton(Button.A))
-		                      - oneIf(oi.xbox.getButton(Button.B)),
-		                      1);
-		control.fBall = control.fBall < 0 ? 0.5*control.fBall : control.fBall;
-		control.bBall = control.bBall < 0 ? 0.75*control.bBall : control.bBall;
 
+		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);
+
+		/* I'm eyeballing 10 degrees ≈ 50 clicks */
+		//if (robot.armE.pidGet() < 50 && robot.armL.get()) {
+		//	robot.armE.reset();
+		//}
 
 		/* Take pictures */
 		/*