diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java index 1fbae43..aadcfef 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java @@ -55,10 +55,16 @@ public static Command ArmTransfer(Robot r) { public static Command ArmScore(Robot r) { return Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem); } - - public static Command SlidesZero(Robot r) { + public static Command SlidesUp(Robot r) { + return Command.create(r.verticalSlidesSubsystem::slidesUp, r.verticalSlidesSubsystem); + } + public static Command SlidesDown(Robot r) { return Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem); } + public static Command SlidesZero(Robot r) { + return Command.create(r.verticalSlidesSubsystem::resetSlideZero, r.verticalSlidesSubsystem); + } + //transfer //inc/dec diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java index 5476b31..9df0ca9 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorController.java @@ -9,12 +9,16 @@ import org.firstinspires.ftc.sixteen750.commands.driving.JoystickDriveCommand; import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalAnalogCommand; import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalSlidesCommands; +import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesCommands; public class OperatorController { public Robot robot; - public Stick horislidesLeftStick; public CommandGamepad gamepad; + public CommandButton shiftButton; + + //horizontal buttons + public Stick horislidesLeftStick; public CommandButton openClaw; public CommandButton closeClaw; public CommandButton wristPickup; @@ -23,6 +27,36 @@ public class OperatorController { public CommandButton wristDecrement; public CommandButton horislidesExtend; public CommandButton horislidesRetract; + //vertical buttons + public Stick vertslidesLeftStick; + public CommandButton bucketTransfer; + public CommandButton bucketScore; + public CommandButton bucketIncrement; + public CommandButton bucketDecrement; + public CommandButton armTransfer; + public CommandButton armScore; + public CommandButton slidesLow; + public CommandButton slidesHigh; + public CommandButton slidesZero; + public CommandButton slidesDown; + //public CommandButton wristDecrement; + + //can we make a command happen when we switch modes? like horizontal retract when vertical is active vice versa + //can make sequential on the button to retract slides when shifting + + //choice commands + public CommandButton horizontalSlides_verticalSlides; + public boolean shifted = false; + + public boolean isShifted() { + return shifted; + } + public boolean notShifted() { + return !shifted; + } + public void toggleShift() { + shifted = !shifted; + } public OperatorController(CommandGamepad g, Robot r) { robot = r; @@ -41,6 +75,14 @@ private void AssignNamedControllerButton() { horislidesLeftStick = gamepad.leftStick; horislidesExtend = gamepad.ps_cross; horislidesRetract = gamepad.ps_square; + + //choice commands + shiftButton = gamepad.ps_share; + horizontalSlides_verticalSlides = gamepad.ps_triangle; + slidesHigh = gamepad.dpadUp; + slidesLow = gamepad.dpadDown; + slidesDown = gamepad.rightStickButton; + slidesZero = gamepad.ps_options; } private void BindButtons() { @@ -49,8 +91,13 @@ private void BindButtons() { bindHorizontalSlidesControls(); bindHorizontalAnalogControls(); } - } + if (Setup.Connected.VERTICALSLIDESUBSYSTEM) { + bindVerticalSlidesControls(); + } + shiftButton.whenPressed(this::toggleShift); + } +//can we toggle between regular buttons ex:open/close claw as one button - kevin will work on that private void bindHorizontalSlidesControls() { openClaw.whenPressed(HorizontalSlidesCommands.clawOpen(robot)); closeClaw.whenPressed(HorizontalSlidesCommands.clawChomp(robot)); @@ -61,6 +108,19 @@ private void bindHorizontalSlidesControls() { horislidesExtend.whenPressed(HorizontalSlidesCommands.intake(robot)); horislidesRetract.whenPressed(HorizontalSlidesCommands.transferring(robot)); } + private void bindVerticalSlidesControls() { +// bucketTransfer.whenPressed(VerticalSlidesCommands.BucketTransfer(robot)); +// bucketScore.whenPressed(VerticalSlidesCommands.BucketEmpty(robot)); +// armTransfer.whenPressed(VerticalSlidesCommands.ArmTransfer(robot)); +// armScore.whenPressed(VerticalSlidesCommands.ArmScore(robot)); + slidesHigh.whenPressed(VerticalSlidesCommands.HighBasket(robot)); + slidesLow.whenPressed(VerticalSlidesCommands.LowBasket(robot)); + slidesDown.whenPressed(VerticalSlidesCommands.SlidesDown(robot)); + slidesZero.whenPressed(VerticalSlidesCommands.SlidesZero(robot)); + //wristDecrement.whenPressed(SlidesCommands.wristDecrement(robot)); +// slidesUpTesting.whenPressed(VerticalSlidesCommands.SlidesUp(robot)); +// slidesDownTesting.whenPressed(VerticalSlidesCommands.SlidesDown(robot)); + } public void bindHorizontalAnalogControls() { CommandScheduler.scheduleJoystick( diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorControllerVertical.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorControllerVertical.java index e9ab6f8..947d24c 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorControllerVertical.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/controls/OperatorControllerVertical.java @@ -16,6 +16,7 @@ public class OperatorControllerVertical { public CommandButton bucketTransfer; public CommandButton bucketScore; public CommandButton bucketIncrement; + public CommandButton bucketDecrement; public CommandButton armTransfer; public CommandButton armScore; public CommandButton slidesLow; @@ -33,13 +34,13 @@ public OperatorControllerVertical(CommandGamepad g, Robot r) { private void AssignNamedControllerButton() { bucketTransfer = gamepad.ps_circle; bucketScore = gamepad.ps_triangle; - bucketIncrement = armTransfer = gamepad.ps_cross; armScore = gamepad.ps_square; - slidesLow = gamepad.dpadLeft; - slidesHigh = gamepad.dpadUp; + slidesHigh = gamepad.ps_share; + slidesLow = gamepad.dpadUp; slidesDown = gamepad.dpadDown; - //wristDecrement = gamepad.dpadDown; + bucketIncrement = gamepad.dpadDown; + bucketDecrement = gamepad.dpadDown; vertslidesLeftStick = gamepad.leftStick; } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java index 05ec881..12a38a1 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/DualTeleOp.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.sixteen750.controls.DriverController; import org.firstinspires.ftc.sixteen750.controls.OperatorController; import org.firstinspires.ftc.sixteen750.helpers.StartingPosition; +import org.firstinspires.ftc.sixteen750.subsystems.HorizontalSlidesSubsystem; @TeleOp(name = "Dual Control") @SuppressWarnings("unused") @@ -48,7 +49,12 @@ public void uponInit() { @Override public void uponStart() { - robot.horizontalSlidesSubsystem.slidesin(); - robot.horizontalSlidesSubsystem.ClawWristServoTransfer(); + if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM == true) { + robot.horizontalSlidesSubsystem.slidesin(); + robot.horizontalSlidesSubsystem.ClawWristServoTransfer(); + } + if (Setup.Connected.VERTICALSLIDESUBSYSTEM){ + robot.verticalSlidesSubsystem.slidesDown(); + } } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/VertTeleOp.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/VertTeleOp.java index 46fcac7..30f0158 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/VertTeleOp.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/VertTeleOp.java @@ -51,8 +51,9 @@ public void uponInit() { @Override public void uponStart() { + if (Setup.Connected.HORIZONTALSLIDESUBSYSTEM){ robot.horizontalSlidesSubsystem.slidesin(); - robot.horizontalSlidesSubsystem.ClawWristServoTransfer(); + robot.horizontalSlidesSubsystem.ClawWristServoTransfer();} robot.verticalSlidesSubsystem.slidesDown(); } } diff --git a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java index 759d329..81f9641 100644 --- a/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java +++ b/Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/subsystems/VerticalSlidesSubsystem.java @@ -19,10 +19,10 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable { //arm servo - transfer, pickup, neutral //bucket servo - drop, pickup (long and short) - public static double LOW_BASKET = -950; - public static double HIGH_BASKET = -1350; + public static int LOW_BASKET = -450; + public static int HIGH_BASKET = -850; // public static double HIGH_POS = 1000; - public static double SLIDE_ZERO = 0; + public static int SLIDE_ZERO = 0; public static double SLIDE_POS = 0; public static double ARM_POS = 0; public static double MIN_SERVO_SPEED = -.5; @@ -56,13 +56,13 @@ public class VerticalSlidesSubsystem implements Subsystem, Loggable { @Log(name = "bucketTarget") public double bucketTargetPos; - public static PIDCoefficients PID = new PIDCoefficients(0.0, 0.0, 0.0); public Servo armServo; public Servo bucketServo; public EncodedMotor slideMotor; private boolean isHardware; - public static PIDCoefficients slidePID = new PIDCoefficients(0.0, 0.0, 0.0); + public static PIDCoefficients slidePID = new PIDCoefficients(0.0001, 0.0, 0.0); private PIDFController slidePidController; + public static double FEEDFORWARD_COEFFICIENT = 0.13; public int slideResetPos; public VerticalSlidesSubsystem(Hardware hw) { @@ -73,7 +73,9 @@ public VerticalSlidesSubsystem(Hardware hw) { armServo = hw.armservo; bucketServo = hw.bucketservo; isHardware = true; - slidePidController = new PIDFController(PID, 0, 0, 0, (x, y) -> 0.1); + slidePidController = new PIDFController(slidePID, 0, 0, 0, (ticks, velocity) -> + FEEDFORWARD_COEFFICIENT + ); resetSlideZero(); } @@ -124,16 +126,22 @@ private void setSlideTargetPosition(int p) { public void slidesDown() { // lowers the bucket system //probably going to do the slide thing with the joysticks (negative of slidesup) - setSlideMotorPower(SLIDE_ZERO); + setSlidePos(SLIDE_ZERO); } public void slideBasketLow() { //takes the arm to the first level - setSlideMotorPower(LOW_BASKET); + setSlidePos(LOW_BASKET); + } + + public void slidesUp() { + // lowers the bucket system + //probably going to do the slide thing with the joysticks (negative of slidesup) + setSlidePos(LOW_BASKET); } public void slideBasketHigh() { - setSlideMotorPower(HIGH_BASKET); + setSlidePos(HIGH_BASKET); } public void slideChamberLow() {