diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Robot.java')
-rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Robot.java | 138 |
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() { + } +} |