stuff

3 files changed, 140 insertions, 48 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
index 22a7afc..cab834e 100644
--- a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java
@@ -1,6 +1,5 @@
 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;
@@ -55,18 +54,17 @@ public class HwRobot {
 	public 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(
 		new PIDOutputSplitter(new Talon(/*PWM*/4),
 		                      new Talon(/*PWM*/5)),
-		/*bottom*/new DigitalInput(/*DIO*/6),
-		/*top*/   new DigitalInput(/*DIO*/7),
-		true);
+		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);
-	public Relay lights  = new Relay(/*Relay*/2, Relay.Direction.kForward);
-	
+
 	public HwRobot() {
 		lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
 		rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate);
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
index 95a47b7..f9569a6 100644
--- a/src/org/usfirst/frc/team4272/robot2015/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -6,8 +6,16 @@ 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;
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the IterativeRobot
@@ -18,19 +26,98 @@ import edu.wpi.first.wpilibj.Relay;
 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;
+		}
+	}
+	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();
     }
 
     private Toggler grabButton = new Toggler();
@@ -50,41 +137,50 @@ public class Robot extends IterativeRobot {
     	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() {
-        /* Drive */
-    	robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY()));
-    	robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY()));
-    	
+    	/* This runs at 50Hz */
+
+    	/* Drive */
+    	control.lDrive = jsScale(oi.lStick);
+    	control.rDrive = jsScale(oi.rStick);
     	/* Winch */
-    	double w = oi.xbox.getAxis(Axis.LY);
-    	if (Math.abs(w) < 0.1) { w = 0; }
-    	robot.winchM.pidWrite(-w);
+    	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)) {
-    		robot.lIntake.set(Relay.Value.kReverse);
+    		control.lIntake = Relay.Value.kReverse;
     	} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
-    		robot.lIntake.set(Relay.Value.kForward);
+    		control.lIntake = Relay.Value.kForward;
     	} else {
-    		robot.lIntake.set(Relay.Value.kOff);
+    		control.lIntake = Relay.Value.kOff;
     	}
+    	  	
     	/* right intake */
     	if (oi.xbox.getButton(Button.RB)) {
-    		robot.rIntake.set(Relay.Value.kReverse);
+    		control.rIntake=(Relay.Value.kReverse);
     	} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
-    		robot.rIntake.set(Relay.Value.kForward);
+    		control.rIntake=(Relay.Value.kForward);
     	} else {
-    		robot.rIntake.set(Relay.Value.kOff);
+    		control.rIntake=(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);
-    	}
+    	control.grab = grabButton.update(oi.xbox.getButton(Button.A));
+    	control.push = pushButton.update(oi.xbox.getButton(Button.B));
+
+    	doRobot();
     }
     
     public void disabledInit() {
@@ -95,6 +191,10 @@ public class Robot extends IterativeRobot {
     	robot.push.setEnabled(false);
     }
     
+    public void disabledPeriodic() {
+    	
+    }
+    
     /**
      * This function is called periodically during test mode
      */
diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
index 73073c2..aa3aeaf 100644
--- a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
+++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java
@@ -6,33 +6,27 @@ import edu.wpi.first.wpilibj.PIDOutput;
 public class LimitSwitchedPIDOutput implements PIDOutput {
 	private final PIDOutput out;
 	private final DigitalInput forward;
-	private final DigitalInput backward;
-	private final boolean i_dont_know_which_way_is_which;
+	private final DigitalInput bakward;
+	private final boolean for_pressed;
+	private final boolean bak_pressed;
 	
-	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) {
+	public LimitSwitchedPIDOutput(PIDOutput out,
+			DigitalInput forward, boolean for_pressed,
+			DigitalInput backward, boolean back_pressed) {
 		this.out = out;
 		this.forward = forward;
-		this.backward = backward;
-		this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which;
+		this.bakward = backward;
+		this.for_pressed = for_pressed;
+		this.bak_pressed = back_pressed;
 	}
 	
 	public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) {
-		this(out, forward,backward, false);
+		this(out, forward, true, backward, true);
 	}
 	
 	public void pidWrite(double v) {
-		if (i_dont_know_which_way_is_which) {
-			if (forward.get() || backward.get()) {
-				v = 0;
-			}
-		} else {
-			if (forward.get()) {
-				v = Math.min(v, 0);
-			}
-			if (backward.get()) {
-				v = Math.max(v, 0);
-			}
-		}
+		if (forward.get() == for_pressed) { v = Math.min(v, 0); }
+		if (bakward.get() == bak_pressed) { v = Math.max(v, 0); }
 		out.pidWrite(v);
 	}
 }