Fix imu

ZodiacEFZ · Dec 15, 2024 · 3da0b9c · 3da0b9c
1 parent d231fe6
commit 3da0b9c
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 22 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumOpMode.java
@@ -4,6 +4,7 @@
 import com.arcrobotics.ftclib.geometry.Translation2d;
 import com.arcrobotics.ftclib.hardware.RevIMU;
 import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.qualcomm.hardware.bosch.BHI260IMU;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.util.ElapsedTime;
@@ -14,18 +15,17 @@
 @TeleOp(name = "Mecanum", group = "Iterative OpMode")
 public class MecanumOpMode extends OpMode {
     private final ElapsedTime timer = new ElapsedTime();
-    private final Translation2d size = new Translation2d(0.381, 0.381);
+    private final Translation2d size = new Translation2d(0.1675, 0.2075);
     private Mecanum chassis;
-    private RevIMU imu;
+    private BHI260IMU imu;
     private Xbox driver;
 
     @Override
     public void init() {
-        imu = new RevIMU(hardwareMap);
-        imu.init();
+        imu = hardwareMap.get(BHI260IMU.class, "imu");
 
         chassis = new Mecanum(hardwareMap, size, "frontLeft", "frontRight", "rearLeft", "rearRight",
-                Motor.GoBILDA.RPM_435, imu);
+                Motor.GoBILDA.RPM_312, imu);
 
         driver = new Xbox(gamepad1).onPressed(GamepadKeys.Button.B, () -> chassis.toggleHeadless());
 
@@ -45,7 +45,7 @@ public void start() {
     public void loop() {
         driver.execute();
 
-        chassis.drive(driver.getLeftX(), driver.getLeftY(), driver.getRightX());
+        chassis.drive(-driver.getLeftX(), driver.getLeftY(), -driver.getRightX());
 
         telemetry.addData("Status", "Time: " + timer);
         telemetry.addData("Speed", "Speed: " + chassis.getWheelSpeeds());

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Mecanum.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Mecanum.java
@@ -4,6 +4,7 @@
 
 import com.arcrobotics.ftclib.drivebase.MecanumDrive;
 import com.arcrobotics.ftclib.geometry.Pose2d;
+import com.arcrobotics.ftclib.geometry.Rotation2d;
 import com.arcrobotics.ftclib.geometry.Translation2d;
 import com.arcrobotics.ftclib.hardware.GyroEx;
 import com.arcrobotics.ftclib.hardware.motors.Motor;
@@ -20,7 +21,8 @@ public class Mecanum {
     private final GyroEx imu;
     private final MecanumDrive drive;
     private final MecanumDriveOdometry odometry;
-    private boolean headless = true;
+    private final MecanumDriveKinematics kinematics;
+    private boolean headless = false;
     private MecanumDriveWheelSpeeds wheelSpeeds;
     private Pose2d pose;
 
@@ -32,28 +34,17 @@ public Mecanum(HardwareMap hardwareMap, Translation2d size, String frontLeftMoto
         rearRight = new MotorEx(hardwareMap, rearRightMotorName, goBILDA);
         this.imu = imu;
         drive = new MecanumDrive(frontLeft, frontRight, rearLeft, rearRight);
-
-        MecanumDriveKinematics kinematics = getMecanumDriveKinematics(size);
-        odometry = new MecanumDriveOdometry(kinematics, imu.getRotation2d(), new Pose2d());
-    }
-
-    @NonNull
-    private static MecanumDriveKinematics getMecanumDriveKinematics(Translation2d size) {
-        Translation2d frontLeftLocation = new Translation2d(size.getX(), size.getY());
-        Translation2d frontRightLocation = new Translation2d(size.getX(), -size.getY());
-        Translation2d backLeftLocation = new Translation2d(-size.getX(), size.getY());
-        Translation2d backRightLocation = new Translation2d(-size.getX(), -size.getY());
-
-        return new MecanumDriveKinematics(frontLeftLocation, frontRightLocation, backLeftLocation,
-                backRightLocation);
+        kinematics = new MecanumDriveKinematics(new Translation2d(size.getX(), size.getY()), new Translation2d(size.getX(), -size.getY()),
+                new Translation2d(-size.getX(), size.getY()), new Translation2d(-size.getX(), -size.getY()));
+        odometry = new MecanumDriveOdometry(kinematics, imu == null ? new Rotation2d() : imu.getRotation2d(), new Pose2d());
     }
 
     public void drive(double strafe, double forward, double omega) {
         drive.driveFieldCentric(strafe, forward, omega,
                 headless ? imu.getRotation2d().getDegrees() : 0);
         wheelSpeeds = new MecanumDriveWheelSpeeds(frontLeft.getVelocity(), frontRight.getVelocity(),
                 rearLeft.getVelocity(), rearRight.getVelocity());
-        pose = odometry.updateWithTime(timer.seconds(), imu.getRotation2d(), wheelSpeeds);
+        pose = odometry.updateWithTime(timer.seconds(), new Rotation2d(), wheelSpeeds);
     }
 
     public boolean toggleHeadless() {