From aed85a1d0c85b555da4cb10bab36910eb48e1fa3 Mon Sep 17 00:00:00 2001 From: joshua smith Date: Mon, 2 Dec 2024 20:51:54 -0500 Subject: [PATCH 1/2] all on one controller --- .../ftc16072/Mechanisms/IntakeSlides.java | 17 +++++-- .../ftc16072/OpModes/NoTurnTeleop.java | 51 +++++++++---------- 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java index 88afd16..792dd63 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeSlides.java @@ -32,9 +32,6 @@ public class IntakeSlides extends QQMechanism { private DcMotor rightIntakeSlideMotor; private DcMotor leftIntakeSlideMotor; private TouchSensor limitSwitch; -/* - private TouchSensor limitSwitch; -*/ public int currentPos; @@ -44,7 +41,7 @@ public class IntakeSlides extends QQMechanism { public static double kP = 0.01; public static double kI = 0.0; public static double kD = 0.0; - public static double kF = 0.4; + public static double kF = 0.0; public static double max = 0.8; public static double min = -max; @@ -76,8 +73,20 @@ public void manualPositionChange(int changeAmount){ } @Override public void update(Telemetry telemetry){ + if(limitSwitch.isPressed()) { + leftIntakeSlideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftIntakeSlideMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + rightIntakeSlideMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightIntakeSlideMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + if (desiredPos < 0) { + desiredPos = 0; + } + } currentPos = (leftIntakeSlideMotor.getCurrentPosition() + rightIntakeSlideMotor.getCurrentPosition())/2;//average left and right speeds double motorPower = pidfController.calculate(desiredPos,currentPos); + if (desiredPos == 0 && !limitSwitch.isPressed()){ + motorPower = -0.8; + } this.motorPower = motorPower; leftIntakeSlideMotor.setPower(motorPower); rightIntakeSlideMotor.setPower(motorPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java index f44cd29..4394ebc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java @@ -16,6 +16,7 @@ public class NoTurnTeleop extends QQOpMode{ boolean manipulatorXWasPressed; boolean chamberContactWasPressed; boolean clawWasClosed; + boolean slidesSwitchWasPressed; int contactLostPos; public void init(){ @@ -38,28 +39,30 @@ public void loop(){ } else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL); if (gamepad1.a){ + if (robot.intakeSlides.isSwitchPressed()){ + robot.scoreArm.goToIntake(); + robot.scoringClaw.open(); + robot.intakeArm.goToDropPos(); + }else { + robot.intakeSlides.startPosition(); + } + if (robot.intakeSlides.isSwitchPressed() && !slidesSwitchWasPressed){ robot.scoreArm.goToIntake(); robot.scoringClaw.open(); robot.intakeArm.goToDropPos(); + } }else if (robot.scoringClaw.isClawClosed() && !clawWasClosed) { robot.intakeClaw.open(); robot.scoreArm.goToPlace(); robot.intakeArm.transfer(); }else if (gamepad1.dpad_up){ - robot.scoreArm.manualPositionChange(MANUAL_CHANGE); + robot.intakeSlides.manualPositionChange(MANUAL_CHANGE); }else if (gamepad1.dpad_down){ - robot.scoreArm.manualPositionChange(-MANUAL_CHANGE); + robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE); } if (gamepad1.left_stick_button){ robot.controlHub.resetGyro(); } - - if (gamepad1.right_trigger > TRIGGER_THRESHOLD) { - robot.intakeClaw.open(); - robot.scoringClaw.close(); - }else if (gamepad1.right_bumper){ - robot.scoringClaw.open();} - if(robot.scoreArm.isChamberContacted()){ robot.scoreArm.goToScoring(); @@ -72,24 +75,15 @@ public void loop(){ } - if(gamepad2.y){ - robot.intakeSlides.fullExtension(); + if(gamepad1.y){ robot.intakeClaw.close(); + robot.intakeSlides.fullExtension(); } - if(gamepad2.b){ - robot.intakeSlides.halfExtension(); + if(gamepad1.b){ robot.intakeClaw.close(); + robot.intakeSlides.halfExtension(); } - if(gamepad2.a){ - robot.intakeSlides.startPosition(); - } - if(gamepad2.dpad_up){ - robot.intakeSlides.manualPositionChange(5); - } - if(gamepad2.dpad_down){ - robot.intakeSlides.manualPositionChange(-5); - } - if (gamepad2.x && !manipulatorXWasPressed){ + if (gamepad1.x && !manipulatorXWasPressed){ if(isSearching){ robot.intakeClaw.open(); robot.intakeArm.goToIntake(); @@ -99,27 +93,28 @@ public void loop(){ robot.intakeArm.searching(); isSearching = true; } - }if (isIntaking && !gamepad2.x){ + }if (isIntaking && !gamepad1.x){ robot.intakeClaw.close(); robot.intakeClaw.wristFlat(); robot.intakeArm.transfer(); isIntaking = false; } - if (gamepad2.right_bumper){ + if (gamepad1.right_bumper){ robot.intakeClaw.open(); - }else if(gamepad2.right_trigger>TRIGGER_THRESHOLD){ + }else if(gamepad1.right_trigger>TRIGGER_THRESHOLD){ robot.intakeClaw.close(); } - if (gamepad2.dpad_right){ + if (gamepad1.dpad_right){ robot.intakeClaw.adjustWrist(MANUAL_CHANGE_AMOUNT_WRIST); - }else if(gamepad2.dpad_left){ + }else if(gamepad1.dpad_left){ robot.intakeClaw.adjustWrist(-MANUAL_CHANGE_AMOUNT_WRIST); } manipulatorXWasPressed = gamepad2.x; chamberContactWasPressed = robot.scoreArm.isChamberContacted(); clawWasClosed = robot.scoringClaw.isClawClosed(); + slidesSwitchWasPressed = robot.intakeSlides.isSwitchPressed(); } } From 0fcbd9788eda41b0496755d555d12ec4d20c34ac Mon Sep 17 00:00:00 2001 From: joshua smith Date: Mon, 2 Dec 2024 21:25:49 -0500 Subject: [PATCH 2/2] full cycle works on one controller --- .../ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java index 4394ebc..b0d69fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/NoTurnTeleop.java @@ -13,7 +13,7 @@ public class NoTurnTeleop extends QQOpMode{ private boolean isPlacing = false; private boolean isSearching = false; private boolean isIntaking = false; - boolean manipulatorXWasPressed; + boolean XWasPressed; boolean chamberContactWasPressed; boolean clawWasClosed; boolean slidesSwitchWasPressed; @@ -83,9 +83,9 @@ public void loop(){ robot.intakeClaw.close(); robot.intakeSlides.halfExtension(); } - if (gamepad1.x && !manipulatorXWasPressed){ + if (gamepad1.x && !XWasPressed){ if(isSearching){ - robot.intakeClaw.open(); + robot.intakeClaw.close(); robot.intakeArm.goToIntake(); isIntaking = true; isSearching = false; @@ -111,7 +111,7 @@ public void loop(){ robot.intakeClaw.adjustWrist(-MANUAL_CHANGE_AMOUNT_WRIST); } - manipulatorXWasPressed = gamepad2.x; + XWasPressed = gamepad1.x; chamberContactWasPressed = robot.scoreArm.isChamberContacted(); clawWasClosed = robot.scoringClaw.isClawClosed(); slidesSwitchWasPressed = robot.intakeSlides.isSwitchPressed();