1 files changed, 105 insertions, 0 deletions
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() {
+    
+    }
+    
+}