From 3da0b9cb3a7e69824747a16dddcb0e4e99849271 Mon Sep 17 00:00:00 2001
From: Blue-Flag-666 <64064866+Blue-Flag-666@users.noreply.github.com>
Date: Sun, 15 Dec 2024 18:18:44 +0800
Subject: [PATCH] Fix imu

---
 .../ftc/teamcode/MecanumOpMode.java           | 12 +++++-----
 .../ftc/teamcode/lib/Mecanum.java             | 23 ++++++-------------
 2 files changed, 13 insertions(+), 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
index 127ee712bfa8..5091732516a0 100644
--- 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
index a4ab257e7f44..7c40a040b83f 100644
--- 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,20 +34,9 @@ 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) {
@@ -53,7 +44,7 @@ public void drive(double strafe, double forward, double 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() {