diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java index dfcd231a..e5872e52 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/examples/TeleOpEnhancements.java @@ -6,8 +6,6 @@ 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 void init() { rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - driveVector = new Vector(); - headingVector = new Vector(); + follower.startTeleopDrive(); } /** @@ -57,15 +51,7 @@ public void init() { */ @Override public void loop() { - driveVector.setOrthogonalComponents(-gamepad1.left_stick_y, -gamepad1.left_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.setTeleOpMovementVectors(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x); follower.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java index 383b7aa9..3d61d7ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/follower/Follower.java +++ b/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 Follower(HardwareMap hardwareMap) { 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 void initialize() { motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); } - 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(); + // TODO: Set this to true if you want to use field centric teleop drive. + fieldCentric = false; dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + breakFollowing(); } /** @@ -428,6 +414,14 @@ public void followPath(PathChain pathChain) { 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 void update() { dashboardPoseTracker.update(); } - if (auto) { + if (!teleopDrive) { if (holdingPosition) { closestPose = currentPath.getClosestPoint(poseUpdater.getPose(), 1); @@ -499,7 +493,19 @@ public void update() { 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 void update() { } } + /** + * 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 void updateCallbacks() { * 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 void breakFollowing() { } 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 boolean isBusy() { 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 double getDriveVelocityError() { 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 Vector getTranslationalError() { 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 Pose getClosestPose() { 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