teleop enhancements fix

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java

@@ -6,8 +6,6 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 
 import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.MathFunctions;
-import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
 
 /**
  * This is the TeleOpEnhancements OpMode. It is an example usage of the TeleOp enhancements that
@@ -27,15 +25,12 @@ public class TeleOpEnhancements extends OpMode {
     private DcMotorEx rightFront;
     private DcMotorEx rightRear;
 
-    private Vector driveVector;
-    private Vector headingVector;
-
     /**
      * This initializes the drive motors as well as the Follower and motion Vectors.
      */
     @Override
     public void init() {
-        follower = new Follower(hardwareMap, false);
+        follower = new Follower(hardwareMap);
 
         leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
         leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
@@ -47,8 +42,7 @@ public class TeleOpEnhancements extends OpMode {
         rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
-        driveVector = new Vector();
+        follower.startTeleopDrive();
-        headingVector = new Vector();
     }
 
     /**
@@ -57,15 +51,7 @@ public class TeleOpEnhancements extends OpMode {
      */
     @Override
     public void loop() {
-        driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_stick_x);
+        follower.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x);
-        driveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1));
-
-        // TODO: if you want to make this field centric, then just remove this line
-        driveVector.rotateVector(follower.getPose().getHeading());
-
-        headingVector.setComponents(-gamepad1.right_stick_x, follower.getPose().getHeading());
-
-        follower.setMovementVectors(follower.getCentripetalForceCorrection(), headingVector, driveVector);
         follower.update();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java

@@ -82,9 +82,10 @@ public class Follower {
     private boolean followingPathChain;
     private boolean holdingPosition;
     private boolean isBusy;
-    private boolean auto = true;
     private boolean reachedParametricPathEnd;
     private boolean holdPositionAtEnd;
+    private boolean teleopDrive;
+    private boolean fieldCentric;
 
     private double maxPower = 1;
     private double previousSmallTranslationalIntegral;
@@ -97,6 +98,7 @@ public class Follower {
     private long reachedParametricPathEndTime;
 
     private double[] drivePowers;
+    private double[] teleopDriveValues;
 
     private Vector[] teleOpMovementVectors = new Vector[]{new Vector(), new Vector(), new Vector()};
 
@@ -144,19 +146,6 @@ public class Follower {
         initialize();
     }
 
-    /**
-     * This creates a new Follower given a HardwareMap and sets whether the Follower is being used
-     * in autonomous or teleop.
-     *
-     * @param hardwareMap HardwareMap required
-     * @param setAuto     sets whether or not the Follower is being used in autonomous or teleop
-     */
-    public Follower(HardwareMap hardwareMap, boolean setAuto) {
-        this.hardwareMap = hardwareMap;
-        setAuto(setAuto);
-        initialize();
-    }
-
     /**
      * This initializes the follower.
      * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are
@@ -188,15 +177,12 @@ public class Follower {
             motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
         }
 
-        for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) {
+        // TODO: Set this to true if you want to use field centric teleop drive.
-            velocities.add(new Vector());
+        fieldCentric = false;
-        }
-        for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) {
-            accelerations.add(new Vector());
-        }
-        calculateAveragedVelocityAndAcceleration();
 
         dashboardPoseTracker = new DashboardPoseTracker(poseUpdater);
+
+        breakFollowing();
     }
 
     /**
@@ -428,6 +414,14 @@ public class Follower {
         followPath(pathChain, false);
     }
 
+    /**
+     * This starts teleop drive control.
+     */
+    public void startTeleopDrive() {
+        breakFollowing();
+        teleopDrive = true;
+    }
+
     /**
      * This calls an update to the PoseUpdater, which updates the robot's current position estimate.
      * This also updates all the Follower's PIDFs, which updates the motor powers.
@@ -439,7 +433,7 @@ public class Follower {
             dashboardPoseTracker.update();
         }
 
-        if (auto) {
+        if (!teleopDrive) {
             if (holdingPosition) {
                 closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1);
 
@@ -499,7 +493,19 @@ public class Follower {
 
             calculateAveragedVelocityAndAcceleration();
 
-            drivePowers = driveVectorScaler.getDrivePowers(teleOpMovementVectors[0], teleOpMovementVectors[1], teleOpMovementVectors[2], poseUpdater.getPose().getHeading());
+            Vector teleopDriveVector = new Vector();
+            Vector teleopHeadingVector = new Vector();
+
+            teleopDriveVector.setOrthogonalComponents(teleopDriveValues[0], teleopDriveValues[1]);
+            teleopDriveVector.setMagnitude(MathFunctions.clamp(driveVector.getMagnitude(), 0, 1));
+
+            if (!fieldCentric) {
+                teleopDriveVector.rotateVector(getPose().getHeading());
+            }
+
+            teleopHeadingVector.setComponents(teleopDriveValues[2], getPose().getHeading());
+
+            drivePowers = driveVectorScaler.getDrivePowers(getCentripetalForceCorrection(), teleopHeadingVector, teleopDriveVector, poseUpdater.getPose().getHeading());
 
             limitDrivePowers();
 
@@ -509,6 +515,21 @@ public class Follower {
         }
     }
 
+    /**
+     * This sets the teleop drive vectors.
+     *
+     * @param forwardDrive determines the forward drive vector for the robot in teleop. In field centric
+     *                     movement, this is the x-axis.
+     * @param lateralDrive determines the lateral drive vector for the robot in teleop. In field centric
+     *                     movement, this is the y-axis.
+     * @param heading determines the heading vector for the robot in teleop.
+     */
+    public void setTeleOpMovementVectors(double forwardDrive, double lateralDrive, double heading) {
+        teleopDriveValues[0] = MathFunctions.clamp(forwardDrive, -1, 1);
+        teleopDriveValues[1] = MathFunctions.clamp(lateralDrive, -1, 1);
+        teleopDriveValues[2] = MathFunctions.clamp(heading, -1, 1);
+    }
+
     /**
      * This calculates an averaged approximate velocity and acceleration. This is used for a
      * real-time correction of centripetal force, which is used in teleop.
@@ -564,6 +585,7 @@ public class Follower {
      * This resets the PIDFs and stops following the current Path.
      */
     public void breakFollowing() {
+        teleopDrive = false;
         holdingPosition = false;
         isBusy = false;
         reachedParametricPathEnd = false;
@@ -594,6 +616,15 @@ public class Follower {
         }
         driveKalmanFilter.reset();
 
+        for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER; i++) {
+            velocities.add(new Vector());
+        }
+        for (int i = 0; i < AVERAGED_VELOCITY_SAMPLE_NUMBER / 2; i++) {
+            accelerations.add(new Vector());
+        }
+        calculateAveragedVelocityAndAcceleration();
+        teleopDriveValues = new double[3];
+
         for (int i = 0; i < motors.size(); i++) {
             motors.get(i).setPower(0);
         }
@@ -608,14 +639,6 @@ public class Follower {
         return isBusy;
     }
 
-    /**
-     * Sets the correctional, heading, and drive movement vectors for teleop enhancements.
-     * The correctional Vector only accounts for an approximated centripetal correction.
-     */
-    public void setMovementVectors(Vector correctional, Vector heading, Vector drive) {
-        teleOpMovementVectors = new Vector[]{correctional, heading, drive};
-    }
-
     /**
      * This returns a Vector in the direction the robot must go to move along the path. This Vector
      * takes into account the projected position of the robot to calculate how much power is needed.
@@ -681,14 +704,14 @@ public class Follower {
         previousRawDriveError = rawDriveError;
         rawDriveError =  velocityErrorVector.getMagnitude() * MathFunctions.getSign(MathFunctions.dotProduct(velocityErrorVector, currentPath.getClosestPointTangentVector()));
 
-        double projection = 2 * driveErrors[2] - driveErrors[1];
+        double projection = 2 * driveErrors[1] - driveErrors[0];
 
         driveKalmanFilter.update(rawDriveError - previousRawDriveError, projection);
 
         for (int i = 0; i < driveErrors.length - 1; i++) {
             driveErrors[i] = driveErrors[i + 1];
         }
-        driveErrors[2] = driveKalmanFilter.getState();
+        driveErrors[1] = driveKalmanFilter.getState();
 
         return driveKalmanFilter.getState();
     }
@@ -809,7 +832,7 @@ public class Follower {
     public Vector getCentripetalForceCorrection() {
         if (!useCentripetal) return new Vector();
         double curvature;
-        if (auto) {
+        if (!teleopDrive) {
             curvature = currentPath.getClosestPointCurvature();
         } else {
             double yPrime = averageVelocity.getYComponent() / averageVelocity.getXComponent();
@@ -832,15 +855,6 @@ public class Follower {
         return closestPose;
     }
 
-    /**
-     * This sets whether or not the Follower is being used in auto or teleop.
-     *
-     * @param set sets auto or not
-     */
-    public void setAuto(boolean set) {
-        auto = set;
-    }
-
     /**
      * This returns whether the follower is at the parametric end of its current Path.
      * The parametric end is determined by if the closest Point t-value is greater than some specified