stuff from Indy

1 files changed, 12 insertions, 0 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
index 22abd30..454f226 100644
--- a/src/org/usfirst/frc/team4272/robot2015/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -2,8 +2,10 @@
 package org.usfirst.frc.team4272.robot2015;
 
 
+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.io.FileWriter;
 import java.io.IOException;
@@ -18,6 +20,7 @@ import java.io.PrintWriter;
 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;
 
@@ -36,6 +39,7 @@ public class Robot extends IterativeRobot {
 	private void inLog() throws IOException {
 		log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
 		log.println("\ntime"
+			+",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"
@@ -43,7 +47,14 @@ public class Robot extends IterativeRobot {
 	}
 
 	private void doLog() throws Exception {
+		SmartDashboard.putNumber("dist", robot.rDriveE.getDistance());
+		SmartDashboard.putNumber("raw", robot.rDriveE.getRaw());
+		if (!ds.isFMSAttached())
+			return;
 		log.println(0
+			+","+ds.getAlliance()
+			+","+ds.getBatteryVoltage()
+			+","+ds.getMatchTime()
 			+","+control.lDrive
 			+","+control.rDrive
 			+","+control.winch
@@ -84,6 +95,7 @@ public class Robot extends IterativeRobot {
 	}
 
 	public void teleopInit() {
+		robot.rDriveE.reset();
 		teleop = new Teleop();
 	}