SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Update Follower.java

Browse Source 
Made the follower optionally take in a second parameter for the localizer
This commit is contained in:
Havish Sripada
2024-12-23 09:35:30 -08:00
committed by GitHub
parent 06431bb98f
commit 81514f34b4
1 changed files with 53 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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.
*