From 81585537acee6e6642615fc9b5ddc14e49535f0b Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Sat, 28 Feb 2015 15:46:48 -0500 Subject: Clean up --- .../usfirst/frc/team4272/robot2015/Autonomous.java | 25 ++ .../usfirst/frc/team4272/robot2015/Control.java | 15 ++ .../usfirst/frc/team4272/robot2015/HwRobot.java | 46 ++-- src/org/usfirst/frc/team4272/robot2015/Robot.java | 263 +++++++-------------- src/org/usfirst/frc/team4272/robot2015/Teleop.java | 54 +++++ 5 files changed, 196 insertions(+), 207 deletions(-) create mode 100644 src/org/usfirst/frc/team4272/robot2015/Autonomous.java create mode 100644 src/org/usfirst/frc/team4272/robot2015/Control.java create mode 100644 src/org/usfirst/frc/team4272/robot2015/Teleop.java diff --git a/src/org/usfirst/frc/team4272/robot2015/Autonomous.java b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java new file mode 100644 index 0000000..5d48588 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/Autonomous.java @@ -0,0 +1,25 @@ +package org.usfirst.frc.team4272.robot2015; + +import edu.wpi.first.wpilibj.Timer; + +public class Autonomous { + private final Timer time = new Timer(); + private final HwRobot robot; + + public Autonomous(HwRobot robot) { + this.robot = robot; + time.reset(); + time.start(); + robot.rDriveE.reset(); + robot.lDriveE.reset(); + } + + public Control run(Control c) { + if (robot.rDriveE.getDistance() < (15.5*140*1.1)) { + c.rDrive = c.lDrive = -0.7; + } else { + c.rDrive = c.lDrive = 0; + } + return c; + } +} diff --git a/src/org/usfirst/frc/team4272/robot2015/Control.java b/src/org/usfirst/frc/team4272/robot2015/Control.java new file mode 100644 index 0000000..ef5ae8e --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/Control.java @@ -0,0 +1,15 @@ +package org.usfirst.frc.team4272.robot2015; + +import edu.wpi.first.wpilibj.Relay; + +public 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; + } +} diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java index cab834e..0186565 100644 --- a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java @@ -7,7 +7,6 @@ import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.Talon; @@ -30,44 +29,37 @@ public class HwRobot { * And, for communication: USB-B and Ethernet. */ - static class OutDumper implements PIDOutput { - private String p; - public OutDumper(String prefix) { p = prefix; } - public void pidWrite(double v) { - System.out.println(p+": "+v); - } - } - static class InDumper implements PIDSource { - private String p; - public final PIDSource r; - public InDumper(String prefix, PIDSource real) { p = prefix; r = real; } - public double pidGet() { - double v = r.pidGet(); - System.out.println(p+": "+v); - return v; - } - } public Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3); - public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0), + private PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0), new Talon(/*PWM*/1)); public Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); - public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), + private PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), new Talon(/*PWM*/3)); public Encoder winchE = new Encoder(/*DIO*/4,/*DIO*/5);//), - public static DigitalInput winchT = new DigitalInput(/*DIO*/6); - public static DigitalInput winchB = new DigitalInput(/*DIO*/7); - public PIDOutput winchM = new LimitSwitchedPIDOutput( + private static DigitalInput winchT = new DigitalInput(/*DIO*/6); + private static DigitalInput winchB = new DigitalInput(/*DIO*/7); + private PIDOutput winchM = new LimitSwitchedPIDOutput( new PIDOutputSplitter(new Talon(/*PWM*/4), new Talon(/*PWM*/5)), winchT, false, winchB, false); - public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3); - public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1); - public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth); - public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth); + private DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3); + private DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1); + private Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth); + private Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth); public HwRobot() { lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); winchE.setPIDSourceParameter(PIDSourceParameter.kRate); } + + public void run(Control c) { + lDriveM.pidWrite( c.lDrive); + rDriveM.pidWrite(-c.rDrive); + winchM.pidWrite(-c.winch); + lIntake.set(c.lIntake); + rIntake.set(c.rIntake); + grab.setForward(c.grab); + push.setForward(!c.push); + } } diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java index f9569a6..22abd30 100644 --- a/src/org/usfirst/frc/team4272/robot2015/Robot.java +++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java @@ -1,19 +1,11 @@ package org.usfirst.frc.team4272.robot2015; -import org.usfirst.frc.team4272.robotlib.Toggler; -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; /** @@ -24,182 +16,93 @@ import java.io.PrintWriter; * directory. */ 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; + private final HwRobot robot = new HwRobot(); + private final HwOI oi = new HwOI(); + 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("/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"; } } - 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(); - } + public void autonomousInit() { + auto = new Autonomous(robot); + } + public void autonomousPeriodic() { + try { + robot.run(auto.run(control)); + doLog(); + } catch (Exception e) {} + } + + public void teleopInit() { + teleop = new Teleop(); + } + + public void teleopPeriodic() { + try { + robot.run(teleop.run(control, oi)); + doLog(); + } catch (Exception e) {} + } - private Toggler grabButton = new Toggler(); - private Toggler pushButton = new Toggler(); - private Toggler ledsButton = new Toggler(); - public void teleopInit() { - //robot.lDrive.reset(); - //robot.rDrive.reset(); - //robot.winch.reset(); - //robot.lDrive.enable(); - //robot.rDrive.enable(); - //robot.winch.enable(); - robot.grab.setEnabled(true); - robot.push.setEnabled(true); - oi = new HwOI(); - grabButton.update(false); - 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() { - /* This runs at 50Hz */ + public void disabledInit() { + } - /* Drive */ - control.lDrive = jsScale(oi.lStick); - control.rDrive = jsScale(oi.rStick); - /* Winch */ - 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)) { - control.lIntake = Relay.Value.kReverse; - } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { - control.lIntake = Relay.Value.kForward; - } else { - control.lIntake = Relay.Value.kOff; - } - - /* right intake */ - if (oi.xbox.getButton(Button.RB)) { - control.rIntake=(Relay.Value.kReverse); - } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { - control.rIntake=(Relay.Value.kForward); - } else { - control.rIntake=(Relay.Value.kOff); - } - - control.grab = grabButton.update(oi.xbox.getButton(Button.A)); - control.push = pushButton.update(oi.xbox.getButton(Button.B)); + public void disabledPeriodic() { + } - doRobot(); - } - - public void disabledInit() { - //robot.lDrive.reset(); - //robot.rDrive.reset(); - //robot.winch.reset(); - robot.grab.setEnabled(false); - robot.push.setEnabled(false); - } - - public void disabledPeriodic() { - - } - - /** - * This function is called periodically during test mode - */ - public void testPeriodic() { - - } - + /** + * This function is called periodically during test mode + */ + public void testPeriodic() { + } } diff --git a/src/org/usfirst/frc/team4272/robot2015/Teleop.java b/src/org/usfirst/frc/team4272/robot2015/Teleop.java new file mode 100644 index 0000000..541099c --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/Teleop.java @@ -0,0 +1,54 @@ +package org.usfirst.frc.team4272.robot2015; + +import org.usfirst.frc.team4272.robotlib.Toggler; +import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; +import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Relay; + +public class Teleop { + private Toggler grabButton = new Toggler(); + private Toggler pushButton = new Toggler(); + + public Teleop() { + } + + 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 Control run(Control control, HwOI oi) { + /* Drive */ + control.lDrive = jsScale(oi.lStick); + control.rDrive = jsScale(oi.rStick); + /* Winch */ + control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */ + if (Math.abs(control.winch) < 0.1) { control.winch = 0; } + + /* left intake */ + if (oi.xbox.getButton(Button.LB)) { + control.lIntake = Relay.Value.kReverse; + } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { + control.lIntake = Relay.Value.kForward; + } else { + control.lIntake = Relay.Value.kOff; + } + + /* right intake */ + if (oi.xbox.getButton(Button.RB)) { + control.rIntake=(Relay.Value.kReverse); + } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { + control.rIntake=(Relay.Value.kForward); + } else { + control.rIntake=(Relay.Value.kOff); + } + + control.grab = grabButton.update(oi.xbox.getButton(Button.A)); + control.push = pushButton.update(oi.xbox.getButton(Button.B)); + + return control; + } +} -- cgit v1.2.3