bound some calculations

1 files changed, 14 insertions, 2 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index 0249eaa..0de200d 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -18,13 +18,25 @@ public class Teleop {
 		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
 	}
 
+	private static double bound(double min, double v, double max) {
+		return Math.min(Math.max(v, min), max);
+	}
+
+	private static double oneIf(boolean b) {
+		return b ? 1 : 0;
+	}
+
 	public Control run(Control control, HwOI oi) {
 		/* Drive */
 		control.lDrive = jsScale(oi.lStick);
 		control.rDrive = jsScale(oi.rStick);
 		control.arm = -oi.xbox.getAxis(Axis.LY);
-		control.fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
-		control.bBall = Math.min(control.fBall, 0) + (oi.xbox.getButton(Button.A) ? 1 : 0) + (oi.xbox.getButton(Button.B) ? -1 : 0);
+		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);
 
 		SmartDashboard.putNumber("fBall", control.fBall);
 		SmartDashboard.putNumber("bBall", control.bBall);