Clean up

5 files changed, 196 insertions, 207 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2015/Autonomous.java b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java
new file mode 100644
index 0000000..5d48588
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java
@@ -0,0 +1,25 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import edu.wpi.first.wpilibj.Timer;
+
+public class Autonomous {
+	private final Timer time = new Timer();
+	private final HwRobot robot;
+
+	public Autonomous(HwRobot robot) {
+		this.robot = robot;
+		time.reset();
+		time.start();
+		robot.rDriveE.reset();
+		robot.lDriveE.reset();
+	}
+
+	public Control run(Control c) {
+		if (robot.rDriveE.getDistance() < (15.5*140*1.1)) {
+			c.rDrive = c.lDrive = -0.7;
+		} else {
+			c.rDrive = c.lDrive = 0;
+		}
+		return c;
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/Control.java b/src/org/usfirst/frc/team4272/robot2015/Control.java
new file mode 100644
index 0000000..ef5ae8e
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Control.java
@@ -0,0 +1,15 @@
+package org.usfirst.frc.team4272.robot2015;
+
+import edu.wpi.first.wpilibj.Relay;
+
+public class Control {
+	double lDrive, rDrive, winch;
+	Relay.Value lIntake, rIntake;
+	boolean grab, push;
+
+	public Control() {
+		lDrive = rDrive = winch = 0;
+		grab = push = false;
+		lIntake = rIntake = Relay.Value.kOff;
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
index cab834e..0186565 100644
--- a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
@@ -7,7 +7,6 @@ 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;
@@ -30,44 +29,37 @@ public class HwRobot {
 	 * 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),
+	private 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),
+	private PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2),
 	                                                 new Talon(/*PWM*/3));
 	public Encoder   winchE = new Encoder(/*DIO*/4,/*DIO*/5);//),
-	public static DigitalInput winchT = new DigitalInput(/*DIO*/6);
-	public static DigitalInput winchB = new DigitalInput(/*DIO*/7);
-	public PIDOutput winchM = new LimitSwitchedPIDOutput(
+	private static DigitalInput winchT = new DigitalInput(/*DIO*/6);
+	private static DigitalInput winchB = new DigitalInput(/*DIO*/7);
+	private PIDOutput winchM = new LimitSwitchedPIDOutput(
 		new PIDOutputSplitter(new Talon(/*PWM*/4),
 		                      new Talon(/*PWM*/5)),
 		winchT, false, winchB, false);
-	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);
+	private DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3);
+	private DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1);
+	private Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth);
+	private Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth);
 
 	public HwRobot() {
 		lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
 		rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
 		winchE.setPIDSourceParameter(PIDSourceParameter.kRate);
 	}
+
+	public void run(Control c) {
+		lDriveM.pidWrite( c.lDrive);
+		rDriveM.pidWrite(-c.rDrive);
+		winchM.pidWrite(-c.winch);
+		lIntake.set(c.lIntake);
+		rIntake.set(c.rIntake);
+		grab.setForward(c.grab);
+		push.setForward(!c.push);
+	}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
index f9569a6..22abd30 100644
--- a/src/org/usfirst/frc/team4272/robot2015/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -1,19 +1,11 @@
 
 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.Joystick;
 import edu.wpi.first.wpilibj.Relay;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.Timer;
 
-import java.io.File;
 import java.io.FileWriter;
-import java.io.BufferedWriter;
 import java.io.IOException;
 import java.io.PrintWriter;
 /**
@@ -24,182 +16,93 @@ import java.io.PrintWriter;
  * directory.
  */
 public class Robot extends IterativeRobot {
-	private HwRobot robot = new HwRobot();
-	private HwOI oi;
-	
-	private  PrintWriter log;
-	
-	private class Control {
-		double lDrive, rDrive, winch;
-		Relay.Value lIntake, rIntake;
-		boolean grab, push;
-		public Control() {
-			lDrive = rDrive = winch = 0;
-			grab = push = false;
-			lIntake = rIntake = Relay.Value.kOff;
+	private final HwRobot robot = new HwRobot();
+	private final HwOI oi = new HwOI();
+	private final Control control = new Control();
+	private PrintWriter log;
+
+	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() {
+		try {
+			inLog();
+		} catch (Exception e) {}
+	}
+
+	private void inLog() throws IOException {
+		log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
+		log.println("\ntime"
+			+",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
+			+",i:lDrive.dist,i:lDrive.rate"
+			+",i:rDrive.dist,i:rDrive.rate"
+			+",i:winch.dist");
+	}
+
+	private void doLog() throws Exception {
+		log.println(0
+			+","+control.lDrive
+			+","+control.rDrive
+			+","+control.winch
+			+","+fmtRelay(control.lIntake)
+			+","+fmtRelay(control.rIntake)
+			+","+control.grab
+			+","+control.push
+			+","+robot.lDriveE.getDistance()
+			+","+robot.lDriveE.getRate()
+			+","+robot.rDriveE.getDistance()
+			+","+robot.rDriveE.getRate()
+			+","+robot.winchE.getRate()
+			);
+	}
+
+	private String fmtRelay(Relay.Value v) {
+		if (v == Relay.Value.kForward) {
+			return "kForward";
+		} else if (v == Relay.Value.kReverse) {
+			return "kReverse";
+		} else if (v == Relay.Value.kOn) {
+			return "kOn";
+		} else if (v == Relay.Value.kOff) {
+			return "kOff";
+		} else {
+			return "unknown";
 		}
 	}
-	Control control = new Control();
-	
-    /**
-     * This function is run when the robot is first started up and should be
-     * used for any initialization code.
-     */
-    public void robotInit() {
-    	try {
-    		inLog();
-    	} catch (Exception e) {}
-    }
-    
-    private void inLog() throws IOException {
-    	log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
-    	log.println("\ntime"
-    			+",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
-    			+",i:lDrive.dist,i:lDrive.rate"
-    			+",i:rDrive.dist,i:rDrive.rate"
-    			+",i:winch.dist");
-    }
-    
-    private void doLog() throws Exception {
-    	log.println(0
-    			+","+control.lDrive
-    			+","+control.rDrive
-    			+","+control.winch
-    			+","+fmtRelay(control.lIntake)
-    			+","+fmtRelay(control.rIntake)
-    			+","+control.grab
-    			+","+control.push
-    			+","+robot.lDriveE.getDistance()
-    			+","+robot.lDriveE.getRate()
-    			+","+robot.rDriveE.getDistance()
-    			+","+robot.rDriveE.getRate()
-    			+","+robot.winchE.getRate()
-    			);
-    }
-    
-    private String fmtRelay(Relay.Value v) {
-    	if (v == Relay.Value.kForward) {
-    		return "kForward";
-    	} else if (v == Relay.Value.kReverse) {
-    		return "kReverse";
-    	} else if (v == Relay.Value.kOn) {
-    		return "kOn";
-    	} else if (v == Relay.Value.kOff) {
-    		return "kOff";
-    	} else {
-    		return "unknown";
-    	}
-    }
-    
-    private void doRobot() {
-    	try {
-    		robot.lDriveM.pidWrite( control.lDrive);
-    		robot.rDriveM.pidWrite(-control.rDrive);    	
-    		robot.winchM.pidWrite(-control.winch);
-    		robot.lIntake.set(control.lIntake);
-    		robot.rIntake.set(control.rIntake);
-    		robot.grab.setForward(control.grab);
-    		robot.push.setForward(!control.push);
-    		doLog();
-    	} catch (Exception e) {}
-    }
 
-    private Timer autotime = new Timer();
-    public void autonomousInit() {
-    	autotime.reset();
-    	autotime.start();
-    	robot.rDriveE.reset();
-    	robot.lDriveE.reset();
-    }
-    public void autonomousPeriodic() {
-    	if (/*autotime.get() < 4*/robot.rDriveE.getDistance() < (15.5*140*1.1)) {
-    		control.rDrive = control.lDrive = -0.7;
-    	} else {
-    		control.rDrive = control.lDrive = 0;
-    	}
-    	doRobot();
-    }
+	public void autonomousInit() {
+		auto = new Autonomous(robot);
+	}
+	public void autonomousPeriodic() {
+		try {
+			robot.run(auto.run(control));
+			doLog();
+		} catch (Exception e) {}
+	}
+
+	public void teleopInit() {
+		teleop = new Teleop();
+	}
+
+	public void teleopPeriodic() {
+		try {
+			robot.run(teleop.run(control, oi));
+			doLog();
+		} catch (Exception e) {}
+	}
 
-    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);
-    }
-    
-    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);
-    }
-    
-    public void teleopPeriodic() {
-    	/* This runs at 50Hz */
+	public void disabledInit() {
+	}
 
-    	/* Drive */
-    	control.lDrive = jsScale(oi.lStick);
-    	control.rDrive = jsScale(oi.rStick);
-    	/* Winch */
-    	control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */
-    	if (Math.abs(control.winch) < 0.1) { control.winch = 0; }
-    	
-    	SmartDashboard.putNumber("winch M", -control.winch);
-    	SmartDashboard.putNumber("winch E", robot.winchE.get());
-    	SmartDashboard.putBoolean("winch T", robot.winchT.get());
-    	SmartDashboard.putBoolean("winch B", robot.winchB.get());
-    	
-    	/* left intake */
-    	if (oi.xbox.getButton(Button.LB)) {
-    		control.lIntake = Relay.Value.kReverse;
-    	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
-    		control.lIntake = Relay.Value.kForward;
-    	} else {
-    		control.lIntake = Relay.Value.kOff;
-    	}
-    	  	
-    	/* right intake */
-    	if (oi.xbox.getButton(Button.RB)) {
-    		control.rIntake=(Relay.Value.kReverse);
-    	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
-    		control.rIntake=(Relay.Value.kForward);
-    	} else {
-    		control.rIntake=(Relay.Value.kOff);
-    	}
-    	
-    	control.grab = grabButton.update(oi.xbox.getButton(Button.A));
-    	control.push = pushButton.update(oi.xbox.getButton(Button.B));
+	public void disabledPeriodic() {
+	}
 
-    	doRobot();
-    }
-    
-    public void disabledInit() {
-    	//robot.lDrive.reset();
-    	//robot.rDrive.reset();
-    	//robot.winch.reset();
-    	robot.grab.setEnabled(false);
-    	robot.push.setEnabled(false);
-    }
-    
-    public void disabledPeriodic() {
-    	
-    }
-    
-    /**
-     * This function is called periodically during test mode
-     */
-    public void testPeriodic() {
-    
-    }
-    
+	/**
+	 * This function is called periodically during test mode
+	 */
+	public void testPeriodic() {
+	}
 }
diff --git a/src/org/usfirst/frc/team4272/robot2015/Teleop.java b/src/org/usfirst/frc/team4272/robot2015/Teleop.java
new file mode 100644
index 0000000..541099c
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2015/Teleop.java
@@ -0,0 +1,54 @@
+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.Joystick;
+import edu.wpi.first.wpilibj.Relay;
+
+public class Teleop {
+	private Toggler grabButton = new Toggler();
+	private Toggler pushButton = new Toggler();
+
+	public Teleop() {
+	}
+
+	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);
+	}
+
+	public Control run(Control control, HwOI oi) {
+		/* Drive */
+		control.lDrive = jsScale(oi.lStick);
+		control.rDrive = jsScale(oi.rStick);
+		/* Winch */
+		control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */
+		if (Math.abs(control.winch) < 0.1) { control.winch = 0; }
+
+		/* left intake */
+		if (oi.xbox.getButton(Button.LB)) {
+			control.lIntake = Relay.Value.kReverse;
+		} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
+			control.lIntake = Relay.Value.kForward;
+		} else {
+			control.lIntake = Relay.Value.kOff;
+		}
+
+		/* right intake */
+		if (oi.xbox.getButton(Button.RB)) {
+			control.rIntake=(Relay.Value.kReverse);
+		} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
+			control.rIntake=(Relay.Value.kForward);
+		} else {
+			control.rIntake=(Relay.Value.kOff);
+		}
+
+		control.grab = grabButton.update(oi.xbox.getButton(Button.A));
+		control.push = pushButton.update(oi.xbox.getButton(Button.B));
+
+		return control;
+	}
+}