second part of doing rr things. commiting it only so that if im destr…

…oying everything i can rollback some but not all of it

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/TrajectoryDrive.java

@@ -13,15 +13,26 @@
 //import static org.firstinspires.ftc.teamcode.drive.testbot.DriveConstants.maxAngAccel;
 //import static org.firstinspires.ftc.teamcode.drive.testbot.DriveConstants.maxAngVel;
 
+import static org.openftc.apriltag.ApriltagDetectionJNI.getPoseEstimate;
+
 import androidx.annotation.NonNull;
 
 import com.acmerobotics.dashboard.config.Config;
 
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDCoefficients;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
@@ -116,8 +127,8 @@ public TrajectoryDrive(HardwareMap hardwareMap,
         this.maxAngAccel = maxAngAccel;
         this.maxAngVel = maxAngVel;
 
-        follower = new HolonomicPIDVAFollower(translationalPid, translationalPid, headingPid,
-                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+     //   follower = new HolonomicPIDVAFollower(translationalPid, translationalPid, headingPid,
+        //         new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
 
         LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
 
@@ -164,17 +175,17 @@ public TrajectoryDrive(HardwareMap hardwareMap,
         List<Integer> lastTrackingEncVels = new ArrayList<>();
 
         // TODO: if desired, use setLocalizer() to change the localization method
-        setLocalizer(new TrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels,
-                leftEncoderName,
-                rightEncoderName, perpEncoderName,
-        x_mult,
-        y_mult,
-        forward_offset,
-        lateral_distance,
-        gear_ratio,
-        wheel_radius,
-        ticks_per_rev
-        ));
+//        setLocalizer(new TrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels,
+//                leftEncoderName,
+//                rightEncoderName, perpEncoderName,
+//        x_mult,
+//        y_mult,
+//        forward_offset,
+//        lateral_distance,
+//        gear_ratio,
+//        wheel_radius,
+//        ticks_per_rev
+//        ));
 
 
 
@@ -249,6 +260,15 @@ public void update() {
         if (signal != null) setDriveSignal(signal);
     }
 
+    private Object getPoseVelocity() {
+    }
+
+    private void setDriveSignal(DriveSignal signal) {
+    }
+
+    private void updatePoseEstimate() {
+    }
+
     public void waitForIdle() {
         while (!Thread.currentThread().isInterrupted() && isBusy())
             update();
@@ -301,8 +321,11 @@ public void setWeightedDrivePower(Pose2d drivePower) {
         setDrivePower(vel);
     }
 
+    private void setDrivePower(Pose2d vel) {
+    }
+
     @NonNull
-    @Override
+
     public List<Double> getWheelPositions() {
         lastEncPositions.clear();
 
@@ -315,7 +338,7 @@ public List<Double> getWheelPositions() {
         return wheelPositions;
     }
 
-    @Override
+
     public List<Double> getWheelVelocities() {
         lastEncVels.clear();
 
@@ -328,36 +351,36 @@ public List<Double> getWheelVelocities() {
         return wheelVelocities;
     }
 
-    @Override
+
     public void setMotorPowers(double v, double v1, double v2, double v3) {
         leftFront.setPower(v);
         leftRear.setPower(v1);
         rightRear.setPower(v2);
         rightFront.setPower(v3);
     }
 
-    @Override
+
     public double getRawExternalHeading() {
  //       return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
         return 0;
     }
 
-    @Override
+
     public Double getExternalHeadingVelocity() {
     //    return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
         return 0.0;
     }
 
-    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
-        return new MinVelocityConstraint(Arrays.asList(
-                new AngularVelocityConstraint(maxAngularVel),
-                new MecanumVelocityConstraint(maxVel, trackWidth)
-        ));
-    }
+//    public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+//        return new MinVelocityConstraint(Arrays.asList(
+//                new AngularVelocityConstraint(maxAngularVel),
+//                new MecanumVelocityConstraint(maxVel, trackWidth)
+//        ));
+//    }
 
-    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
-        return new ProfileAccelerationConstraint(maxAccel);
-    }
+//    public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+//        return new ProfileAccelerationConstraint(maxAccel);
+//    }
 
     public enum Quadrant {
         RED_AUDIENCE,
@@ -383,6 +406,7 @@ public static TrajectoryDrive.Quadrant whichQuadrant(Pose2d pose) {
     }
 
     public Quadrant currentQuadrant() {
-        return whichQuadrant(getPoseEstimate());
+       // return whichQuadrant(getPoseEstimate());
+        return null;
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java

@@ -1,7 +1,8 @@
 package org.firstinspires.ftc.teamcode.opmode;
 
 import com.acmerobotics.dashboard.config.Config;
-import com.acmerobotics.roadrunner.Pose2d;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -10,9 +11,6 @@
 import org.firstinspires.ftc.teamcode.drive.Robot;
 import org.firstinspires.ftc.teamcode.drive.RoboticaBot;
 import org.firstinspires.ftc.teamcode.opmode.components.Component;
-import org.firstinspires.ftc.teamcode.opmode.components.GoToBoard;
-import org.firstinspires.ftc.teamcode.opmode.components.ParkingIn;
-import org.firstinspires.ftc.teamcode.opmode.components.ParkingOut;
 import org.firstinspires.ftc.teamcode.opmode.components.PurplePixelComponent;
 import org.firstinspires.ftc.teamcode.util.GlobalOpMode;
 import org.firstinspires.ftc.teamcode.util.Timeout;
@@ -29,7 +27,7 @@ public class TestAuto extends LinearOpMode {
     private Pose2d estimateWithAllCameras(AprilTagCamera[] cameras, AprilTagLocalizer aprilTag) {
         Pose2d pose = null;
         for (AprilTagCamera camera : cameras) {
-            pose = aprilTag.estimateRobotPoseFromAprilTags(camera);
+ //           pose = aprilTag.estimateRobotPoseFromAprilTags(camera);
             if (pose != null) {
                 break;
             }
@@ -60,11 +58,11 @@ public void runOpMode() throws InterruptedException {
 
         // Set servo position right away so that it holds
         if (robot.getClass() == RoboticaBot.class) {
-            ((RoboticaBot) robot).pinchServo.setPosition(0.23);
-            ((RoboticaBot) robot).wristLiftServo.setPosition(0.692);
-            ((RoboticaBot) robot).elbowServo.setPosition(0.494);
-            //((RoboticaBot) robot).wristServo.setPosition(0.2);
-            ((RoboticaBot) robot).planeAngleServo.setPosition(0.3);
+//            ((RoboticaBot) robot).pinchServo.setPosition(0.23);
+//            ((RoboticaBot) robot).wristLiftServo.setPosition(0.692);
+//            ((RoboticaBot) robot).elbowServo.setPosition(0.494);
+//            //((RoboticaBot) robot).wristServo.setPosition(0.2);
+//            ((RoboticaBot) robot).planeAngleServo.setPosition(0.3);
         }
 
         //Pose2d startPose = new Pose2d(12,-62, Math.toRadians(90)); // RED_BOARD
@@ -108,7 +106,7 @@ public void runOpMode() throws InterruptedException {
         if (robot.getClass() == RoboticaBot.class) {
             startPose = new Pose2d(startPose.getX(), startPose.getY(), startPose.getHeading() + Math.toRadians(180));
         }
-        robot.getDrive().setPoseEstimate(startPose);
+      //  robot.getDrive().setPoseEstimate(startPose);
 
         if (!config.tensorFlowInInit || tensorPos == null) {
             aprilTag.close();
@@ -130,24 +128,24 @@ public void runOpMode() throws InterruptedException {
             componentList.add(new PurplePixelComponent(robot, tensorPos, config.innerPath && config.doPark));
         }
 
-        if (config.doPark) {
-            if (config.innerPath) {
-                componentList.add(new ParkingIn(robot));
-            } else {
-                componentList.add(new ParkingOut(robot));
-            }
-            if (config.dropOnBoard) {
-                componentList.add(new GoToBoard(robot, tensorPos));
-            }
+//        if (config.doPark) {
+//            if (config.innerPath) {
+//                componentList.add(new ParkingIn(robot));
+//            } else {
+//                componentList.add(new ParkingOut(robot));
+//            }
+//            if (config.dropOnBoard) {
+//                componentList.add(new GoToBoard(robot, tensorPos));
+//            }
         }
 
-        for (Component component : componentList) {
-            GlobalOpMode.lastPose = robot.getDrive().getPoseEstimate();
-            component.drive();
-        }
+//        for (Component component : componentList) {
+//            GlobalOpMode.lastPose = robot.getDrive().getPoseEstimate();
+//            component.drive();
+//        }
 
-        GlobalOpMode.lastPose = robot.getDrive().getPoseEstimate();
-        sleep(30000);
+//        GlobalOpMode.lastPose = robot.getDrive().getPoseEstimate();
+        //sleep(30000);
 
 
     }