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 0a45eb8..0970664 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 @@ -154,6 +154,16 @@ public class Follower { initialize(); } + /** + * This creates a new Follower given a HardwareMap and a localizer. + * @param hardwareMap HardwareMap required + * @param localizer the localizer you wish to use + */ + public Follower(HardwareMap hardwareMap, Localizer localizer) { + this.hardwareMap = hardwareMap; + initialize(localizer); + } + /** * This initializes the follower. * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are @@ -190,6 +200,49 @@ public class Follower { breakFollowing(); } + /** + * This initializes the follower. + * In this, the DriveVectorScaler and PoseUpdater is instantiated, the drive motors are + * initialized and their behavior is set, and the variables involved in approximating first and + * second derivatives for teleop are set. + * @param localizer the localizer you wish to use + */ + + public void initialize(Localizer localizer) { + driveVectorScaler = new DriveVectorScaler(FollowerConstants.frontLeftVector); + poseUpdater = new PoseUpdater(hardwareMap, localizer); + + // rightFront = hardwareMap.get(DcMotorEx.class, "motor_rf"); + // rightRear = hardwareMap.get(DcMotorEx.class, "motor_rb"); + // leftFront = hardwareMap.get(DcMotorEx.class, "motor_lf"); + // leftRear = hardwareMap.get(DcMotorEx.class, "motor_lb"); + + leftFront = hardwareMap.get(DcMotorEx.class, leftFrontMotorName); + leftRear = hardwareMap.get(DcMotorEx.class, leftRearMotorName); + rightRear = hardwareMap.get(DcMotorEx.class, rightRearMotorName); + rightFront = hardwareMap.get(DcMotorEx.class, rightFrontMotorName); + + // TODO: Make sure that this is the direction your motors need to be reversed in. + leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + leftRear.setDirection(DcMotorSimple.Direction.REVERSE); + + motors = Arrays.asList(leftFront, leftRear, rightFront, rightRear); + + for (DcMotorEx motor : motors) { + MotorConfigurationType motorConfigurationType = motor.getMotorType().clone(); + motorConfigurationType.setAchieveableMaxRPMFraction(1.0); + motor.setMotorType(motorConfigurationType); + } + + for (DcMotorEx motor : motors) { + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + } + + dashboardPoseTracker = new DashboardPoseTracker(poseUpdater); + + breakFollowing(); + } + /** * This sets the maximum power the motors are allowed to use. *