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.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() { } }