Commit

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DevTeleop.java

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

@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AsherOrientBlue", group = "Autonomous Pathing Tuning")
+public class AsherOrientBlue extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(9.757, 84.983, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                /*
+                * Only update this path
+                 */
+                .addPath(
+                        // Line 1
+                        new BezierLine(
+                                new Point(20.500, 7.800, Point.CARTESIAN),
+                                new Point(20.500, 87.500, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(20.500, 87.500, Point.CARTESIAN),
+                                new Point(7.800, 87.500, Point.CARTESIAN)
+                        )
+                )
+
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+                /*
+                * End of only update this path
+                 */
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

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

@@ -0,0 +1,133 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.localization.Pose;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierCurve;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+@Config
+@Autonomous(name = "AsherPathBlueV1", group = "Autonomous Pathing Tuning")
+public class AsherPathBlueV1 extends OpMode {
+    private Telemetry telemetryA;
+
+    private Follower follower;
+
+    private PathChain path;
+
+    private final Pose startPose = new Pose(9.757, 84.983, 90);
+
+    /**
+     * This initializes the Follower and creates the PathChain for the "circle". Additionally, this
+     * initializes the FTC Dashboard telemetry.
+     */
+    @Override
+    public void init() {
+        follower = new Follower(hardwareMap);
+
+        follower.setMaxPower(.4);
+
+        follower.setStartingPose(startPose);
+
+        path = follower.pathBuilder()
+                /*
+                * Only update this path
+                 */
+                .addPath(
+                        // Line 1
+                        new BezierCurve(
+                                new Point(7.800, 87.5, Point.CARTESIAN),
+                                new Point(19.000, 116.000, Point.CARTESIAN),
+                                new Point(93.000, 118.000, Point.CARTESIAN),
+                                new Point(45.000, 115.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 2
+                        new BezierLine(
+                                new Point(45.000, 115.000, Point.CARTESIAN),
+                                new Point(14.000, 126.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 3
+                        new BezierCurve(
+                                new Point(14.000, 126.000, Point.CARTESIAN),
+                                new Point(43.000, 112.500, Point.CARTESIAN),
+                                new Point(64.000, 92.000, Point.CARTESIAN),
+                                new Point(77.000, 117.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 4
+                        new BezierLine(
+                                new Point(77.000, 117.000, Point.CARTESIAN),
+                                new Point(20.000, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 5
+                        new BezierCurve(
+                                new Point(20.000, 135.000, Point.CARTESIAN),
+                                new Point(113.000, 95.000, Point.CARTESIAN),
+                                new Point(69.000, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 6
+                        new BezierLine(
+                                new Point(69.000, 135.000, Point.CARTESIAN),
+                                new Point(20.500, 135.000, Point.CARTESIAN)
+                        )
+                )
+                .addPath(
+                        // Line 7
+                        new BezierCurve(
+                                new Point(20.500, 135.000, Point.CARTESIAN),
+                                new Point(101.500, 95.500, Point.CARTESIAN),
+                                new Point(72.500, 95.500, Point.CARTESIAN)
+                        )
+                )
+                .setConstantHeadingInterpolation(Math.toRadians(90)).build();
+                /*
+                * End of only update this path
+                 */
+
+        follower.followPath(path);
+
+        telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+        telemetryA.update();
+    }
+
+    /**
+     * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+     * the Telemetry, as well as the FTC Dashboard.
+     */
+    @Override
+    public void loop() {
+        follower.update();
+        if (follower.atParametricEnd()) {
+            follower.followPath(path);
+        }
+        follower.telemetryDebug(telemetryA);
+    }
+}

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

@@ -15,6 +15,8 @@ import static org.firstinspires.ftc.teamcode.PedroConstants.LEFT_ENCODER_DIRECTI
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER;
 import static org.firstinspires.ftc.teamcode.PedroConstants.RIGHT_ENCODER_DIRECTION;
 
+import android.graphics.Point;
+
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -36,10 +38,14 @@ public class DevTeleop extends OpMode {
     public LiftSubsystem lift;
     public Gamepad currentGamepad1;
     public Gamepad previousGamepad1;
+    public Gamepad currentGamepad2;
+    public Gamepad previousGamepad2;
     public DcMotor frontLeftMotor;
     public DcMotor backLeftMotor;
     public DcMotor frontRightMotor;
     public DcMotor backRightMotor;
+
+    private double MAX_POWER = .45;
     @Override
     public void init() {
         claw = new ClawSubsystem(hardwareMap, ClawSubsystem.ClawState.CLOSED);
@@ -63,25 +69,29 @@ public class DevTeleop extends OpMode {
 
         currentGamepad1 = new Gamepad();
         previousGamepad1 = new Gamepad();
+        currentGamepad2 = new Gamepad();
+        previousGamepad2 = new Gamepad();
     }
 
     public void theDrop(ArmSubsystem arm, WristSubsystem wrist) {
-
-        if (currentGamepad1.a && !previousGamepad1.a) {
+//pick up
+        if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
             wrist.floorWrist();
             arm.engageArm();
         }
 
+
+
     }
     public void thePickup(ClawSubsystem claw) {
-
-        if (currentGamepad1.x && !previousGamepad1.x) {
+//claw open close
+        if (currentGamepad2.right_bumper && !previousGamepad2.right_bumper) {
             claw.switchState();
         }
 
     }
 
-    public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
+    /* public void theLift(ArmSubsystem arm, WristSubsystem wrist) {
 
         if (currentGamepad1.b && !previousGamepad1.b) {
             arm.parkArm();
@@ -89,23 +99,58 @@ public class DevTeleop extends OpMode {
         }
 
     }
-
+*/
     public void theLowBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
-        if (currentGamepad1.y && !previousGamepad1.y) {
+//low bucket
+        if (currentGamepad2.a && !previousGamepad2.a) {
             lift.toLowBucket();
+            arm.bucketArm();
             wrist.bucketWrist();
         }
     }
 
+    public void theHighBucketScore(LiftSubsystem lift, WristSubsystem wrist, ArmSubsystem arm) {
+//high basket
+        if (currentGamepad2.b && !previousGamepad2.b) {
+            lift.toHighBucket();
+            arm.bucketArm();
+            wrist.bucketWrist();
+        }
+    }
+
+    public void theTravel(LiftSubsystem lift, ArmSubsystem arm, WristSubsystem wrist){
+//
+        if (currentGamepad2.dpad_right && !previousGamepad2.dpad_right){
+            lift.toFloor();
+            arm.bucketArm();
+            wrist.floorWrist();
+        }
+    }
+
+    public void hoverState(ArmSubsystem arm, WristSubsystem wrist, LiftSubsystem lift){
+        if (currentGamepad1.dpad_left && !previousGamepad2.dpad_left){
+            lift.toHover();
+            wrist.floorWrist();
+            arm.engageArm();
+        }
+
+
+    }
+
     @Override
     public void loop() {
         previousGamepad1.copy(currentGamepad1);
         currentGamepad1.copy(gamepad1);
+        previousGamepad2.copy(currentGamepad2);
+        currentGamepad2.copy(gamepad2);
+
 
         theDrop(arm, wrist);
         thePickup(claw);
-        theLift(arm, wrist);
+       // theLift(arm, wrist);
         theLowBucketScore(lift, wrist, arm);
+        theHighBucketScore(lift, wrist, arm);
+        theTravel(lift, arm, wrist);
 
         double max;
 
@@ -136,14 +181,15 @@ public class DevTeleop extends OpMode {
 
 
         // Send calculated power to wheels
-        frontLeftMotor.setPower(leftFrontPower);
-        frontRightMotor.setPower(rightFrontPower);
-        backLeftMotor.setPower(leftBackPower);
-        backRightMotor.setPower(rightBackPower);
+        frontLeftMotor.setPower(leftFrontPower * MAX_POWER);
+        frontRightMotor.setPower(rightFrontPower * MAX_POWER);
+        backLeftMotor.setPower(leftBackPower * MAX_POWER);
+        backRightMotor.setPower(rightBackPower * MAX_POWER);
 
         // Show the elapsed game time and wheel power.
         telemetry.addData("Front left/Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
         telemetry.addData("Back  left/Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+        telemetry.addData("Current Lift Position", lift.getPosition());
         telemetry.update();
 
     }

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

@@ -14,6 +14,7 @@ public class PedroConstants {
 
 
     // Robot motor configurations
+    public static final String BRAIN_ROT = "Sikidi rizz 360 no teleop tf2 mama mia 2cool 4skool yasyasy yasyasyasyasyasyasyaysy ohio yes heh me is moar skeebeedee than u walked and got tripped on by your aunt my very educaded mother just served us nine what? just kydinfoiwfowefwofwioefoiejfeoiwjfomdsklfnslefknesfklnkfenfenkfeknfenkfeknfenkefnk";
     public static final String FRONT_LEFT_MOTOR = "Drive front lt";
     public static final String BACK_LEFT_MOTOR  = "Drive back lt";
     public static final String FRONT_RIGHT_MOTOR  = "Drive front rt";

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class RobotConstants {
     public static double clawClose = 1.00;
-    public static double clawOpen = 0.25;
+    public static double clawOpen = 0.05;
 
     public static double armEngage = 0.5;
     public static double armPark = 0.125;
@@ -13,9 +13,10 @@ public class RobotConstants {
 
     public static double wristFloor = 0.625;
     public static double wristBucket = 0.215;
-    public static int liftToFloorPos = 0;
-    public static int liftToPoolPos = 500;
+    public static int liftToFloorPos = 20;
+    public static int liftToFloatPos = 150;
     public static int liftToLowBucketPos = 2250;
-    public static int liftToHighBucketPos = 3700;
+    public static int liftToHighBucketPos = 3900;
     public static double liftPower = .45;
+    public static int liftToHoverState = 60;
 }

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

@@ -1,9 +1,11 @@
 package org.firstinspires.ftc.teamcode.subsystem;
 
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftPower;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloatPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToFloorPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHighBucketPos;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToLowBucketPos;
-import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToPoolPos;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.liftToHoverState;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -17,7 +19,7 @@ public class LiftSubsystem {
     public RunAction toFloor, toLowBucket, toHighBucket;
 
     public enum LiftState {
-        FLOOR, LOW_BUCKET, HIGH_BUCKET, POOL
+        FLOOR, LOW_BUCKET, HIGH_BUCKET, FLOAT, HOVER
     }
 
     private LiftState liftState;
@@ -36,8 +38,8 @@ public class LiftSubsystem {
 
     public void switchState() {
         if (this.liftState == LiftState.FLOOR) {
-            this.toPool();
-        } else if (this.liftState == LiftState.POOL) {
+            this.toFloor();
+        } else if (this.liftState == LiftState.FLOAT) {
             this.toLowBucket();
         } else if (this.liftState == LiftState.LOW_BUCKET) {
             this.toHighBucket();
@@ -46,14 +48,19 @@ public class LiftSubsystem {
         }
     }
 
+     public void toHover() {
+        this.setTarget(liftToHoverState);
+        this.setState(LiftState.HOVER);
+    }
+
     public void toFloor() {
-        this.setTarget(liftToPoolPos);
+        this.setTarget(liftToFloorPos);
         this.setState(LiftState.FLOOR);
     }
 
-    public void toPool() {
-        this.setTarget(liftToPoolPos);
-        this.setState(LiftState.POOL);
+    public void toFloat() {
+        this.setTarget(liftToFloatPos);
+        this.setState(LiftState.FLOAT);
     }
 
     public void toLowBucket() {

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

@@ -18,7 +18,6 @@ import org.firstinspires.ftc.robotcore.external.Telemetry;
 public class MotorsSubsystem {
 
     public HardwareMap hardwareMap;
-
     public Telemetry telemetry;
 
     public DcMotor frontLeftMotor;
@@ -26,6 +25,12 @@ public class MotorsSubsystem {
     public DcMotor frontRightMotor;
     public DcMotor backRightMotor;
 
+    public enum TravelState {
+        PARKED, BUCKET, SUBMARINE
+    }
+
+    public TravelState travelState;
+
     public double power;
 
     public MotorsSubsystem(HardwareMap hardwareMap, Telemetry telemetry) {
@@ -114,4 +119,12 @@ public class MotorsSubsystem {
         this.telemetry.update();
     }
 
+    public void setState(TravelState travelState) {
+        this.travelState = travelState;
+    }
+
+    public TravelState getState() {
+        return this.travelState;
+    }
+
 }