diff --git a/.gitignore b/.gitignore index f178050..1c3574f 100644 --- a/.gitignore +++ b/.gitignore @@ -75,4 +75,5 @@ hs_err_pid* .idea/* +bin/ wpilib/ \ No newline at end of file diff --git a/CODEOWNERS b/CODEOWNERS index 9a4d760..59233ca 100644 --- a/CODEOWNERS +++ b/CODEOWNERS @@ -18,7 +18,6 @@ # Samuel Piltch - Elevator -/src/org/usfirst/frc/team334/robot/subsystems/Drive.java @samuelpiltch /src/org/usfirst/frc/team334/robot/subsystems/Elevator.java @samuelpiltch /src/org/usfirst/frc/team334/robot/commands/Elevator @samuelpiltch diff --git a/bin/org/usfirst/frc/team334/robot/OI.class b/bin/org/usfirst/frc/team334/robot/OI.class index c2d30de..2b20772 100644 Binary files a/bin/org/usfirst/frc/team334/robot/OI.class and b/bin/org/usfirst/frc/team334/robot/OI.class differ diff --git a/bin/org/usfirst/frc/team334/robot/subsystems/elevator.class b/bin/org/usfirst/frc/team334/robot/subsystems/elevator.class index d27e57d..e66a3b4 100644 Binary files a/bin/org/usfirst/frc/team334/robot/subsystems/elevator.class and b/bin/org/usfirst/frc/team334/robot/subsystems/elevator.class differ diff --git a/src/org/usfirst/frc/team334/robot/Constants.java b/src/org/usfirst/frc/team334/robot/Constants.java index dcc5c5a..1fc03e1 100644 --- a/src/org/usfirst/frc/team334/robot/Constants.java +++ b/src/org/usfirst/frc/team334/robot/Constants.java @@ -15,10 +15,16 @@ public class Constants { */ public static final int SWITCH_GEAR_CONTROL = 0; + public static final int ELEVATOR_CONTROL = 1; // Button mappings. The controller device for each button is set above. public static final int SWITCH_GEAR_BUTTON = 1; - + + public static final int ELEVATOR_TO_SWITCH_BUTTON = 2; + public static final int ELEVATOR_TO_SCALE_BUTTON = 3; + public static final int ELEVATOR_TO_EXCHANGE_BUTTON = 4; + public static final int COLLAPSE_ELEVATOR_BUTTON = 5; + // Motors public static final int DRIVETRAIN_LEFT = 0; public static final int DRIVETRAIN_RIGHT = 1; @@ -35,14 +41,14 @@ public class Constants { public static final int FORCE_COMPRESSOR_BUTTON = 2; // Elevator - public static final int ELEVATOR_MOTOR_LEFT = 6; - public static final int ELEVATOR_MOTOR_RIGHT = 7; + public static final int ELEVATOR_MOTOR_LEFT = -1; + public static final int ELEVATOR_MOTOR_RIGHT = -1; // Elevator Sensors (Limit Switches + Encoder) - public static final int ELEVATOR_LIMIT_TOP = 0; - public static final int ELEVATOR_LIMIT_BOTTOM = 2; - public static final int ELEVATOR_ENCODER_A = 4; - public static final int ELEVATOR_ENCODER_B = 5; + public static final int ELEVATOR_LIMIT_TOP = -1; + public static final int ELEVATOR_LIMIT_BOTTOM = -1; + public static final int ELEVATOR_ENCODER_A = -1; + public static final int ELEVATOR_ENCODER_B = -1; // Elevator PID public static final double ELEVATOR_P = 0; public static final double ELEVATOR_I = 0; @@ -51,6 +57,7 @@ public class Constants { public static final int ELEVATOR_SWITCH = 5; public static final int ELEVATOR_SCALE = 5; public static final int ELEVATOR_EXCHANGE = 5; + public static final int ELEVATOR_BOTTOM = 5; // Ultrasonics public static final int ULTRASONIC_L_DRIVETRAIN_PING = 0; diff --git a/src/org/usfirst/frc/team334/robot/OI.java b/src/org/usfirst/frc/team334/robot/OI.java index 200563a..c2a1bd0 100644 --- a/src/org/usfirst/frc/team334/robot/OI.java +++ b/src/org/usfirst/frc/team334/robot/OI.java @@ -4,6 +4,10 @@ import java.util.Arrays; import org.usfirst.frc.team334.robot.commands.Drivetrain.ToggleTransmissionCommand; +import org.usfirst.frc.team334.robot.commands.Elevator.CollapseElevatorCommand; +import org.usfirst.frc.team334.robot.commands.Elevator.SetElevatorToExchangeCommand; +import org.usfirst.frc.team334.robot.commands.Elevator.SetElevatorToScaleCommand; +import org.usfirst.frc.team334.robot.commands.Elevator.SetElevatorToSwitchCommand; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; @@ -24,14 +28,23 @@ public OI() { leftJoystick = new Joystick(Constants.JOYSTICK_LEFT); rightJoystick = new Joystick(Constants.JOYSTICK_RIGHT); xbox = new XboxController(Constants.XBOX); - controls = new ArrayList<>( - Arrays.asList(leftJoystick, rightJoystick, xbox)); + controls = new ArrayList<>(Arrays.asList(leftJoystick, rightJoystick, xbox)); // Init Buttons Button shiftGears = new JoystickButton(controls.get(Constants.SWITCH_GEAR_CONTROL), Constants.SWITCH_GEAR_BUTTON); + Button raiseElevatorToSwitch = new JoystickButton(controls.get(Constants.ELEVATOR_CONTROL), Constants.ELEVATOR_TO_SWITCH_BUTTON); + Button raiseElevatorToScale = new JoystickButton(controls.get(Constants.ELEVATOR_CONTROL), Constants.ELEVATOR_TO_SCALE_BUTTON); + Button raiseElevatorToExchange = new JoystickButton(controls.get(Constants.ELEVATOR_CONTROL), Constants.ELEVATOR_TO_EXCHANGE_BUTTON); + Button collapseElevator = new JoystickButton(controls.get(Constants.ELEVATOR_CONTROL), Constants.COLLAPSE_ELEVATOR_BUTTON); + // Button Actions shiftGears.whenPressed(new ToggleTransmissionCommand()); + + raiseElevatorToSwitch.whenPressed(new SetElevatorToSwitchCommand()); + raiseElevatorToScale.whenPressed(new SetElevatorToScaleCommand()); + raiseElevatorToExchange.whenPressed(new SetElevatorToExchangeCommand()); + collapseElevator.whenPressed(new CollapseElevatorCommand()); } public Joystick getLeftJoystick() { diff --git a/src/org/usfirst/frc/team334/robot/commands/Elevator/CollapseElevatorCommand.java b/src/org/usfirst/frc/team334/robot/commands/Elevator/CollapseElevatorCommand.java index 84e0c6e..99f00ac 100644 --- a/src/org/usfirst/frc/team334/robot/commands/Elevator/CollapseElevatorCommand.java +++ b/src/org/usfirst/frc/team334/robot/commands/Elevator/CollapseElevatorCommand.java @@ -1,5 +1,6 @@ package org.usfirst.frc.team334.robot.commands.Elevator; +import org.usfirst.frc.team334.robot.Constants; import org.usfirst.frc.team334.robot.Robot; import edu.wpi.first.wpilibj.command.Command; @@ -15,13 +16,18 @@ public CollapseElevatorCommand() { @Override protected void initialize() { System.out.println("COLLAPSING ELEVATOR"); + + Robot.sElevator.setSetpoint(Constants.ELEVATOR_BOTTOM); + Robot.sElevator.enable(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.sElevator.setSetpoint(0); - Robot.sElevator.enable(); + // End command if either limit switches are triggered. + if (Robot.sElevator.getTopLimitSwitch() || Robot.sElevator.getBottomLimitSwitch()) { + end(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToExchangeCommand.java b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToExchangeCommand.java index 7c81add..4b2276d 100644 --- a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToExchangeCommand.java +++ b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToExchangeCommand.java @@ -15,13 +15,18 @@ public SetElevatorToExchangeCommand() { @Override protected void initialize() { System.out.println("MOVING ELEVATOR TO EXCHANGE POSITION"); + + Robot.sElevator.setSetpoint(Constants.ELEVATOR_EXCHANGE); + Robot.sElevator.enable(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.sElevator.setSetpoint(Constants.ELEVATOR_EXCHANGE); - Robot.sElevator.enable(); + // End command if either limit switches are triggered. + if (Robot.sElevator.getTopLimitSwitch() || Robot.sElevator.getBottomLimitSwitch()) { + end(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToScaleCommand.java b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToScaleCommand.java index 015643a..948ec31 100644 --- a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToScaleCommand.java +++ b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToScaleCommand.java @@ -15,13 +15,18 @@ public SetElevatorToScaleCommand() { @Override protected void initialize() { System.out.println("MOVING ELEVATOR TO SCALE POSITION"); + + Robot.sElevator.setSetpoint(Constants.ELEVATOR_SCALE); + Robot.sElevator.enable(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.sElevator.setSetpoint(Constants.ELEVATOR_SCALE); - Robot.sElevator.enable(); + // End command if either limit switches are triggered. + if (Robot.sElevator.getTopLimitSwitch() || Robot.sElevator.getBottomLimitSwitch()) { + end(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToSwitchCommand.java b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToSwitchCommand.java index 8d2a4e4..3629da6 100644 --- a/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToSwitchCommand.java +++ b/src/org/usfirst/frc/team334/robot/commands/Elevator/SetElevatorToSwitchCommand.java @@ -16,13 +16,18 @@ public SetElevatorToSwitchCommand() { @Override protected void initialize() { System.out.println("MOVING ELEVATOR TO SWITCH POSITION"); + + Robot.sElevator.setSetpoint(Constants.ELEVATOR_SWITCH); + Robot.sElevator.enable(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - Robot.sElevator.setSetpoint(Constants.ELEVATOR_SWITCH); - Robot.sElevator.enable(); + // End command if either limit switches are triggered. + if (Robot.sElevator.getTopLimitSwitch() || Robot.sElevator.getBottomLimitSwitch()) { + end(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team334/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team334/robot/subsystems/Elevator.java index 9416873..5fbf66a 100644 --- a/src/org/usfirst/frc/team334/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc/team334/robot/subsystems/Elevator.java @@ -29,8 +29,8 @@ public Elevator() { rElevatorEncoder = new Encoder(Constants.ELEVATOR_ENCODER_A, Constants.ELEVATOR_ENCODER_B); this.disable(); - this.setAbsoluteTolerance(0); - this.setOutputRange(-1, 0); + this.setAbsoluteTolerance(5); + this.setOutputRange(-1, 1); } public void setMotors(double speed) { @@ -50,8 +50,8 @@ protected double returnPIDInput() { @Override protected void usePIDOutput(double output) { -// System.out.println("Error: " + output + "Encoder Value: " + rElevatorEncoder.get()); - setMotors(output); + // System.out.println("Error: " + output + "Encoder Value: " + rElevatorEncoder.get()); + setMotors(output * .1); } public boolean getTopLimitSwitch() {