working drive code arm needs work

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

@@ -118,6 +118,7 @@ public class CometBotDriveV2 extends OpMode {
                 moveToPark();
                 break;
             case 99:
+                wrist.toTravelPosition();
                 break;
             default:
                 telemetry.addLine("default");
@@ -133,7 +134,6 @@ public class CometBotDriveV2 extends OpMode {
     public void moveToPark() {
         if (runtime.seconds() > 26) {
             path6.moveToBasketPath6(follower);
-            wrist.toTravelPosition();
             state = 99;
         }
     }

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

@@ -0,0 +1,336 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
+
+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.ClawSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.ArmSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.DualMotorSliderSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
+import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
+
+public class CometBotTeleopCompetition {
+
+    /*
+    Subsystems
+     */
+    private MotorsSubsystem motors;
+    private ClawSubsystem claw;
+    private ArmSubsystem arm;
+    private WristSubsystem wrist;
+    private DualMotorSliderSubsystem lift;
+
+    /*
+    Controllers
+     */
+    public Gamepad GP1;
+    public Gamepad GP2;
+    public Gamepad currentGP1;
+    public Gamepad previousGP1;
+    public Gamepad currentGP2;
+    public Gamepad previousGP2;
+
+    /*
+    Pedro/FTC Components
+     */
+    private Follower follower;
+    private Telemetry telemetry;
+
+    /*
+    States
+     */
+    public FieldStates fieldStates;
+
+    /*
+    Configurations
+     */
+    public double currentPower = MAX_POWER;
+
+    public CometBotTeleopCompetition(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
+        this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
+
+        this.GP1 = gp1;
+        this.GP2 = gp2;
+        this.telemetry = telemetry;
+        this.currentGP1 = new Gamepad();
+        this.currentGP2 = new Gamepad();
+        this.previousGP1 = new Gamepad();
+        this.previousGP2 = new Gamepad();
+        this.fieldStates = new FieldStates();
+        this.follower = new Follower(hardwareMap);
+        claw = new ClawSubsystem(hardwareMap);
+        arm = new ArmSubsystem(hardwareMap);
+        wrist = new WristSubsystem(hardwareMap);
+        lift = new DualMotorSliderSubsystem(hardwareMap);
+    }
+
+    public void init() {
+        this.motors.init();
+        claw.init();
+        arm.initTeleOp();
+        lift.init();
+       ;
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+        claw.closeClaw();
+        wrist.initTeleOp();
+    }
+
+    public void initCloseClaw(){
+        this.motors.init();
+        this.fieldStates.setFieldLocation(FieldStates.FieldLocation.TRAVELING);
+        follower.setMaxPower(MAX_POWER);
+        follower.startTeleopDrive();
+    }
+
+    public void update() {
+        this.previousGP1.copy(currentGP1);
+        this.currentGP1.copy(this.GP1);
+        this.previousGP2.copy(currentGP2);
+        this.currentGP2.copy(this.GP2);
+//        this.moveSkyHook();
+        this.toHighBucketScore();
+        this.toLowBucketScore();
+        this.toArmParkPosition();
+        this.toArmParkThenSwitchBetweenSubmarineAndFloorPosition();
+        this.clawControl();
+//        this.decreaseMaxPower();
+//        this.increaseMaxPower();
+
+
+//        Actions.runBlocking(this.lift.toFloorPosition());
+
+        follower.setTeleOpMovementVectors(-this.GP1.left_stick_y, -this.GP1.left_stick_x, -this.GP1.right_stick_x);
+        follower.update();
+        lift.update();
+        this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
+//        this.telemetry.addData("Claw State", this.claw.getState());
+//        this.telemetry.addData("Claw Position", this.claw.getPosition());
+//        this.telemetry.addData("Wrist State", this.wrist.getState());
+//        this.telemetry.addData("Arm State", this.arm.getState());
+//        this.telemetry.addData("Lift State", this.lift.getState());
+//        this.telemetry.addData("Lift Position", this.lift.getPosition());
+        this.telemetry.addData("MaxPower", MAX_POWER);
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Decreases maximum speed by -.05
+     */
+//    public void decreaseMaxPower() {
+//        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
+//            this.currentPower = this.currentPower - .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 1
+    Button: Left Bumper
+    Assumption: Working motor mechanism
+    Action: Increases maximum speed by +.05
+     */
+//    public void increaseMaxPower() {
+//        if (this.currentGP1.right_bumper && !this.previousGP1.right_bumper) {
+//            this.currentPower = this.currentPower + .05;
+//            this.follower.setMaxPower(this.currentPower);
+//        }
+//    }
+//    public void moveSkyHook() {
+//        if (this.currentGP2.dpad_down) {
+//            hook.moveSkyHook(-1.00);
+//        }
+//        else if (this.currentGP2.dpad_up) {
+//            hook.moveSkyHook(1.00);
+//        }
+//        else{
+//            hook.moveSkyHook(0.00);
+//        }
+//
+//    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: TRIANGLE / Y
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to high bucket.  Once at high bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toHighBucketScore() {
+        if (this.currentGP2.y && !this.previousGP2.y) {
+            lift.toHighBucketPosition();
+            if (lift.getFixedPosition() >= 4450) {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45)
+                    wrist.toBucketPosition();
+                    if(wrist.getPosition() == 0.56)
+                        claw.openClaw();
+            }
+        }
+    }
+
+    public void highBucketDrop() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.5),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.5),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.5),
+//                this.claw.openClaw(),
+//                new SleepAction(.5),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.5),
+//                this.arm.toParkPosition(),
+//                this.lift.toFloorPosition(),
+//                new SleepAction(.5)
+        ));
+    }
+    public void highBucketDropAuto() {
+        Actions.runBlocking(new SequentialAction(
+//                new SleepAction(.1),
+//                this.lift.toHighBucketPosition(),
+//                new SleepAction(.25),
+//                this.arm.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.wrist.toBucketPosition(),
+//                new SleepAction(.25),
+//                this.claw.openClaw(),
+//                new SleepAction(.25),
+//                this.wrist.toFloorPosition(),
+//                new SleepAction(.25),
+//                this.arm.toParkPosition(),
+//                this.lift.toZeroPosition(),
+//                new SleepAction(.25)
+        ));
+    }
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: CIRCLE / B
+    Assumption: Claw is holding specimen, robot is facing buckets ready to score
+    Action: On button press, enter BUCKET state, arm is lifted up, wrist is lifted up and lift
+            raises to low bucket.  Once at low bucket position, move arm forward, wrist forward
+            and open claw to drop specimen into bucket.  Finally, put arm back up and wrist back up,
+            retract lift all the way down to floor position and back to TRAVELING state.
+     */
+    public void toLowBucketScore() {
+        if (this.currentGP2.b && !this.previousGP2.b) {
+        lift.toLowBucketPosition();
+            if (lift.getFixedPosition() >= 1700); {
+                arm.toBucketPosition();
+                if(arm.getPosition() == 0.45)
+                    wrist.toBucketPosition();
+                if(wrist.getPosition() == 0.56)
+                    claw.openClaw();
+            }
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: RIGHT BUMPER
+    Assumption: Working claw mechanism
+    Action: On button press, claw switches state from OPEN to CLOSE
+     */
+    public void clawControl() {
+        if (this.currentGP2.right_bumper && !this.previousGP2.right_bumper) {
+            claw.switchState();
+        }
+    }
+
+    /*
+    Type: PS4 / Logitech
+    Controller: 2
+    Button: SQUARE / X
+    Assumption: Working arm mechanism
+    Action: On button press, pulls arm up and wrist up, ideal for traveling the field when
+            holding a specimen in claws
+     */
+    public void toArmParkPosition() {
+        if (this.currentGP2.square && !this.previousGP2.square) {
+            Actions.runBlocking(new SequentialAction(
+//                    this.wrist.toFloorPosition(),
+//                    this.arm.toParkPosition(),
+//                    this.lift.toFloorPosition()
+            ));
+        }
+    }
+
+
+
+
+    /*
+    Type: PS4
+    Controller: 2
+    Button: CROSS / A
+    Assumption: Working claw, arm and wrist mechanisms
+    Action: On button press, if arm is in PARK (toArmParkPosition), drop the arm to SUBMARINE
+            position.  SUBMARINE position means the arm and wrist are parallel to the floor, raised
+            3 INCHES off the ground.  This state is ideal for moving the arm into the SUBMARINE
+            area of the field.
+
+            When arm is in SUBMARINE position, pressing the button again puts the arm and wrist into
+            FLOOR state.  This angles the arm and wrist down so that it is able to pick specimens
+            from within the SUBMARINE floor.
+     */
+    public void toArmParkThenSwitchBetweenSubmarineAndFloorPosition() {
+        if (this.currentGP2.cross && !previousGP2.cross) {
+//            if (this.arm.getState() == ArmActionsSubsystem.ArmState.PARK) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+//                                this.arm.toSubmarinePosition(),
+//                                this.wrist.toFloorPosition()
+//                        )
+//                );
+//            } else if (this.arm.getState() == ArmActionsSubsystem.ArmState.SUBMARINE && this.wrist.getState() == WristActionsSubsystem.WristState.FLOOR) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+////                                this.lift.toFloorPosition(),
+//                                this.arm.toSubmarinePosition(),
+//                                this.wrist.toPickupPosition()
+//                        )
+//                );
+//            } else if (this.wrist.getState() == WristActionsSubsystem.WristState.PICKUP) {
+//                Actions.runBlocking(
+//                        new SequentialAction(
+//                                //  this.arm.toSubmarinePosition(),
+//                                this.wrist.toFloorPosition()
+//                        )
+//                );
+//            }
+        }
+    }
+
+
+
+
+
+
+
+
+
+
+}
+
+
+

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

@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.cometbots.paths;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+
+@TeleOp(name = "ComeBot Teleop Drive V2", group = "Competition")
+public class CometBotTeleopDriveV2 extends OpMode {
+
+    public CometBotTeleopCompetition runMode;
+
+    @Override
+    public void init() {
+        this.runMode = new CometBotTeleopCompetition(hardwareMap, telemetry, gamepad1, gamepad2);
+        this.runMode.init();
+
+    }
+
+    @Override
+    public void loop() {
+        this.runMode.update();
+        telemetry.update();
+    }
+}

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

@@ -22,9 +22,10 @@ public class HighBasketPath6 {
         builder
                 .addPath(
                         // Line 1
-                        new BezierLine(
-                                new Point(27.000, 128.500, Point.CARTESIAN),
-                                new Point(104.00, 100.00, Point.CARTESIAN)
+                        new BezierCurve(
+                                new Point(27.000, 128.000, Point.CARTESIAN),
+                                new Point(80.000, 130.000, Point.CARTESIAN),
+                                new Point(100.000, 118.000, Point.CARTESIAN)
                         )
                 )
                 .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(270));