diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 5b35029..afb8482 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -33,6 +33,6 @@ dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation "com.acmerobotics.roadrunner:ftc:0.1.11" + implementation "com.acmerobotics.roadrunner:ftc:0.1.12" implementation "com.acmerobotics.dashboard:dashboard:0.4.14" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index 87e3884..fb49198 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -26,6 +26,7 @@ import com.acmerobotics.roadrunner.VelConstraint; import com.acmerobotics.roadrunner.ftc.DownsampledWriter; import com.acmerobotics.roadrunner.ftc.Encoder; import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.LazyImu; import com.acmerobotics.roadrunner.ftc.LynxFirmware; import com.acmerobotics.roadrunner.ftc.OverflowEncoder; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; @@ -36,7 +37,6 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -109,7 +109,7 @@ public final class MecanumDrive { public final VoltageSensor voltageSensor; - public final IMU imu; + public final LazyImu imu; public final Localizer localizer; public Pose2d pose; @@ -141,7 +141,7 @@ public final class MecanumDrive { lastRightBackPos = rightBack.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position; - lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + lastHeading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); } @Override @@ -154,7 +154,7 @@ public final class MecanumDrive { FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage( leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel)); - Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + Rotation2d heading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); double headingDelta = heading.minus(lastHeading); Twist2dDual