6 files changed, 328 insertions, 0 deletions
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;
+	}
+}