summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst/frc/team4272')
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/HwOI.java11
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/HwRobot.java75
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Robot.java105
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java39
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java38
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/Toggler.java13
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java251
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) {}
+}