Merge remote-tracking branch 'origin/branch-swiss-cheese' into branch-swiss-cheese

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotDevAuto.java

@@ -1,24 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-
-import org.firstinspires.ftc.teamcode.cometbots.CometBotAutoDevelopment;
-
-@TeleOp(name = "CometBot Auto v2", group = "Development")
-public class CometBotDevAuto extends OpMode {
-
-    public CometBotAutoDevelopment runMode;
-
-    @Override
-    public void init() {
-        runMode = new CometBotAutoDevelopment(hardwareMap, gamepad1, gamepad2);
-        runMode.init();
-    }
-
-    @Override
-    public void loop() {
-        runMode.update();
-        telemetry.update();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CometBotTeleOpCompetition.java

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.cometbots.CometBotTeleOpDevelopment;
+
+@TeleOp(name = "CometBot Drive V2.137", group = "Competition")
+public class CometBotTeleOpCompetition extends OpMode {
+
+    public CometBotTeleOpDevelopment runMode;
+
+    @Override
+    public void init() {
+        runMode = new CometBotTeleOpDevelopment(hardwareMap, gamepad1, gamepad2);
+        runMode.init();
+    }
+
+    @Override
+    public void loop() {
+        runMode.update();
+        telemetry.update();
+    }
+    public void stop(){
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java

@@ -2,27 +2,17 @@ package org.firstinspires.ftc.teamcode.cometbots;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
-import androidx.annotation.NonNull;
-
-import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.SequentialAction;
-import com.acmerobotics.roadrunner.SleepAction;
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.states.FieldStates;
 import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
-import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 
-public class CometBotAutoDevelopment {
+public class CometBotTeleOpDevelopment {
 
     /*
     Subsystems
@@ -31,7 +21,7 @@ public class CometBotAutoDevelopment {
     private ClawSubsystem claw;
     private WristSubsystem wrist;
     private ArmSubsystem arm;
-    private HangMotorSubsystem hang;
+    private HangMotorSubsystem skyhook;
     /*
     Controllers
      */
@@ -45,12 +35,12 @@ public class CometBotAutoDevelopment {
 
     private Follower follower;
 
-    public CometBotAutoDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
+    public CometBotTeleOpDevelopment(HardwareMap hardwareMap, Gamepad gamepad1, Gamepad gamepad2) {
         dualSlides = new DualMotorSliderSubsystem(hardwareMap);
         claw = new ClawSubsystem(hardwareMap);
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
-        hang = new HangMotorSubsystem(hardwareMap);
+        skyhook = new HangMotorSubsystem(hardwareMap);
 
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
@@ -66,6 +56,7 @@ public class CometBotAutoDevelopment {
         claw.init();
         wrist.initTeleOp();
         arm.initTeleOp();
+        skyhook.init();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
     }
@@ -89,41 +80,14 @@ public class CometBotAutoDevelopment {
         openClaw();
         openThumb();
         //hang
-        grabBlueberrySkyhook();
+
-        hangSkyhook();
+        hang();
-        robotDown();
-        robotUp();
-        stopHook();
         follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
         follower.update();
     }
 
-    private void grabBlueberrySkyhook() {
-        if (currentGamepad1.x) {
-            //  claw.grabBlueberry();
-            //wrist.grabBlueberrySkyhook();
-            //arm.grabBlueberrySkyhook();
-        }
 
-        /*if(arm.getState() == ArmSubsystem.ArmState.GRAB_BLUEBERRY_SKYHOOK)
-            claw.closeClaw();*/
-    }
 
-    private void hangSkyhook() {
-        if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
-            dualSlides.toHangHeight();
-            //check wrist make go throught test files again
-            //   wrist.hangBlueberrySkyhook();
-            // arm.hangBlueberrySkyhook();
-//            if (arm.getState() == ArmSubsystem.ArmState.HANG_BLUEBERRY_SKYHOOK) {
-//                try {
-//                    Thread.sleep(2000);
-//                } catch (InterruptedException e) {
-//                    throw new RuntimeException(e);
-//                }
-//            }
-        }
-    }
 
     private void openClaw() {
         if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
@@ -175,27 +139,6 @@ public class CometBotAutoDevelopment {
         }
     }
 
-    private void robotUp() {
-        if (currentGamepad1.dpad_up && !previousGamepad1.dpad_up) {
-            dualSlides.toHangBelowFloor();
-            dualSlides.update();
-            hang.hangRobot();
-        }
-
-    }
-
-    private void robotDown() {
-        if (currentGamepad1.dpad_down && !previousGamepad1.dpad_down) {
-            hang.robotToFloor();
-        }
-    }
-
-    private void stopHook() {
-        if (gamepad1.left_bumper) {
-            hang.disableMotor();
-        }
-    }
-
     private void dualSlidesToFloorPosition() {
         if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
             dualSlides.toFloorPosition();
@@ -207,5 +150,49 @@ public class CometBotAutoDevelopment {
             dualSlides.toLowBucketPosition();
         }
     }
+    private void hang(){
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
+            arm.grabBlueberrySkyhook();
+
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+        }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            dualSlides.toFixedPosition(500);
+            dualSlides.update();
+        }
+        if (gamepad1.y) {
+            arm.hangBlueberrySkyhook();
+            wrist.hangBlueberrySkyhook();
+            try {
+                Thread.sleep(500);
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            dualSlides.toFixedPosition(1800);
+            dualSlides.update();
+        }
+        if (gamepad1.right_bumper) {
+            claw.openClaw();
+        }
+        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
+            skyhook.hangRobot();
+        }
+        skyhook.setPower(0);
+        if (gamepad1.dpad_down) {
+            skyhook.disableMotor();
+            skyhook.setPower(1);
+        }
+    }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/paths/CometBotTeleopDriveV2.java

@@ -1,10 +1,12 @@
 package org.firstinspires.ftc.teamcode.cometbots.paths;
 
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 
 @TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
+@Disabled
 public class CometBotTeleopDriveV2 extends OpMode {
 
     public CometBotTeleopCompetition runMode;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -4,20 +4,23 @@ import com.acmerobotics.dashboard.config.Config;
 
 @Config
 public class RobotConstants {
-    public final static double clawClose = 0.95;
+    public final static double clawClose = 1;
     public final static double clawOpen = 0.05;
 
+
+
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
     public final static double armReverseBucket = 0.08;
     public final static double armPark = 0.33;
     //value for grabbing the hook Specimen
-    public final static double grabBlueberry = 0.35;
+    public final static double grabBlueberry = 0.69;
-    public final static double armGrabBlueberrySkyhook = 0.1;
+    public final static double armGrabBlueberrySkyhook = 0.05;
-    public final static double wristGrabBlueberrySkyhook = 0.15;
+    public final static double wristGrabBlueberrySkyhook = 0.1;
-    public final static double armHangBlueberrySkyhook = 0.23;
+    public final static double armHangBlueberrySkyhook = 0.18;
-    public final static double wristHangBlueberrySkyhook = 0.7;
+    public final static double wristHangBlueberrySkyhook = 0;
     public final static int slideHangBlueberrySkyhook = 500;
+
     public final static int slideBelowFloor = -150;
     public final static int slideSpecimanHang = 900;
     public final static int slideSpecimanRelease = 200;
@@ -38,7 +41,7 @@ public class RobotConstants {
 
     public final static double wristSpeciemen = 0.1;
 
-    public final static int toBar = 500;
+    public final static int toBar = -7000;
     public final static double wristtravel = 0.525;
 
     public final static int toFloor = 0;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java

@@ -27,7 +27,7 @@ public class ArmSubsystem {
     }
     public void grabBlueberrySkyhook(){
         setState(ArmState.GRAB_BLUEBERRY_SKYHOOK);
-        arm.setPosition(armGrabBlueberrySkyhook+ 0.3);
+        arm.setPosition(armGrabBlueberrySkyhook);
     }
     public void hangBlueberrySkyhook(){
         setState(ArmState.HANG_BLUEBERRY_SKYHOOK);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/DualMotorSliderSubsystem.java

@@ -142,10 +142,7 @@ public class DualMotorSliderSubsystem {
     public void toFixedPosition(int value) {
         setTargetPosition(value);
     }
-    public void toHangHeight(){
+    public void toHangHeight(){setTargetPosition(slideHangBlueberrySkyhook);}
-        setTargetPosition(slideHangBlueberrySkyhook);
-
-    }
     public void toHangBelowFloor(){
         setTargetPosition(slideBelowFloor);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java

@@ -30,15 +30,17 @@ public class HangMotorSubsystem {
 
     }
     public void hangRobot(){
+        hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         hang.setTargetPosition(toBar);
-
+        while(hang.isBusy()){
+            hang.setPower(1);
+        }
     }
     public void robotToFloor(){
         hang.setTargetPosition(toFloor);
     }
     public void disableMotor(){
         hang.setPower(0);
-        hang.setMotorDisable();
 
     }
     public void setPower(double power){

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HookTest.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
@@ -8,8 +9,9 @@ import org.firstinspires.ftc.teamcode.subsystem.ClawSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.HangMotorSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
-
+@TeleOp(name = "Hook Test")
 public class HookTest extends OpMode {
+
     private HangMotorSubsystem skyhook;
     private ArmSubsystem arm;
     private WristSubsystem wrist;
@@ -26,37 +28,66 @@ public class HookTest extends OpMode {
         arm.initTeleOp();
         wrist.initTeleOp();
         slides.init();
+        claw.thumbUp();
     }
 
     @Override
     public void loop() {
+        telemetry.addData("hook", skyhook.getCurrentPosition());
         telemetry.addData("arm", arm.getState());
         telemetry.addData("wrist", wrist.getState());
+        telemetry.addData("slides", slides.getCurrentPosition());
         telemetry.update();
-        if(gamepad1.a){
+        slides.update();
+
+
+        if (gamepad1.a) {
+            claw.grabBlueberry();
+            arm.setPosition(0.15);
             arm.grabBlueberrySkyhook();
             telemetry.update();
+            //claw Open small amount
+            wrist.grabBlueberrySkyhook();
+            //wrist grab in strange way
+            telemetry.update();
+
+
+            }
+        if(gamepad1.b) {
+            //confirm grab
+            claw.closeClaw();
+
+        }
+
+        if (gamepad1.x) {
+            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            slides.toFixedPosition(500);
+            slides.update();
+        }
+        if (gamepad1.y) {
+            arm.hangBlueberrySkyhook();
+            wrist.hangBlueberrySkyhook();
             try {
                 Thread.sleep(500);
             } catch (InterruptedException e) {
                 throw new RuntimeException(e);
             }
-            //claw Open small amount
+            slides.toFixedPosition(1800);
-            wrist.grabBlueberrySkyhook();
+            slides.update();
-            //wrist grab in strange way
-            telemetry.update();
         }
-        if(gamepad1.b){
+        if (gamepad1.right_bumper) {
-            //after confirmed grab, close claw
+            claw.openClaw();
-            claw.closeClaw();
         }
-        if(gamepad1.x){
+        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
-            //now slap on bar, first wrist, then arm, then claw then driver must drive away
+            skyhook.hangRobot();
         }
-        if(gamepad1.y){
+        skyhook.setPower(0);
-            //go up
+        if (gamepad1.dpad_down) {
+            skyhook.disableMotor();
+            skyhook.setPower(1);
         }
 
+
     }
 
 }