diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2015/Robot.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2015/Robot.java | 148 |
1 files changed, 124 insertions, 24 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java index 95a47b7..f9569a6 100644 --- a/src/org/usfirst/frc/team4272/robot2015/Robot.java +++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java @@ -6,8 +6,16 @@ import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Relay; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.Timer; +import java.io.File; +import java.io.FileWriter; +import java.io.BufferedWriter; +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 @@ -18,19 +26,98 @@ import edu.wpi.first.wpilibj.Relay; public class Robot extends IterativeRobot { private HwRobot robot = new HwRobot(); private HwOI oi; + + private PrintWriter log; + + private 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; + } + } + Control control = new Control(); + /** * 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("/usr/local/frc/share/lukeshu.log", true)); + log.println("\ntime" + +",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 doLog() throws Exception { + log.println(0 + +","+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"; + } + } + + private void doRobot() { + try { + robot.lDriveM.pidWrite( control.lDrive); + robot.rDriveM.pidWrite(-control.rDrive); + robot.winchM.pidWrite(-control.winch); + robot.lIntake.set(control.lIntake); + robot.rIntake.set(control.rIntake); + robot.grab.setForward(control.grab); + robot.push.setForward(!control.push); + doLog(); + } catch (Exception e) {} } + private Timer autotime = new Timer(); public void autonomousInit() { - + autotime.reset(); + autotime.start(); + robot.rDriveE.reset(); + robot.lDriveE.reset(); } public void autonomousPeriodic() { - + if (/*autotime.get() < 4*/robot.rDriveE.getDistance() < (15.5*140*1.1)) { + control.rDrive = control.lDrive = -0.7; + } else { + control.rDrive = control.lDrive = 0; + } + doRobot(); } private Toggler grabButton = new Toggler(); @@ -50,41 +137,50 @@ public class Robot extends IterativeRobot { pushButton.update(false); ledsButton.update(false); } + + 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 void teleopPeriodic() { - /* Drive */ - robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY())); - robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY())); - + /* This runs at 50Hz */ + + /* Drive */ + control.lDrive = jsScale(oi.lStick); + control.rDrive = jsScale(oi.rStick); /* Winch */ - double w = oi.xbox.getAxis(Axis.LY); - if (Math.abs(w) < 0.1) { w = 0; } - robot.winchM.pidWrite(-w); + control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */ + if (Math.abs(control.winch) < 0.1) { control.winch = 0; } + + SmartDashboard.putNumber("winch M", -control.winch); + SmartDashboard.putNumber("winch E", robot.winchE.get()); + SmartDashboard.putBoolean("winch T", robot.winchT.get()); + SmartDashboard.putBoolean("winch B", robot.winchB.get()); /* left intake */ if (oi.xbox.getButton(Button.LB)) { - robot.lIntake.set(Relay.Value.kReverse); + control.lIntake = Relay.Value.kReverse; } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { - robot.lIntake.set(Relay.Value.kForward); + control.lIntake = Relay.Value.kForward; } else { - robot.lIntake.set(Relay.Value.kOff); + control.lIntake = Relay.Value.kOff; } + /* right intake */ if (oi.xbox.getButton(Button.RB)) { - robot.rIntake.set(Relay.Value.kReverse); + control.rIntake=(Relay.Value.kReverse); } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { - robot.rIntake.set(Relay.Value.kForward); + control.rIntake=(Relay.Value.kForward); } else { - robot.rIntake.set(Relay.Value.kOff); + control.rIntake=(Relay.Value.kOff); } - /* grab */ - robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A))); - robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B))); - if (ledsButton.update(oi.xbox.getButton(Button.RT))) { - robot.lights.set(Relay.Value.kForward); - } else { - robot.lights.set(Relay.Value.kOff); - } + control.grab = grabButton.update(oi.xbox.getButton(Button.A)); + control.push = pushButton.update(oi.xbox.getButton(Button.B)); + + doRobot(); } public void disabledInit() { @@ -95,6 +191,10 @@ public class Robot extends IterativeRobot { robot.push.setEnabled(false); } + public void disabledPeriodic() { + + } + /** * This function is called periodically during test mode */ |