Moved slide manualchange to intakeslides file

ftc16072 · Dec 28, 2024 · cf332c4 · cf332c4
1 parent 02641a8
commit cf332c4
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 18 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/IntakeArm.java
@@ -14,7 +14,7 @@
 @Config
 public class IntakeArm extends QQMechanism {
     private static final double MAX_SERVO_POS = 1;
-    public static double MANUAL_CHANGE_AMOUNT = 0.005;
+    public static double MANUAL_CHANGE_AMOUNT = 0.01;
     private final static double ARM_DROP_POSITION = 0.1;
     private final static double ARM_INTAKE_POSITION = 0.62;
     Servo leftArmServo;

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
@@ -28,6 +28,7 @@ public class IntakeSlides extends QQMechanism {
     public static final int SLIDES_EXTENSION_BOUNDARY = FULL_EXTENSION_POSITION+10;
     private static final int HALF_EXTENSION_POSITION = 740;
     private static final int START_POSITION = 0;
+    public static int MANUAL_CHANGE_AMOUNT = 30;
 
     private DcMotor rightIntakeSlideMotor;
     private DcMotor leftIntakeSlideMotor;
@@ -68,7 +69,13 @@ public void init(HardwareMap hwMap) {
     public void setPosition(int desiredPos){
         this.desiredPos = desiredPos;
     }
-    public void manualPositionChange(int changeAmount){
+    public void extend(){
+        manualPositionChange(MANUAL_CHANGE_AMOUNT);
+    }
+    public void retract(){
+        manualPositionChange(-MANUAL_CHANGE_AMOUNT);
+    }
+    private void manualPositionChange(int changeAmount){
         desiredPos += changeAmount;
     }
     @Override
@@ -106,6 +113,7 @@ public void update(Telemetry telemetry){
     public boolean isSwitchPressed(){
         return limitSwitch.isPressed();
     }
+    public boolean isSafeToRotate() { return currentPos > HALF_EXTENSION_POSITION;}
 
 
 

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
@@ -8,7 +8,7 @@
 public class NoTurnTeleop extends QQOpMode {
 
     public static final double TRIGGER_THRESHOLD = 0.5;
-    public static final int MANUAL_CHANGE = 15;
+    public static final int SCORE_ARM_MANUAL_CHANGE = 15;
     public static final double MANUAL_CHANGE_AMOUNT_WRIST = 0.03;
     boolean isPlacing;
     boolean XWasPressed;
@@ -28,23 +28,24 @@ public void loop() {
         super.loop();
         double forward = -gamepad1.left_stick_y;
         double left = gamepad1.left_stick_x;
+        double rotate = 0;
         if (gamepad1.right_stick_button) {
-            double rotate = gamepad1.right_stick_x;
-            nav.driveFieldRelative(forward, left, rotate);
-        } else {
             if (gamepad1.right_stick_y < -0.2) {
-                robot.intakeSlides.manualPositionChange(MANUAL_CHANGE);
+                robot.intakeSlides.extend();
             } else if (gamepad1.right_stick_y > 0.2) {
-                robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE);
+                robot.intakeSlides.retract();
             }
-
-            if (gamepad1.right_stick_x > 0.3) {
-                robot.intakeArm.rotateArmRight();
-            } else if (gamepad1.right_stick_x < -0.3) {
-                robot.intakeArm.rotateArmLeft();
+            if (robot.intakeSlides.isSafeToRotate()) {
+                if (gamepad1.right_stick_x > 0.3) {
+                    robot.intakeArm.rotateArmRight();
+                } else if (gamepad1.right_stick_x < -0.3) {
+                    robot.intakeArm.rotateArmLeft();
+                }
             }
+        }else{
+            rotate = gamepad1.right_stick_x;
         }
-        nav.driveFieldRelative(forward, left, 0);
+        nav.driveFieldRelative(forward, left, rotate);
 
         if (gamepad1.left_trigger > TRIGGER_THRESHOLD) {
             robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO);
@@ -69,9 +70,9 @@ public void loop() {
             robot.scoreArm.goToPlace();
             robot.intakeArm.goToIntake();
         } else if (gamepad1.dpad_up) {
-            robot.scoreArm.manualPositionChange(MANUAL_CHANGE);
+            robot.scoreArm.manualPositionChange(SCORE_ARM_MANUAL_CHANGE);
         } else if (gamepad1.dpad_down) {
-            robot.scoreArm.manualPositionChange(-MANUAL_CHANGE);
+            robot.scoreArm.manualPositionChange(-SCORE_ARM_MANUAL_CHANGE);
         }
         if (gamepad1.left_stick_button) {
             robot.controlHub.resetGyro();
@@ -98,6 +99,7 @@ public void loop() {
         if (robot.intakeClaw.isClawClosed() && !intakeClawWasClosed) {
             robot.intakeClaw.wristTransfer();
             robot.intakeSlides.startPosition();
+            robot.intakeArm.goToIntake();
         }
 
         if (gamepad1.dpad_right) {
@@ -118,9 +120,9 @@ public void loop() {
             robot.scoreArm.goToMove();
         }
         if (gamepad2.dpad_up) {
-            robot.intakeSlides.manualPositionChange(MANUAL_CHANGE);
+            robot.intakeSlides.extend();
         } else if (gamepad2.dpad_down) {
-            robot.intakeSlides.manualPositionChange(-MANUAL_CHANGE);
+            robot.intakeSlides.retract();
         }
         if (gamepad2.right_trigger > TRIGGER_THRESHOLD) {
             robot.scoringClaw.open();