1 files changed, 111 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Teleop.java b/src/org/usfirst/frc/team4272/robot2017/Teleop.java
new file mode 100644
index 0000000..b7ff063
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/Teleop.java
@@ -0,0 +1,111 @@
+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 double armSoftStop = 95;//295;
+
+	private final PushButton shootButton = new PushButton();
+	private final Timer time = new Timer();
+	private double lastTime;
+	private double currentTime;
+	private boolean state = false;
+	private PIDSource lRate, rRate;
+	private Preferences prefs = Preferences.getInstance();
+
+	public Teleop(HwRobot robot) {
+		this.robot = robot;
+		this.lRate = new RollingAvg(5, robot.lDriveE);
+		this.rRate = new RollingAvg(5, robot.rDriveE);
+	}
+
+	private static double jsScale(Joystick j) {
+		double y = j.getY();/* +:forward; -:backward */
+		double z = j.getZ();/* +:more-sensitive; -:less-sensitive */
+		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);
+
+		/* shifting */
+		double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28);
+		double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28);
+		if (oi.lStick.getTrigger()) {
+			control.highGear = false;
+		} else if (oi.rStick.getTrigger()) {
+			control.highGear = true;
+		} else {
+			double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2);
+			if (!control.highGear) {
+				if (speed > shiftUp)
+					control.highGear = true;
+			} else {
+				if (speed < shiftDown)
+					control.highGear = false;
+			}
+		}
+
+		/* auto gear wiggle */
+		if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { 
+			currentTime = Timer.getMatchTime();
+			if (currentTime-lastTime > 0.15) {
+				state = !state;
+				lastTime = currentTime;
+			}
+			if (state) {
+				control.lDrive = 0.5;
+				control.rDrive = 0;
+			} else {
+				control.lDrive = 0;
+				control.rDrive = 0.5;
+			}
+		}
+
+		/* climber */
+		if (Math.abs(oi.xbox.getY(Hand.kLeft)) > 0.2) {
+			control.climber = oi.xbox.getY(Hand.kLeft);
+		} else {
+			control.climber = 0;
+		}
+
+		/* gear place */
+		if (oi.xbox.getAButton()) {
+			control.gedOut = true;
+		}
+		if (oi.xbox.getBButton()) {
+			control.gedOut = false;
+		}
+		
+		/* camera */
+		if (oi.xbox.getTriggerAxis(Hand.kRight) > 0) {
+			control.camRotate = 1;
+		} else if (oi.xbox.getTriggerAxis(Hand.kLeft) > 0) {
+			control.camRotate = -1;
+		} else {
+			control.camRotate = 0;
+		}
+		control.camTilt = oi.xbox.getX(Hand.kRight);
+
+		return control;
+	}
+}