teleop is back up and running

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java

@@ -65,4 +65,7 @@ public class Front_Poses {
     public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
+
+    //For PedroPathing TODO: figure out how to change start poses in auto
+    public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -5,21 +5,21 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.26; //0.13;
+    public static double spindexer_intakePos1 = 0.18; //0.13;
 
-    public static double spindexer_intakePos2 = 0.45; //0.33;//0.5;
+    public static double spindexer_intakePos2 = 0.37; //0.33;//0.5;
 
-    public static double spindexer_intakePos3 = 0.63; //0.53;//0.66;
+    public static double spindexer_intakePos3 = 0.55; //0.53;//0.66;
 
-    public static double spindexer_outtakeBall3 = 0.91; //0.65; //0.24;
+    public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24;
-    public static double spindexer_outtakeBall3b = 0.35; //0.65; //0.24;
+    public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24;
 
-    public static double spindexer_outtakeBall2 = 0.73; //0.46; //0.6;
+    public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6;
-    public static double spindexer_outtakeBall1 = 0.54; //0.27; //0.4;
+    public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4;
-    public static double spinStartPos = 0.05;
+    public static double spinStartPos = 0;
     public static double spinEndPos = 0.6;
 
-    public static double shootAllSpindexerSpeedIncrease = 0.0095;
+    public static double shootAllSpindexerSpeedIncrease = 0.01;
 
     public static double transferServo_out = 0.15;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java

@@ -148,7 +148,7 @@ class LocalizationTest extends OpMode {
 
     @Override
     public void init() {
-        follower.setStartingPose(new Pose(0,0));
+        follower.setStartingPose(new Pose(72,72, 0));
     }
 
     /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -2,6 +2,9 @@ package org.firstinspires.ftc.teamcode.teleop;
 
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
+import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
@@ -12,6 +15,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDFController;
+import com.pedropathing.follower.Follower;
 import com.pedropathing.geometry.Pose;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -21,6 +25,7 @@ import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.constants.StateEnums;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.Drivetrain;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Light;
@@ -38,10 +43,11 @@ import java.util.List;
 public class TeleopV3 extends LinearOpMode {
     public static double manualVel = 3000;
     public static double hoodDefaultPos = 0.5;
-
+    private double predictedResetX, predictedResetY, predictedResetH;
+    public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0;
+    public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180;
     public static double spinPow = 0.09;
     public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
-    public static double spinSpeedIncrease = 0.03;
     public static int resetSpinTicks = 0;
     public static double hoodSpeedOffset = 0.01;
     public static double turretSpeedOffset = 0.01;
@@ -56,12 +62,13 @@ public class TeleopV3 extends LinearOpMode {
     Light light;
     Servos servo;
     Flywheel flywheel;
-    MecanumDrive drive;
+//    MecanumDrive drive;
     Spindexer spindexer;
     Targeting targeting;
     Targeting.Settings targetingSettings;
     Drivetrain drivetrain;
     MeasuringLoopTimes loopTimes;
+    Follower follower;
     double autoHoodOffset = 0.0;
     int shooterTicker = 0;
     boolean intake = false;
@@ -84,7 +91,6 @@ public class TeleopV3 extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
-        robot.light.setPosition(0);
         List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
 
         for (LynxModule hub : allHubs) {
@@ -94,12 +100,15 @@ public class TeleopV3 extends LinearOpMode {
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         servo = new Servos(hardwareMap);
         flywheel = new Flywheel(hardwareMap);
-        drive = new MecanumDrive(hardwareMap, teleStart);
+//        drive = new MecanumDrive(hardwareMap, teleStart);
+        follower = Constants.createFollower(hardwareMap);
+        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
+        follower.setStartingPose(start);
         spindexer = new Spindexer(hardwareMap);
         targeting = new Targeting();
         targetingSettings = new Targeting.Settings(0.0, 0.0);
 
-        drivetrain = new Drivetrain(robot, drive);
+        drivetrain = new Drivetrain(robot, follower);
 
         loopTimes = new MeasuringLoopTimes();
         loopTimes.init();
@@ -118,21 +127,33 @@ public class TeleopV3 extends LinearOpMode {
         Spindexer.teleop = true;
 
         while (opModeInInit()) {
+            //ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS
+            if (gamepad1.crossWasPressed()){
+                redAlliance = !redAlliance;
+            }
+
             robot.limelight.start();
             if (redAlliance) {
                 turret.pipelineSwitch(4);
                 light.setManualLightColor(Color.LightRed);
+                predictedResetX = redPredictedResetX;
+                predictedResetY = redPredictedResetY;
+                predictedResetH = Math.toRadians(redPredictedResetH);
             } else {
                 turret.pipelineSwitch(2);
                 light.setManualLightColor(Color.LightBlue);
-
+                predictedResetX = bluePredictedResetX;
+                predictedResetY = bluePredictedResetY;
+                predictedResetH = Math.toRadians(bluePredictedResetH);
             }
-            robot.light.setPosition(1);
+            limelightUsed = true;
+
+            TELE.addData("Red Alliance?", redAlliance);
+            TELE.update();
 
             light.update();
         }
 
-        limelightUsed = true;
 
         waitForStart();
         if (isStopRequested()) return;
@@ -143,8 +164,8 @@ public class TeleopV3 extends LinearOpMode {
         while (opModeIsActive()) {
 
             //TELE.addData("Is limelight on?", robot.limelight.getStatus());
-            drive.updatePoseEstimate();
+            follower.update();
-            Pose2d currentPose = drive.localizer.getPose();
+            Pose currentPose = follower.getPose();
 
             if (enableSpindexerManager) {
                 //if (!shootAll) {
@@ -226,20 +247,21 @@ public class TeleopV3 extends LinearOpMode {
 
             //TURRET TRACKING
 
-            double robX = currentPose.position.x;
+            double robX = currentPose.getX();
-            double robY = currentPose.position.y;
+            double robY = currentPose.getY();
-            double robH = currentPose.heading.toDouble();
+            double robH = currentPose.getHeading();
 
             double robotX = robX + xOffset;
             double robotY = robY + yOffset;
             double robotHeading = robH + hOffset;
 
-            double goalX = -15;
+//            double goalX = -15;
-            double goalY = 0;
+//            double goalY = 0;
-
+//
-            double dx = robotX - goalX;  // delta x from robot to goal
+//            double dx = robotX - goalX;  // delta x from robot to goal
-            double dy = robotY - goalY;  // delta y from robot to goal
+//            double dy = robotY - goalY;  // delta y from robot to goal
-            Pose deltaPose = new Pose(dx, dy, robotHeading);
+//            Pose deltaPose = new Pose(dx, dy, robotHeading);
+            Pose deltaPose = new Pose(robotX, robotY, robotHeading);
 
 //            double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
@@ -253,14 +275,15 @@ public class TeleopV3 extends LinearOpMode {
                 gamepad2.rumble(500);
             }
 
-            if (relocalize){
+            //TODO: relocalize using limelight
-                turret.relocalize();
+//            if (relocalize){
-                xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
+//                turret.relocalize();
-                yOffset = (turret.getLimelightX() * 39.3701) - robY;
+//                xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
-                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
+//                yOffset = (turret.getLimelightX() * 39.3701) - robY;
-            } else {
+//                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
+//            } else {
                 turret.trackGoal(deltaPose);
-            }
+            //}
 
             //VELOCITY AUTOMATIC
             if (autoVel) {
@@ -324,7 +347,8 @@ public class TeleopV3 extends LinearOpMode {
             }
 
             if (gamepad2.crossWasPressed()) {
-                drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+//                drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+                follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH));
                 gamepad2.rumble(200);
                 sleep(500);
             }
@@ -381,9 +405,9 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
             TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
             TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
-            TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
+//            TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
-            TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
+//            TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
-            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
+//            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
 
             TELE.update();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
-import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
 
 import com.acmerobotics.dashboard.FtcDashboard;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java

@@ -1,33 +1,31 @@
 package org.firstinspires.ftc.teamcode.utils;
 
-import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.ProfileAccelConstraint;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.TranslationalVelConstraint;
-import com.acmerobotics.roadrunner.Vector2d;
+import com.pedropathing.follower.Follower;
-import com.acmerobotics.roadrunner.ftc.Actions;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 
 public class Drivetrain {
 
     Robot robot;
     boolean autoDrive = false;
 
-    Pose2d brakePos = new Pose2d(0, 0, 0);
+    Pose brakePos = new Pose(0, 0, 0);
 
-    MecanumDrive drive;
+//    MecanumDrive drive;
+    Follower follower;
 
     private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200);
     private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200);
 
 
-    public Drivetrain (Robot rob, MecanumDrive mecanumDrive){
+    public Drivetrain (Robot rob, Follower follower){
 
         this.robot = rob;
-        this.drive = mecanumDrive;
+        this.follower = follower;
 
     }
 
@@ -71,7 +69,7 @@ public class Drivetrain {
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
         }
-        Pose2d currentPos = drive.localizer.getPose();
+        Pose currentPos = follower.getPose();
         brakePos = currentPos;
 
         if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
@@ -84,13 +82,13 @@ public class Drivetrain {
 
         } else if (brake > 0.4) {
 
-            TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
+            PathChain traj2 = follower.pathBuilder()
-                    .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+                    .addPath(new BezierLine(currentPos, brakePos))
+                    .setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading())
+                    .build();
 
-            if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) {
+            if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) {
-                Actions.runBlocking(
+                follower.followPath(traj2);
-                        traj2.build()
-                );
             }
         } else {
             autoDrive = false;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java

@@ -3,7 +3,9 @@ package org.firstinspires.ftc.teamcode.utils;
 import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import java.lang.Math;
 
+import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 
 public class Targeting {
@@ -71,7 +73,7 @@ public class Targeting {
     public final int TILE_UPPER_QUARTILE = 18;
     public final int TILE_LOWER_QUARTILE = 6;
     public double robotInchesX, robotInchesY = 0.0;
-    public int robotGridX, robotGridY = 0;
+    public int robotGridX = 0, robotGridY = 0;
     MultipleTelemetry TELE;
     double cancelOffsetX = 0.0;  // was -40.0
     double cancelOffsetY = 0.0;  // was 7.0
@@ -89,45 +91,59 @@ public class Targeting {
         Settings recommendedSettings = new Settings(0.0, 0.0);
         int gridX;
         int gridY;
-        if (!redAlliance){
+        int remX = 0;
-            sin54 = Math.sin(Math.toRadians(54));
+        int remY = 0;
-            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
+        // Old code
-            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
+//        if (!redAlliance){
-
+//            sin54 = Math.sin(Math.toRadians(54));
-            // Convert robot coordinates to inches
+//            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
-            robotInchesX = rotatedX * unitConversionFactor + 20;
+//            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
-            robotInchesY = rotatedY * unitConversionFactor + 20;
+//
-
+//            // Convert robot coordinates to inches
-            // Find approximate location in the grid
+//            robotInchesX = rotatedX * unitConversionFactor + 20;
-            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
+//            robotInchesY = rotatedY * unitConversionFactor + 20;
-            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+//
-        } else {
+//            // Find approximate location in the grid
-            sin54 = Math.sin(Math.toRadians(-54));
+//            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
-            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
+//            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
-            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
+//        } else {
-
+//            sin54 = Math.sin(Math.toRadians(-54));
-            // Convert robot coordinates to inches
+//            double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
-            robotInchesX = rotatedX * unitConversionFactor;
+//            double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
-            robotInchesY = rotatedY * unitConversionFactor;
+//
-
+//            // Convert robot coordinates to inches
-            // Find approximate location in the grid
+//            robotInchesX = rotatedX * unitConversionFactor;
-            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
+//            robotInchesY = rotatedY * unitConversionFactor;
-            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+//
-        }
+//            // Find approximate location in the grid
-
+//            gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
-
+//            gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
-        int remX = Math.floorMod((int) robotInchesX, tileSize);
+//        }
-        int remY = Math.floorMod((int) robotInchesY, tileSize);
+//
-
+//
-        //clamp
+        remX = Math.floorMod((int) robotX, tileSize);
+        remY = Math.floorMod((int) robotY, tileSize);
+//
+//        //clamp
+//
+//        if (redAlliance) {
+//            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+//            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+//        } else {
+//            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+//            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+//        }
 
+        // New code
         if (redAlliance){
-            robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+            gridY = Math.round((float) (((144-robotX)-12) / 24));
-            robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
         } else {
+            gridY = Math.round((float) ((robotX-12) / 24));
+        }
+        gridX = Math.round((float) (((144-robotY)-12) / 24));
+
         robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
         robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
-        }
 
         // Determine if we need to interpolate based on tile position.
         // if near upper or lower quarter or tile interpolate with next tile.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

@@ -25,7 +25,7 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 0.00011264432;
-    public static double turret180Range = 0.54;
+    public static double turret180Range = 0.55;
     public static double turrDefault = 0.35;
     public static double turrMin = 0;
     public static double turrMax = 0.69;
@@ -262,7 +262,12 @@ public class Turret {
         double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45;
 
         // Robot heading (field → robot)
-        double robotHeadingDeg = posH + 135;
+        double robotHeadingDeg;
+        if (Color.redAlliance){
+            robotHeadingDeg = posH + 135;
+        } else {
+            robotHeadingDeg = posH + 45;
+        }
 
         // Turret angle needed relative to robot
         double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
@@ -362,16 +367,16 @@ public class Turret {
 
         /* ---------------- TELEMETRY ---------------- */
 
-        TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
+//        TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
-        TELE.addData("Target Pos", "%.3f", targetTurretPos);
+//        TELE.addData("Target Pos", "%.3f", targetTurretPos);
-        TELE.addData("Current Localization Pos", "%.3f", deltaPos);
+//        TELE.addData("Current Localization Pos", deltaPos);
-        TELE.addData("Commanded Pos", "%.3f", turretPos);
+//        TELE.addData("Commanded Pos", "%.3f", turretPos);
 //        TELE.addData("LL Valid", result.isValid());
 //        TELE.addData("LL getTx", result.getTx());
 //        TELE.addData("LL Offset", offset);
 //        TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
 //        TELE.addData("Learned Offset", "%.2f", offset);
-        TELE.update();
+//        TELE.update();
     }
 
 }