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