1 files changed, 87 insertions, 0 deletions
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);
+	}
+}