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

Rename LazyImu drive members

Browse Source 
Also makes sure the mecanum drive localizer fetches the IMU
on construction.
This commit is contained in:
Ryan Brott
2024-02-20 22:07:37 -08:00
parent 9d2cd48287
commit 03d95d791e
3 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -36,6 +36,7 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
@ -109,7 +110,7 @@ public final class MecanumDrive {
public final VoltageSensor voltageSensor; public final VoltageSensor voltageSensor;
public final LazyImu imu; public final LazyImu lazyImu;
public final Localizer localizer; public final Localizer localizer;
public Pose2d pose; public Pose2d pose;
@ -123,6 +124,7 @@ public final class MecanumDrive {
public class DriveLocalizer implements Localizer { public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront; public final Encoder leftFront, leftBack, rightBack, rightFront;
public final IMU imu;
private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos;
private Rotation2d lastHeading; private Rotation2d lastHeading;
@ -134,6 +136,8 @@ public final class MecanumDrive {
rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
imu = lazyImu.get();
// TODO: reverse encoders if needed // TODO: reverse encoders if needed
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE); // leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
} }
@ -145,7 +149,7 @@ public final class MecanumDrive {
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
YawPitchRollAngles angles = imu.get().getRobotYawPitchRollAngles(); YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles();
FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles));
@ -228,7 +232,7 @@ public final class MecanumDrive {
// TODO: make sure your config has an IMU with this name (can be BNO or BHI) // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
imu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot( lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next(); voltageSensor = hardwareMap.voltageSensor.iterator().next();

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -110,7 +110,7 @@ public final class TankDrive {
public final List<DcMotorEx> leftMotors, rightMotors; public final List<DcMotorEx> leftMotors, rightMotors;
public final LazyImu imu; public final LazyImu lazyImu;
public final VoltageSensor voltageSensor; public final VoltageSensor voltageSensor;
@ -237,7 +237,7 @@ public final class TankDrive {
// TODO: make sure your config has an IMU with this name (can be BNO or BHI) // TODO: make sure your config has an IMU with this name (can be BNO or BHI)
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html // see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
imu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot( lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection)); PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
voltageSensor = hardwareMap.voltageSensor.iterator().next(); voltageSensor = hardwareMap.voltageSensor.iterator().next();

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
View File

@ -97,7 +97,7 @@ public final class TuningOpModes {
rightEncs, rightEncs,
parEncs, parEncs,
perpEncs, perpEncs,
md.imu, md.lazyImu,
md.voltageSensor, md.voltageSensor,
() -> new MotorFeedforward(MecanumDrive.PARAMS.kS, () -> new MotorFeedforward(MecanumDrive.PARAMS.kS,
MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick, MecanumDrive.PARAMS.kV / MecanumDrive.PARAMS.inPerTick,
@ -140,7 +140,7 @@ public final class TuningOpModes {
rightEncs, rightEncs,
parEncs, parEncs,
perpEncs, perpEncs,
td.imu, td.lazyImu,
td.voltageSensor, td.voltageSensor,
() -> new MotorFeedforward(TankDrive.PARAMS.kS, () -> new MotorFeedforward(TankDrive.PARAMS.kS,
TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick, TankDrive.PARAMS.kV / TankDrive.PARAMS.inPerTick,