random

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

@@ -87,21 +87,21 @@ public class CometBotDriveV2 extends OpMode {
         switch (state) {
             case 0:
                 moveToPathOneAndHighBucket();
-//                doArmThing1();
                 break;
             case 1:
-//                doArmThing2();
+                doArmThing();
                 state = 2;
                 break;
             case 2:
                 moveToPathTwoAndPickSampleUp();
                 break;
-//            case 3:
+            case 3:
 //                doPickUpThing();
-//                break;
+                state = 4;
-//            case 4:
+                break;
-//                moveToBasketPath3();
+            case 4:
-//                break;
+               // moveToBasketPath3();
+                break;
 //            case 5:
 //                theArmThing();
 //                break;
@@ -154,19 +154,17 @@ public class CometBotDriveV2 extends OpMode {
         return new SetStateAction(value);
     }
 
-    private void doArmThing1() {
+    private void doArmThing() {
-        lift.toHighBucketPosition();
+
+        lift.toHighBucketReverseDrop();
         arm.toBucketPosition();
+        wrist.toBucketPosition();
+        claw.openClaw();
+        wrist.toFloorPosition();
 
         state = 2;
     }
-    private void doArmThing2() {
-       wrist.toBucketPosition();
-       claw.openClaw();
-       wrist.toFloorPosition();
 
-        state = 2;
-    }
 
 //    private void theArmThing() {
 //        telemetry.addData("busy?", follower.isBusy());
@@ -192,12 +190,12 @@ public class CometBotDriveV2 extends OpMode {
         }
     }
 //
-    private void moveToPathBasketPath3() {
+    private void moveToBasketPath3() {
         if (!follower.isBusy()) {
             path3.moveToBasketPath3(follower);
-            state = 3;
+            state = 5;
-
+        }
-    }   }
+    }
 
 //    private void moveToPickupAgainPath4() {
 //        if (!followingPath) {

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

@@ -0,0 +1,35 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+
+/*
+    AutoLine# - This file does something of a path......
+
+ */
+public class HighBasketPath3 {
+
+
+    public void moveToBasketPath3(Follower robot) {
+        PathChain pathChain;
+        PathBuilder builder = new PathBuilder();
+        builder
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(26.000, 117.000, Point.CARTESIAN),
+                                new Point(10.000, 89.00, Point.CARTESIAN)
+                        )
+                )
+                .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(320));
+        pathChain = builder.build();
+        robot.followPath(pathChain);
+    }
+
+}
+

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

@@ -34,13 +34,13 @@ public class ClawSubsystem {
     }
     public void closeClaw() {
         claw.setPosition(clawClose);
-        thumbDown();
+
         state = ClawState.CLOSED;
     }
 
     public void openClaw() {
         claw.setPosition(clawOpen);
-        thumbUp();
+
         state = ClawState.OPEN;
     }
 

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

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.backwardBucketDrop;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
 
@@ -71,7 +72,9 @@ public class DualMotorSliderSubsystem {
         liftSlideLeft = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_LEFT_MOTOR);
         liftSlideRight = hardwareMap.get(DcMotorEx.class, LIFT_SLIDE_RIGHT_MOTOR);
     }
-
+    public int getCurrentPosition(){
+        return liftSlideLeft.getCurrentPosition();
+    }
     public void init() {
         /*
         Initialize the motors with the following settings (assuming slide is at the very bottom position):
@@ -141,5 +144,6 @@ public class DualMotorSliderSubsystem {
     }
 
     public void toFloorPosition(){setTargetPosition(0);}
+    public void toHighBucketReverseDrop(){setTargetPosition(backwardBucketDrop);}
 
 }

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

@@ -108,6 +108,7 @@ public class LiftArmWrist extends LinearOpMode {
                 wrist.setPosition(wrist.getPosition() - .05);
             }
 
+
             if (currentGamepad1.right_bumper && !previousGamepad1.right_bumper) {
                lift.toHighBucketPosition();
             }
@@ -124,7 +125,7 @@ public class LiftArmWrist extends LinearOpMode {
             telemetry.addData("Lift Drive Position", lift.getFixedPosition());
             telemetry.addData("Wrist Position", wrist.getPosition());
             telemetry.addData("Arm Position", arm.getPosition());
-
+            telemetry.addData("Slide Ticks", lift.getCurrentPosition());
             telemetry.update();
         }
     }