3 files changed, 191 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwOI.java b/src/org/usfirst/frc/team4272/robot2015/HwOI.java
new file mode 100644
index 0000000..9d09b65
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/HwOI.java
@@ -0,0 +1,11 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller;
+
+import edu.wpi.first.wpilibj.Joystick;
+
+public class HwOI {
+	public Joystick lStick = new Joystick(0);
+	public Joystick rStick = new Joystick(1);
+	public Xbox360Controller xbox = new Xbox360Controller(2);
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
new file mode 100644
index 0000000..22a7afc
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
@@ -0,0 +1,75 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import org.mckenzierobotics.lib.robot.PIDController;
+import org.mckenzierobotics.lib.robot.PIDOutputSplitter;
+import org.usfirst.frc.team4272.robotlib.DoubleSolenoid;
+import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.Talon;
+
+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.
+	 */
+	
+	static class OutDumper implements PIDOutput {
+		private String p;
+		public OutDumper(String prefix) { p = prefix; }
+		public void pidWrite(double v) {
+			System.out.println(p+": "+v);
+		}
+	}
+	static class InDumper implements PIDSource {
+		private String p;
+		public final PIDSource r;
+		public InDumper(String prefix, PIDSource real) { p = prefix; r = real; }
+		public double pidGet() {
+			double v = r.pidGet();
+			System.out.println(p+": "+v);
+			return v;
+		}
+	}
+	public Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
+	public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0),
+	                                                 new Talon(/*PWM*/1));
+	public Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
+	public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2),
+	                                                 new Talon(/*PWM*/3));
+	public Encoder   winchE = new Encoder(/*DIO*/4,/*DIO*/5);//),
+	public PIDOutput winchM = new LimitSwitchedPIDOutput(
+		new PIDOutputSplitter(new Talon(/*PWM*/4),
+		                      new Talon(/*PWM*/5)),
+		/*bottom*/new DigitalInput(/*DIO*/6),
+		/*top*/   new DigitalInput(/*DIO*/7),
+		true);
+	public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3);
+	public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1);
+	public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth);
+	public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth);
+	public Relay lights  = new Relay(/*Relay*/2, Relay.Direction.kForward);
+	
+	public HwRobot() {
+		lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
+		rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
+		winchE.setPIDSourceParameter(PIDSourceParameter.kRate);
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
new file mode 100644
index 0000000..95a47b7
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -0,0 +1,105 @@
+
+package org.usfirst.frc.team4272.robot2015;
+
+import org.usfirst.frc.team4272.robotlib.Toggler;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
+import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Relay;
+
+/**
+ * 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 HwRobot robot = new HwRobot();
+	private HwOI oi;
+    /**
+     * 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() {
+    	
+    }
+    public void autonomousPeriodic() {
+
+    }
+
+    private Toggler grabButton = new Toggler();
+    private Toggler pushButton = new Toggler();
+    private Toggler ledsButton = new Toggler();
+    public void teleopInit() {
+    	//robot.lDrive.reset();
+    	//robot.rDrive.reset();
+    	//robot.winch.reset();
+    	//robot.lDrive.enable();
+    	//robot.rDrive.enable();
+    	//robot.winch.enable();
+    	robot.grab.setEnabled(true);
+    	robot.push.setEnabled(true);
+    	oi = new HwOI();
+    	grabButton.update(false);
+    	pushButton.update(false);
+    	ledsButton.update(false);
+    }
+    public void teleopPeriodic() {
+        /* Drive */
+    	robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY()));
+    	robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY()));
+    	
+    	/* Winch */
+    	double w = oi.xbox.getAxis(Axis.LY);
+    	if (Math.abs(w) < 0.1) { w = 0; }
+    	robot.winchM.pidWrite(-w);
+    	
+    	/* left intake */
+    	if (oi.xbox.getButton(Button.LB)) {
+    		robot.lIntake.set(Relay.Value.kReverse);
+    	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
+    		robot.lIntake.set(Relay.Value.kForward);
+    	} else {
+    		robot.lIntake.set(Relay.Value.kOff);
+    	}
+    	/* right intake */
+    	if (oi.xbox.getButton(Button.RB)) {
+    		robot.rIntake.set(Relay.Value.kReverse);
+    	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
+    		robot.rIntake.set(Relay.Value.kForward);
+    	} else {
+    		robot.rIntake.set(Relay.Value.kOff);
+    	}
+    	
+    	/* grab */
+    	robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A)));
+    	robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B)));
+    	if (ledsButton.update(oi.xbox.getButton(Button.RT))) {
+    		robot.lights.set(Relay.Value.kForward);
+    	} else {
+    		robot.lights.set(Relay.Value.kOff);
+    	}
+    }
+    
+    public void disabledInit() {
+    	//robot.lDrive.reset();
+    	//robot.rDrive.reset();
+    	//robot.winch.reset();
+    	robot.grab.setEnabled(false);
+    	robot.push.setEnabled(false);
+    }
+    
+    /**
+     * This function is called periodically during test mode
+     */
+    public void testPeriodic() {
+    
+    }
+    
+}