rename to robot2016

6 files changed, 323 insertions, 0 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
new file mode 100644
index 0000000..749e4cb
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
@@ -0,0 +1,30 @@
+package org.usfirst.frc.team4272.robot2016;
+
+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.15)) {
+			c.rDrive = c.lDrive = 0.7;
+			//initially -0.7 to drive backwards
+			//0 to 9787 for going backwards
+			//0 to negative for forward 
+			//INDY: (15.5*140*1.1)
+			//PURDUE: (15.5*140*1.15) for a drop further
+		} else {
+			c.rDrive = c.lDrive = 0;
+		}
+		return c;
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
new file mode 100644
index 0000000..490269b
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/Control.java
@@ -0,0 +1,15 @@
+package org.usfirst.frc.team4272.robot2016;
+
+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/robot2016/HwOI.java b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
new file mode 100644
index 0000000..fc5cc78
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
@@ -0,0 +1,11 @@
+package org.usfirst.frc.team4272.robot2016;
+
+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/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
new file mode 100644
index 0000000..a1072dc
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -0,0 +1,65 @@
+package org.usfirst.frc.team4272.robot2016;
+
+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.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.
+	 */
+	
+	public Encoder   lDriveE = new Encoder(/*DIO*/2,/*DIO*/3);
+	private PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0),
+	                                                 new Talon(/*PWM*/1));
+	public Encoder   rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true);
+	private PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2),
+	                                                 new Talon(/*PWM*/3));
+	public Encoder   winchE = new Encoder(/*DIO*/4,/*DIO*/5);//),
+	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);
+	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/robot2016/Robot.java b/src/org/usfirst/frc/team4272/robot2016/Robot.java
new file mode 100644
index 0000000..603b449
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/Robot.java
@@ -0,0 +1,138 @@
+
+package org.usfirst.frc.team4272.robot2016;
+
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import java.util.Date;
+import java.io.FileWriter;
+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
+ * 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 final HwRobot robot = new HwRobot();
+	private final HwOI oi = new HwOI();
+	private final DriverStation ds = DriverStation.getInstance();
+	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("/home/lvuser/tweeterbot/var/data.log", true));
+		log.println("\ntime"
+			+",ds:FMS,ds:alliance,ds:voltage,ds:FMStime"
+			+",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 doLogOnce(String tag) throws Exception {
+		log.println(new Date()
+			+","+ds.isFMSAttached()
+			+","+tag);
+	}
+
+	private void doLog() throws Exception {
+		SmartDashboard.putNumber("dist", robot.rDriveE.getDistance());
+		SmartDashboard.putNumber("raw", robot.rDriveE.getRaw());
+		if (!ds.isFMSAttached())
+			return;
+		log.println(new Date()
+			+","+ds.isFMSAttached()
+			+","+ds.getAlliance()
+			+","+ds.getBatteryVoltage()
+			+","+ds.getMatchTime()
+			+","+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";
+		}
+	}
+
+	public void autonomousInit() {
+		try {
+			auto = new Autonomous(robot);
+			doLogOnce("autonomous");
+		} catch (Exception e) {}
+	}
+	public void autonomousPeriodic() {
+		try {
+			robot.run(auto.run(control));
+			doLog();
+		} catch (Exception e) {}
+	}
+
+	public void teleopInit() {
+		try {
+			robot.rDriveE.reset();
+			teleop = new Teleop();
+			doLogOnce("teleop");
+		} catch (Exception e) {}
+	}
+
+	public void teleopPeriodic() {
+		try {
+			robot.run(teleop.run(control, oi));
+			doLog();
+		} catch (Exception e) {}
+	}
+
+	public void disabledInit() {
+		try {
+			doLogOnce("disabled");
+			log.flush();
+		} catch (Exception e) {}
+	}
+
+	public void disabledPeriodic() {
+	}
+
+	/**
+	 * This function is called periodically during test mode
+	 */
+	public void testPeriodic() {
+	}
+}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
new file mode 100644
index 0000000..4bc1b46
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -0,0 +1,64 @@
+package org.usfirst.frc.team4272.robot2016;
+
+import org.usfirst.frc.team4272.robotlib.ToggleButton;
+import org.usfirst.frc.team4272.robotlib.PushButton;
+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 ToggleButton grabButton = new ToggleButton();
+	private ToggleButton pushButton = new ToggleButton();
+	private PushButton camButton = new PushButton();
+
+	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 ) * -1;/* 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));
+		
+		if (camButton.update(oi.xbox.getButton(Button.RT))) {
+			try {
+				ProcessBuilder pb = new ProcessBuilder("/home/lvuser/tweeterbot/bin/snapPhoto");
+				pb.redirectErrorStream(true);
+				pb.start();
+			} catch (Exception e) {};
+		}
+
+		return control;
+	}
+}