diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272')
7 files changed, 532 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/HwOI.java b/src/org/usfirst/frc/team4272/robot2015/HwOI.java new file mode 100644 index 0000000..9d09b65 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/HwOI.java @@ -0,0 +1,11 @@ +package org.usfirst.frc.team4272.robot2015; + +import org.usfirst.frc.team4272.robotlib.Xbox360Controller; + +import edu.wpi.first.wpilibj.Joystick; + +public class HwOI { + public Joystick lStick = new Joystick(0); + public Joystick rStick = new Joystick(1); + public Xbox360Controller xbox = new Xbox360Controller(2); +} diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java new file mode 100644 index 0000000..22a7afc --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java @@ -0,0 +1,75 @@ +package org.usfirst.frc.team4272.robot2015; + +import org.mckenzierobotics.lib.robot.PIDController; +import org.mckenzierobotics.lib.robot.PIDOutputSplitter; +import org.usfirst.frc.team4272.robotlib.DoubleSolenoid; +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; + +public class HwRobot { + /* Relay == a Spike */ + /* PCM = Pneumatics Control Module */ + + /* All of the numbered inputs are in the classes: + * - DIO: 0-9 + * - Relay: 0-3 + * - Analog In: 0-3 + * - PWM: 0-9 + * - PCM: 0-7 (the PCM is connected via CAN). + * - CAN + * + * For completeness, the roboRIO also has: i2c, RS-232, SPI, + * RSL, 2xUSB-A, an accelerometer, and an expansion port. + * + * 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), + new Talon(/*PWM*/1)); + public Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); + public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), + new Talon(/*PWM*/3)); + public Encoder winchE = new Encoder(/*DIO*/4,/*DIO*/5);//), + public PIDOutput winchM = new LimitSwitchedPIDOutput( + new PIDOutputSplitter(new Talon(/*PWM*/4), + new Talon(/*PWM*/5)), + /*bottom*/new DigitalInput(/*DIO*/6), + /*top*/ new DigitalInput(/*DIO*/7), + true); + 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); + public Relay lights = new Relay(/*Relay*/2, Relay.Direction.kForward); + + public HwRobot() { + lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); + rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); + winchE.setPIDSourceParameter(PIDSourceParameter.kRate); + } +} diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java new file mode 100644 index 0000000..95a47b7 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java @@ -0,0 +1,105 @@ + +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.Relay; + +/** + * 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 HwRobot robot = new HwRobot(); + private HwOI oi; + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + public void robotInit() { + + } + + public void autonomousInit() { + + } + public void autonomousPeriodic() { + + } + + 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); + } + 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())); + + /* Winch */ + double w = oi.xbox.getAxis(Axis.LY); + if (Math.abs(w) < 0.1) { w = 0; } + robot.winchM.pidWrite(-w); + + /* left intake */ + if (oi.xbox.getButton(Button.LB)) { + robot.lIntake.set(Relay.Value.kReverse); + } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { + robot.lIntake.set(Relay.Value.kForward); + } else { + robot.lIntake.set(Relay.Value.kOff); + } + /* right intake */ + if (oi.xbox.getButton(Button.RB)) { + robot.rIntake.set(Relay.Value.kReverse); + } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { + robot.rIntake.set(Relay.Value.kForward); + } else { + robot.rIntake.set(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); + } + } + + public void disabledInit() { + //robot.lDrive.reset(); + //robot.rDrive.reset(); + //robot.winch.reset(); + robot.grab.setEnabled(false); + robot.push.setEnabled(false); + } + + /** + * This function is called periodically during test mode + */ + public void testPeriodic() { + + } + +} diff --git a/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java new file mode 100644 index 0000000..8698277 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4272.robotlib; + +public class DoubleSolenoid extends edu.wpi.first.wpilibj.DoubleSolenoid { + private boolean enabled = true; + private Value value; + + public DoubleSolenoid(int forwardChannel, int reverseChannel) { + super(forwardChannel, reverseChannel); + value = get(); + } + public DoubleSolenoid(int moduleNumber, int forwardChannel, int reverseChannel) { + super(moduleNumber, forwardChannel, reverseChannel); + value = get(); + } + + public void setEnabled(boolean enabled) { + this.enabled = enabled; + if (enabled) { + set(value); + } else { + set(Value.kOff); + } + } + + public void setForward(boolean forward) { + set(forward ? Value.kForward : Value.kReverse); + } + + public boolean getForward() { + return value == Value.kForward; + } + + public void set(Value value) { + this.value = value; + if (enabled) { + super.set(value); + } + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java new file mode 100644 index 0000000..73073c2 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PIDOutput; + +public class LimitSwitchedPIDOutput implements PIDOutput { + private final PIDOutput out; + private final DigitalInput forward; + private final DigitalInput backward; + private final boolean i_dont_know_which_way_is_which; + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) { + this.out = out; + this.forward = forward; + this.backward = backward; + this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which; + } + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) { + this(out, forward,backward, false); + } + + public void pidWrite(double v) { + if (i_dont_know_which_way_is_which) { + if (forward.get() || backward.get()) { + v = 0; + } + } else { + if (forward.get()) { + v = Math.min(v, 0); + } + if (backward.get()) { + v = Math.max(v, 0); + } + } + out.pidWrite(v); + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/Toggler.java b/src/org/usfirst/frc/team4272/robotlib/Toggler.java new file mode 100644 index 0000000..03fffec --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/Toggler.java @@ -0,0 +1,13 @@ +package org.usfirst.frc.team4272.robotlib; + +public class Toggler { + private boolean prev = false; + private boolean state = false; + public boolean update(boolean next) { + if (next && ! prev) { + state = !state; + } + prev = next; + return state; + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java new file mode 100644 index 0000000..d04fa23 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java @@ -0,0 +1,251 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) Luke Shumaker 2015. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.Joystick; + +/** + * Handle input from a wired Xbox 360 controller connected to the + * Driver Station. + */ +public class Xbox360Controller extends Joystick { + /* Constants ************************************************/ + + /** + * Represents an analog axis on an Xbox 360 controller. + */ + public static enum Axis { + LX(0), LY(1), /** left trigger */ LT(2), + RX(4), RY(5), /** right trigger */ RT(3), + /** D-Pad X */ DX(6), /** D-Pad Y */ DY(7); + + private final int id; + private Axis(int id) { this.id = id; } + public int getId() { return id; } + } + + /** + * Represents a digital button on Xbox 360 controller. + */ + public static enum Button { + A(0), B(1), + X(2), Y(3), + /** left bumper */ LB(4), /** right bumper */RB( 5), + Back(6), Start(7), /*Home(8),*/ + /** left thumb */ LT(8), /** right thumb */ RT(9); + + public final int id; + private Button(int id) { this.id = id+1; } + } + + /* Constructor **********************************************/ + + /** + * Construct an instance of a joystick. + * The joystick index is the USB port on the drivers station. + * + * @param port The port on the driver station that the joystick is plugged into. + */ + public Xbox360Controller(final int port) { + super(port); + } + + /* Core functions *******************************************/ + + /** + * Get the value of an axis base on an enumerated type. + * + * @param axis The axis to read. + * @return The value of the axis. + */ + public double getAxis(Axis axis) { + return getRawAxis(axis.id); + } + + /** + * Get buttons based on an enumerated type. + * + * @param button The button to read. + * @return The state of the button. + */ + public boolean getButton(Button button) { + return getRawButton(button.id); + } + + + /* The actual code is done. The rest is boilerplate. See, + * this is why Java is terrible. Just 30% of the file + * actually doing useful stuff, the rest just filling + * interfaces. */ + /* Stupid little wrappers ***********************************/ + + /** + * Get the X value of a thumb-stick. + * + * @param hand Left stick or right? + * @return The X value of the joystick. + */ + public double getX(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LX); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RX); + return 0.0; + } + + /** + * Get the Y value of a thumb-stick. + + * @param hand Left stick or right? + * @return The Y value of the joystick. + */ + public double getY(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LY); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RY); + return 0.0; + } + + /** + * Get the value of a trigger. + * + * @param hand Left trigger or right? + * @return The trigger value. + */ + public double getZ(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LT); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RT); + return 0.0; + } + + /** + * Get the state of a bumper. + * + * @param hand Left trigger or right? + * @return the state of the bumper. + */ + public boolean getBumper(Hand hand) { + if (hand.value == Hand.kLeft.value) + return getButton(Button.LB); + if (hand.value == Hand.kRight.value) + return getButton(Button.RB); + return false; + } + + /** + * Get the state of a thumb-stick button. + * + * @param hand Left trigger or right? + * @return the state of the button. + */ + public boolean getTop(Hand hand) { + if (hand.value == Hand.kLeft.value) + return getButton(Button.LT); + if (hand.value == Hand.kRight.value) + return getButton(Button.RB); + return false; + } + + /** + * Get the state of a trigger; whether it is more than + * half-way pressed or not. + * + * @param hand Left trigger or right? + * @return The state of the trigger. + */ + public boolean getTrigger(Hand hand) { + return getZ(hand) > 0.75; + } + + /** + * Get the magnitude of the direction vector formed by the thumb-stick's + * current position relative to its origin + * + * @return The magnitude of the direction vector + */ + public double getMagnitude(Hand hand) { + return Math.sqrt(Math.pow(getX(hand), 2) + Math.pow(getY(hand), 2)); + } + + public double getMagnitude() { + return getMagnitude(Hand.kRight); + } + + /** + * Get the direction of the vector formed by the thumb-stick and its origin + * in radians + * + * @return The direction of the vector in radians + */ + public double getDirectionRadians(Hand hand) { + return Math.atan2(getX(hand), -getY(hand)); + } + + public double getDirectionRadians() { + return getDirectionRadians(Hand.kRight); + } + + /** + * Get the direction of the vector formed by the thumb-stick and its origin + * in degrees + * + * uses acos(-1) to represent Pi due to absence of readily accessable Pi + * constant in C++ + * + * @return The direction of the vector in degrees + */ + public double getDirectionDegrees(Hand hand) { + return Math.toDegrees(getDirectionRadians(hand)); + } + + public double getDirectionDegrees() { + return Math.toDegrees(getDirectionRadians(Hand.kRight)); + } + + + /* Unused wrappers for GenericHID/Joystick ******************/ + + /** + * This method is only here to complete the GenericHID interface. + * + * @return Always 0.0 + */ + public double getTwist() { + return 0.0; + } + + /** + * This method is only here to complete the GenericHID interface. + * + * @return Always 0.0 + */ + public double getThrottle() { + return 0.0; + } + + /** + * This method is only here to complete the Joystick interface. + * + * @param axis unused + * @return Always 0 + */ + public int getAxisChannel(AxisType axis) { + return 0; + } + + /** + * This method is only here to complete the Joystick interface. + * + * @param axis unused + * @param channel unused + */ + public void setAxisChannel(AxisType axis, int channel) {} +} |