From 178f9cf75f06969b9349dda8d4cb1c425b27ed6f Mon Sep 17 00:00:00 2001
From: Luke Shumaker <lukeshu@lukeshu.com>
Date: Tue, 21 Feb 2017 01:59:48 -0500
Subject: Rename from robot2016 to robot2017

---
 .project                                           |   2 +-
 build.properties                                   |   2 +-
 build.xml                                          |   4 +-
 .../usfirst/frc/team4272/robot2016/Autonomous.java |  47 ---------
 .../usfirst/frc/team4272/robot2016/Control.java    |  12 ---
 src/org/usfirst/frc/team4272/robot2016/HwOI.java   |  10 --
 .../usfirst/frc/team4272/robot2016/HwRobot.java    |  87 ----------------
 src/org/usfirst/frc/team4272/robot2016/Robot.java  |  61 -----------
 src/org/usfirst/frc/team4272/robot2016/Teleop.java | 111 ---------------------
 .../usfirst/frc/team4272/robot2017/Autonomous.java |  47 +++++++++
 .../usfirst/frc/team4272/robot2017/Control.java    |  12 +++
 src/org/usfirst/frc/team4272/robot2017/HwOI.java   |  10 ++
 .../usfirst/frc/team4272/robot2017/HwRobot.java    |  87 ++++++++++++++++
 src/org/usfirst/frc/team4272/robot2017/Robot.java  |  61 +++++++++++
 src/org/usfirst/frc/team4272/robot2017/Teleop.java | 111 +++++++++++++++++++++
 15 files changed, 332 insertions(+), 332 deletions(-)
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/Autonomous.java
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/Control.java
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/HwOI.java
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/HwRobot.java
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/Robot.java
 delete mode 100644 src/org/usfirst/frc/team4272/robot2016/Teleop.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/Autonomous.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/Control.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/HwOI.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/HwRobot.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/Robot.java
 create mode 100644 src/org/usfirst/frc/team4272/robot2017/Teleop.java

diff --git a/.project b/.project
index 528bd79..2480def 100644
--- a/.project
+++ b/.project
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <projectDescription>
-	<name>frc4272-robot2016</name>
+	<name>frc4272-robot2017</name>
 	<comment></comment>
 	<projects>
 	</projects>
diff --git a/build.properties b/build.properties
index f1d586e..0b89af4 100644
--- a/build.properties
+++ b/build.properties
@@ -1,5 +1,5 @@
 # Project specific information
-package=org.usfirst.frc.team4272.robot2016
+package=org.usfirst.frc.team4272.robot2017
 robot.class=${package}.Robot
 simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
 #Uncomment and point at user libraries to include them in the build. Do not put libraries in the \wpilib\java folder, this folder is completely overwritten on plugin update.
diff --git a/build.xml b/build.xml
index ab7b4e8..71110c3 100644
--- a/build.xml
+++ b/build.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 
-<project name="frc4272-robot2016" default="deploy">
+<project name="frc4272-robot2017" default="deploy">
 
   <!--
   The following properties can be defined to override system level
@@ -28,7 +28,7 @@
   <import file="${wpilib.ant.dir}/build.xml"/>
 
   <target name="javadoc">
-    <javadoc access="public" author="true" classpath="${wpilib.jar}:${networktables.jar}" destdir="doc" nodeprecated="false" nodeprecatedlist="false" noindex="false" nonavbar="false" notree="false" packagenames="org.usfirst.frc.team4272.robot2016,org.usfirst.frc.team4272.robotlib" source="1.8" sourcepath="src" splitindex="true" use="true" version="true">
+    <javadoc access="public" author="true" classpath="${wpilib.jar}:${networktables.jar}" destdir="doc" nodeprecated="false" nodeprecatedlist="false" noindex="false" nonavbar="false" notree="false" packagenames="org.usfirst.frc.team4272.robot2017,org.usfirst.frc.team4272.robotlib" source="1.8" sourcepath="src" splitindex="true" use="true" version="true">
       <link href="${wpilib}/javadoc/"/>
     </javadoc>
   </target>
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
deleted file mode 100644
index 39b792d..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ /dev/null
@@ -1,47 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-public class Autonomous {
-	private final HwRobot robot;
-
-	public Autonomous(HwRobot robot) {
-		this.robot = robot;
-		robot.lDriveE.reset();
-		robot.rDriveE.reset();
-	}
-
-	public Control run(Control c) {
-		double lDist = robot.lDriveE.getDistance();
-		double rDist = robot.rDriveE.getDistance();
-
-		double speed = 0.6;
-		double thresh = 10;
-		double target = 15;
-		double max = 16;
-		
-		if (lDist < thresh) {
-			c.lDrive = speed;
-		} else if (lDist < max) {
-			c.lDrive = speed*Math.sqrt((target-lDist)/(target-thresh));
-		} else {
-			c.lDrive = 0;
-		}
-
-		if (rDist < thresh) {
-			c.rDrive = speed;
-		} else if (rDist < max) {
-			c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh));
-		} else {
-			c.rDrive = 0;
-		}
-
-		if (Math.abs(lDist - rDist) > 0.25) {
-			if (lDist > rDist) {
-				c.lDrive *= 0.2;
-			} else {
-				c.rDrive *= 0.2;
-			}
-		}
-
-		return c;
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
deleted file mode 100644
index eae613e..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/Control.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-public class Control {
-	double lDrive = 0,
-	       rDrive = 0,
-	       camRotate = 0,
-	       camTilt = 0,
-	       climber = 0;
-	boolean highGear = false,
-		gedOut = false;
-	public Control() {}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwOI.java b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
deleted file mode 100644
index 00b0b05..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/HwOI.java
+++ /dev/null
@@ -1,10 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.XboxController;
-
-public class HwOI {
-	public Joystick lStick = new Joystick(0);
-	public Joystick rStick = new Joystick(1);
-	public XboxController xbox = new XboxController(2);
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
deleted file mode 100644
index 1368d97..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ /dev/null
@@ -1,87 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
-import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
-import org.usfirst.frc.team4272.robotlib.PIDServo;
-
-public class HwRobot {
-	/* Relay == a Spike */
-	/* PCM = Pneumatics Control Module */
-
-	/* All of the numbered inputs are in the classes:
-	 * - DIO: 0-9
-	 * - Relay: 0-3
-	 * - Analog In: 0-3
-	 * - PWM: 0-9
-	 * - PCM: 0-7 (the PCM is connected via CAN).
-	 * - CAN
-	 *
-	 * For completeness, the roboRIO also has: i2c, RS-232, SPI,
-	 * RSL, 2xUSB-A, an accelerometer, and an expansion port.
-	 *
-	 * And, for communication: USB-B and Ethernet.
-	 */
-
-	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
-
-	private final PIDOutput
-	    rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
-	    lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
-	    climber = new Talon(/*PWM*/3), /* PDP 4 */
-	    camRotate = new PIDServo(8),
-	    camTilt = new PIDServo(9);
-	public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
-	public final Encoder   rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
-	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
-
-	private double maxRate = 0;
-
-	public DoubleSolenoid
-		shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),
-		ged     = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);
-	
-	public HwRobot() {
-		lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
-		rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
-
-		lDriveE.setPIDSourceType(PIDSourceType.kRate);
-		rDriveE.setPIDSourceType(PIDSourceType.kRate);
-		//PDP.initTable(NetworkTable.getTable("PDP"));
-	}
-
-	public void run(Control c) {
-		lDriveM.pidWrite(c.lDrive);
-		rDriveM.pidWrite(-c.rDrive);
-		climber.pidWrite(c.climber);
-		camRotate.pidWrite(c.camRotate);
-		camTilt.pidWrite(c.camTilt);
-
-		shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
-		ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
-
-		double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);
-		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
-		SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));
-		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
-		SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));
-		SmartDashboard.putNumber("rate", rate);
-		if (rate > maxRate)
-			maxRate = rate;
-		SmartDashboard.putNumber("maxRate", maxRate);
-
-		SmartDashboard.putBoolean("highGear", c.highGear);
-		SmartDashboard.putBoolean("gedOut", c.gedOut);
-
-		//PDP.updateTable();
-		//SmartDashboard.putData("PDP", PDP);
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Robot.java b/src/org/usfirst/frc/team4272/robot2016/Robot.java
deleted file mode 100644
index bb15ccc..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/Robot.java
+++ /dev/null
@@ -1,61 +0,0 @@
-
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.IterativeRobot;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the IterativeRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
- * directory.
- */
-public class Robot extends IterativeRobot {
-	private final HwRobot robot = new HwRobot();
-	private final HwOI oi = new HwOI();
-	private final Control control = new Control();
-
-	private Autonomous auto;
-	private Teleop teleop;
-	/**
-	 * This function is run when the robot is first started up and should be
-	 * used for any initialization code.
-	 */
-	public void robotInit() {
-	}
-
-	public void autonomousInit() {
-		try {
-			auto = new Autonomous(robot);
-		} catch (Exception e) {}
-	}
-	public void autonomousPeriodic() {
-		try {
-			robot.run(auto.run(control));
-		} catch (Exception e) {}
-	}
-
-	public void teleopInit() {
-		try {
-			teleop = new Teleop(robot);
-		} catch (Exception e) {}
-	}
-
-	public void teleopPeriodic() {
-		try {
-			robot.run(teleop.run(control, oi));
-		} catch (Exception e) {}
-	}
-
-	public void disabledInit() {
-	}
-
-	public void disabledPeriodic() {
-	}
-
-	/**
-	 * This function is called periodically during test mode
-	 */
-	public void testPeriodic() {
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
deleted file mode 100644
index 625e29c..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ /dev/null
@@ -1,111 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-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;
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
new file mode 100644
index 0000000..f3ce43f
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -0,0 +1,47 @@
+package org.usfirst.frc.team4272.robot2017;
+
+public class Autonomous {
+	private final HwRobot robot;
+
+	public Autonomous(HwRobot robot) {
+		this.robot = robot;
+		robot.lDriveE.reset();
+		robot.rDriveE.reset();
+	}
+
+	public Control run(Control c) {
+		double lDist = robot.lDriveE.getDistance();
+		double rDist = robot.rDriveE.getDistance();
+
+		double speed = 0.6;
+		double thresh = 10;
+		double target = 15;
+		double max = 16;
+		
+		if (lDist < thresh) {
+			c.lDrive = speed;
+		} else if (lDist < max) {
+			c.lDrive = speed*Math.sqrt((target-lDist)/(target-thresh));
+		} else {
+			c.lDrive = 0;
+		}
+
+		if (rDist < thresh) {
+			c.rDrive = speed;
+		} else if (rDist < max) {
+			c.rDrive = speed*Math.sqrt((target-rDist)/(target-thresh));
+		} else {
+			c.rDrive = 0;
+		}
+
+		if (Math.abs(lDist - rDist) > 0.25) {
+			if (lDist > rDist) {
+				c.lDrive *= 0.2;
+			} else {
+				c.rDrive *= 0.2;
+			}
+		}
+
+		return c;
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2017/Control.java b/src/org/usfirst/frc/team4272/robot2017/Control.java
new file mode 100644
index 0000000..0039c72
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/Control.java
@@ -0,0 +1,12 @@
+package org.usfirst.frc.team4272.robot2017;
+
+public class Control {
+	double lDrive = 0,
+	       rDrive = 0,
+	       camRotate = 0,
+	       camTilt = 0,
+	       climber = 0;
+	boolean highGear = false,
+		gedOut = false;
+	public Control() {}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2017/HwOI.java b/src/org/usfirst/frc/team4272/robot2017/HwOI.java
new file mode 100644
index 0000000..fcdfac6
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/HwOI.java
@@ -0,0 +1,10 @@
+package org.usfirst.frc.team4272.robot2017;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class HwOI {
+	public Joystick lStick = new Joystick(0);
+	public Joystick rStick = new Joystick(1);
+	public XboxController xbox = new XboxController(2);
+}
diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
new file mode 100644
index 0000000..1cb1ee9
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java
@@ -0,0 +1,87 @@
+package org.usfirst.frc.team4272.robot2017;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.Talon;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.PIDServo;
+
+public class HwRobot {
+	/* Relay == a Spike */
+	/* PCM = Pneumatics Control Module */
+
+	/* All of the numbered inputs are in the classes:
+	 * - DIO: 0-9
+	 * - Relay: 0-3
+	 * - Analog In: 0-3
+	 * - PWM: 0-9
+	 * - PCM: 0-7 (the PCM is connected via CAN).
+	 * - CAN
+	 *
+	 * For completeness, the roboRIO also has: i2c, RS-232, SPI,
+	 * RSL, 2xUSB-A, an accelerometer, and an expansion port.
+	 *
+	 * And, for communication: USB-B and Ethernet.
+	 */
+
+	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
+
+	private final PIDOutput
+	    rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */
+	    lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */
+	    climber = new Talon(/*PWM*/3), /* PDP 4 */
+	    camRotate = new PIDServo(8),
+	    camTilt = new PIDServo(9);
+	public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);
+	public final Encoder   rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);
+	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
+
+	private double maxRate = 0;
+
+	public DoubleSolenoid
+		shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),
+		ged     = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);
+	
+	public HwRobot() {
+		lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
+		rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);
+
+		lDriveE.setPIDSourceType(PIDSourceType.kRate);
+		rDriveE.setPIDSourceType(PIDSourceType.kRate);
+		//PDP.initTable(NetworkTable.getTable("PDP"));
+	}
+
+	public void run(Control c) {
+		lDriveM.pidWrite(c.lDrive);
+		rDriveM.pidWrite(-c.rDrive);
+		climber.pidWrite(c.climber);
+		camRotate.pidWrite(c.camRotate);
+		camTilt.pidWrite(c.camTilt);
+
+		shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
+		ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);
+
+		double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);
+		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
+		SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));
+		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
+		SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));
+		SmartDashboard.putNumber("rate", rate);
+		if (rate > maxRate)
+			maxRate = rate;
+		SmartDashboard.putNumber("maxRate", maxRate);
+
+		SmartDashboard.putBoolean("highGear", c.highGear);
+		SmartDashboard.putBoolean("gedOut", c.gedOut);
+
+		//PDP.updateTable();
+		//SmartDashboard.putData("PDP", PDP);
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2017/Robot.java b/src/org/usfirst/frc/team4272/robot2017/Robot.java
new file mode 100644
index 0000000..afeba04
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2017/Robot.java
@@ -0,0 +1,61 @@
+
+package org.usfirst.frc.team4272.robot2017;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+	private final HwRobot robot = new HwRobot();
+	private final HwOI oi = new HwOI();
+	private final Control control = new Control();
+
+	private Autonomous auto;
+	private Teleop teleop;
+	/**
+	 * This function is run when the robot is first started up and should be
+	 * used for any initialization code.
+	 */
+	public void robotInit() {
+	}
+
+	public void autonomousInit() {
+		try {
+			auto = new Autonomous(robot);
+		} catch (Exception e) {}
+	}
+	public void autonomousPeriodic() {
+		try {
+			robot.run(auto.run(control));
+		} catch (Exception e) {}
+	}
+
+	public void teleopInit() {
+		try {
+			teleop = new Teleop(robot);
+		} catch (Exception e) {}
+	}
+
+	public void teleopPeriodic() {
+		try {
+			robot.run(teleop.run(control, oi));
+		} catch (Exception e) {}
+	}
+
+	public void disabledInit() {
+	}
+
+	public void disabledPeriodic() {
+	}
+
+	/**
+	 * This function is called periodically during test mode
+	 */
+	public void testPeriodic() {
+	}
+}
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;
+	}
+}
-- 
cgit v1.2.3