diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 4ad9af3..dcddf27 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -12,10 +12,10 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.buttons.JoystickButton; -import frc.robot.commands.arm.Brake; import frc.robot.commands.climb.Shift; import frc.robot.commands.hatch.ExtendHatchManipulator; import frc.robot.commands.hatch.RetractHatchManipulator; +import frc.robot.commands.hatch.SetHatchExtender; public class OI { @@ -26,8 +26,8 @@ public class OI { private JoystickButton extendHatchManipulatorButton; private JoystickButton retractHatchManipulatorButton; - private JoystickButton diskBrakeEngage; - private JoystickButton diskBrakeDisengage; + private JoystickButton extendHatcher; + private JoystickButton retractHatcher; private JoystickButton engageButton; private JoystickButton disengageButton; @@ -38,14 +38,14 @@ private OI() { xbox = new XboxController(RobotMap.OI.XBOX); // Xbox binds - extendHatchManipulatorButton = new JoystickButton(xbox, RobotMap.OI.EXTEND_HATCH_MANIPULATOR_BUTTON); - retractHatchManipulatorButton = new JoystickButton(xbox, RobotMap.OI.RETRACT_HATCH_MANIPULATOR_BUTTON); - diskBrakeEngage = new JoystickButton(xbox, RobotMap.OI.ENGAGE_DISK_BRAKE); - diskBrakeDisengage = new JoystickButton(xbox, RobotMap.OI.DISENGAGE_DISK_BRAKE); - diskBrakeEngage.whenPressed(new Brake(true)); - diskBrakeDisengage.whenPressed(new Brake(false)); + extendHatchManipulatorButton = new JoystickButton(xbox, RobotMap.OI.EJECT_HATCH); + retractHatchManipulatorButton = new JoystickButton(xbox, RobotMap.OI.RESET_EJECTOR); + extendHatcher = new JoystickButton(xbox, RobotMap.OI.EXTEND_HATCHER); + retractHatcher = new JoystickButton(xbox, RobotMap.OI.RETRACT_HATCHER); extendHatchManipulatorButton.whenPressed(new ExtendHatchManipulator()); retractHatchManipulatorButton.whenPressed(new RetractHatchManipulator()); + extendHatcher.whenPressed(new SetHatchExtender(true)); + retractHatcher.whenPressed(new SetHatchExtender(false)); // Driver joystick binds disengageButton = new JoystickButton(leftFlight, RobotMap.OI.SHIFT_DISENGAGE_BUTTON); engageButton = new JoystickButton(leftFlight, RobotMap.OI.SHIFT_ENGAGE_BUTTON); diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 0f1eb78..adbb70e 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -25,7 +25,7 @@ public class RobotMap { // public static final int rangefinderModule = 1; public class DRIVETRAIN { - public static final int LEFT_MASTER_CHANNEL = 0; + public static final int LEFT_MASTER_CHANNEL = 17; public static final int LEFT_FOLLOWER_ONE_CHANNEL = 1; public static final int LEFT_FOLLOWER_TWO_CHANNEL = 2; public static final int RIGHT_MASTER_CHANNEL = 13; @@ -46,15 +46,17 @@ public class CLIMBER { public class CARGO_ARM { public static final int ARM_MOTOR_CHANNEL = 6; - public static final int DISK_BRAKE = 2; + public static final int DISK_BRAKE = 1; } public class HATCH_MANIPULATOR { - public static final int PUSH_FORWARD_CHANNEL = 3; + public static final int PUSH_FORWARD_CHANNEL = 0; public static final int PUSH_REVERSE_CHANNEL = 4; - public static final int CENTER_FORWARD_CHANNEL = 5; - public static final int CENTER_REVERSE_CHANNEL = 6; + public static final int CENTER_FORWARD_CHANNEL = 2; + public static final int CENTER_REVERSE_CHANNEL = 3; + + public static final int EXTENDER = 7; } public class CARGO_MANIPULATOR { @@ -73,11 +75,14 @@ public class OI { public static final int SHIFT_ENGAGE_BUTTON = 5; public static final int SHIFT_DISENGAGE_BUTTON = 11; - public static final int ENGAGE_DISK_BRAKE = 1; // A - public static final int DISENGAGE_DISK_BRAKE = 2; // B - public static final int EXTEND_HATCH_MANIPULATOR_BUTTON = 3; // X - public static final int RETRACT_HATCH_MANIPULATOR_BUTTON = 4; // Y + public static final int EXTEND_HATCHER = 1; // A + public static final int RETRACT_HATCHER = 2; // B + public static final int EJECT_HATCH = 3; // X + public static final int RESET_EJECTOR = 4; // Y } public static final int PRESSURE_SENSOR_PORT = 0; + + public static final int PCM_A = 0; + public static final int PCM_B = 1; } diff --git a/src/main/java/frc/robot/commands/arm/TwistArm.java b/src/main/java/frc/robot/commands/arm/TwistArm.java index 734c850..4774db5 100644 --- a/src/main/java/frc/robot/commands/arm/TwistArm.java +++ b/src/main/java/frc/robot/commands/arm/TwistArm.java @@ -14,12 +14,15 @@ public TwistArm() { protected void execute() { boolean lBump = OI.getInstance().getXboxLeftBumper(); boolean rBump = OI.getInstance().getXboxRightBumper(); - if (rBump) { - CargoManipulatorArm.getInstance().lift(0.5); - } else if (lBump) { - CargoManipulatorArm.getInstance().lift(-0.5); + if (lBump) { + CargoManipulatorArm.getInstance().setBrake(true); + CargoManipulatorArm.getInstance().lift(0.75); + } else if (rBump) { + CargoManipulatorArm.getInstance().setBrake(true); + CargoManipulatorArm.getInstance().lift(-0.20); } else { CargoManipulatorArm.getInstance().lift(0); + CargoManipulatorArm.getInstance().setBrake(false); } } diff --git a/src/main/java/frc/robot/commands/hatch/SetHatchExtender.java b/src/main/java/frc/robot/commands/hatch/SetHatchExtender.java new file mode 100644 index 0000000..92f21e2 --- /dev/null +++ b/src/main/java/frc/robot/commands/hatch/SetHatchExtender.java @@ -0,0 +1,24 @@ +package frc.robot.commands.hatch; + +import edu.wpi.first.wpilibj.command.Command; +import frc.robot.subsystems.HatchManipulator; + +public class SetHatchExtender extends Command { + + private boolean extended; + + public SetHatchExtender(boolean e) { + requires(HatchManipulator.getInstance()); + this.extended = e; + } + + @Override + protected void execute() { + HatchManipulator.getInstance().setExtended(extended); + } + + @Override + protected boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/sensors/PressureSensor.java b/src/main/java/frc/robot/sensors/PressureSensor.java index 91cc922..3f9b3cb 100644 --- a/src/main/java/frc/robot/sensors/PressureSensor.java +++ b/src/main/java/frc/robot/sensors/PressureSensor.java @@ -7,10 +7,6 @@ public class PressureSensor { private static final PressureSensor INSTANCE = new PressureSensor(RobotMap.PRESSURE_SENSOR_PORT); private AnalogInput ai; - /** - * The voltage into the sensor. - */ - private static final double VCC = 5.0; public PressureSensor(int inputPin) { this.ai = new AnalogInput(inputPin); @@ -21,13 +17,12 @@ private double getVoltageOut() { } /** - * Return the pressure, in PSI. Has a tolerance of 1.5% according to REV. + * Return the pressure, in PSI. This function was made by a really confident linear regression. * * @return Measured pressure, in PSI. - * @see "http://www.revrobotics.com/content/docs/REV-11-1107-DS.pdf" */ public double getPressure() { - return (250 * (this.getVoltageOut() / VCC)) - 25; + return 31.8 * this.getVoltageOut() - 8.07; } public static PressureSensor getInstance() { diff --git a/src/main/java/frc/robot/subsystems/CargoManipulatorArm.java b/src/main/java/frc/robot/subsystems/CargoManipulatorArm.java index e26d28a..06203b7 100644 --- a/src/main/java/frc/robot/subsystems/CargoManipulatorArm.java +++ b/src/main/java/frc/robot/subsystems/CargoManipulatorArm.java @@ -20,7 +20,7 @@ private CargoManipulatorArm() { armMotor = new TalonSRX(RobotMap.CARGO_ARM.ARM_MOTOR_CHANNEL); armMotor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - diskBrake = new Solenoid(RobotMap.CARGO_ARM.DISK_BRAKE); + diskBrake = new Solenoid(RobotMap.PCM_B, RobotMap.CARGO_ARM.DISK_BRAKE); } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index d4d5eaa..d782e1d 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -11,7 +11,7 @@ public class Climber extends Subsystem { private static Climber INSTANCE = new Climber(); private Climber() { - this.shifter = new DoubleSolenoid(RobotMap.CLIMBER.SHIFTER_FORWARD_CHANNEL, RobotMap.CLIMBER.SHIFTER_REVERSE_CHANNEL); + this.shifter = new DoubleSolenoid(RobotMap.PCM_A, RobotMap.CLIMBER.SHIFTER_FORWARD_CHANNEL, RobotMap.CLIMBER.SHIFTER_REVERSE_CHANNEL); } public void setShifter(DoubleSolenoid.Value value) { diff --git a/src/main/java/frc/robot/subsystems/HatchManipulator.java b/src/main/java/frc/robot/subsystems/HatchManipulator.java index 7272262..4c12bc8 100644 --- a/src/main/java/frc/robot/subsystems/HatchManipulator.java +++ b/src/main/java/frc/robot/subsystems/HatchManipulator.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; @@ -8,6 +9,7 @@ public class HatchManipulator extends Subsystem { private static final HatchManipulator INSTANCE = new HatchManipulator(); private DoubleSolenoid pusher; private DoubleSolenoid centerRod; + private Solenoid extender; // Put methods for controlling this subsystem // here. Call these from Commands. @@ -16,11 +18,13 @@ public static HatchManipulator getInstance() { } private HatchManipulator() { - this.pusher = new DoubleSolenoid(RobotMap.HATCH_MANIPULATOR.PUSH_FORWARD_CHANNEL, RobotMap.HATCH_MANIPULATOR.PUSH_REVERSE_CHANNEL); + this.pusher = new DoubleSolenoid(RobotMap.PCM_B, RobotMap.HATCH_MANIPULATOR.PUSH_FORWARD_CHANNEL, RobotMap.HATCH_MANIPULATOR.PUSH_REVERSE_CHANNEL); this.pusher.set(DoubleSolenoid.Value.kReverse); - this.centerRod = new DoubleSolenoid(RobotMap.HATCH_MANIPULATOR.CENTER_FORWARD_CHANNEL, RobotMap.HATCH_MANIPULATOR.CENTER_REVERSE_CHANNEL); + this.centerRod = new DoubleSolenoid(RobotMap.PCM_B, RobotMap.HATCH_MANIPULATOR.CENTER_FORWARD_CHANNEL, RobotMap.HATCH_MANIPULATOR.CENTER_REVERSE_CHANNEL); this.centerRod.set(DoubleSolenoid.Value.kReverse); + + extender = new Solenoid(RobotMap.HATCH_MANIPULATOR.EXTENDER); } public void ejectHatch() { @@ -31,6 +35,10 @@ public void retractPusher() { this.pusher.set(DoubleSolenoid.Value.kReverse); } + public void setExtended(boolean extended) { + this.extender.set(extended); + } + public void initDefaultCommand() { // TODO: Set the default command, if any, for a subsystem here. Example: // setDefaultCommand(new MySpecialCommand());