fix: rotation matches controller input

flamingchickens1540 · Jan 10, 2024 · 9306630 · 9306630
1 parent bf9be1f
commit 9306630
Show file tree

Hide file tree

Showing 6 changed files with 26 additions and 23 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -11,9 +11,9 @@
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
-    public static final Mode currentMode = Mode.SIM;
+    public static final Mode currentMode = Robot.isReal() ? Mode.REAL : Mode.SIM;
 
-    public static enum Mode {
+    public enum Mode {
         /**
          * Running on a real robot.
          */

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -112,16 +112,12 @@ private void configureButtonBindings() {
                         () -> -controller.getLeftX(),
                         () -> -controller.getRightX()));
         controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-        controller
-                .b()
-                .onTrue(
-                        Commands.runOnce(
-                                        () ->
-                                                drive.setPose(
-                                                        new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
-                                        drive)
-                                .ignoringDisable(true));
-
+        controller.b().onTrue(
+                Commands.runOnce(
+                        () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
+                        drive
+                ).ignoringDisable(true)
+        );
     }
 
     /**

diff --git a/src/main/java/org/team1540/robot2024/commands/DriveCommands.java b/src/main/java/org/team1540/robot2024/commands/DriveCommands.java
@@ -36,7 +36,14 @@ public static Command joystickDrive(Drive drive, DoubleSupplier xSupplier, Doubl
             Translation2d linearVelocity = new Pose2d(new Translation2d(), linearDirection).transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
 
             // Convert to field relative speeds & send command
-            drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), omega * drive.getMaxAngularSpeedRadPerSec(), drive.getRotation()));
+            drive.runVelocity(
+                    ChassisSpeeds.fromFieldRelativeSpeeds(
+                            linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+                            linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+                            omega * drive.getMaxAngularSpeedRadPerSec(),
+                            drive.getRotation()
+                    )
+            );
         }, drive);
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Module.java
@@ -49,12 +49,12 @@ public Module(ModuleIO io, int index) {
         }
 
         turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
-        setBrakeMode(true);
+        setBrakeMode(false);
     }
 
     public void periodic() {
         io.updateInputs(inputs);
-        Logger.processInputs("Drive/Module" + Integer.toString(index), inputs);
+        Logger.processInputs("Drive/Module" + index, inputs);
 
         // On first cycle, reset relative turn encoder
         // Wait until absolute angle is nonzero in case it wasn't initialized yet

diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/drive/ModuleIOTalonFX.java
@@ -84,7 +84,7 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
         inputs.driveCurrentAmps = new double[]{driveCurrent.getValueAsDouble()};
 
-        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).minus(absoluteEncoderOffset);
+        inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()).plus(absoluteEncoderOffset);
         inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / Constants.Drivetrain.TURN_GEAR_RATIO);
         inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / Constants.Drivetrain.TURN_GEAR_RATIO;
         inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble();

diff --git a/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java b/src/main/java/org/team1540/robot2024/util/swerve/SwerveFactory.java
@@ -12,13 +12,13 @@
 
 public class SwerveFactory {
     private static final double[] moduleOffsetsRots = new double[]{
-            -0.417724609375, // Module 1
+            -0.4130859, // Module 1
             0.0, // Module 2
-            -0.216796875, // Module 3
-            -0.772705078125, // Module 4
+            -0.2197265, // Module 3
+            -0.7722, // Module 4
             0.0, // Module 5
             0.0, // Module 6
-            -0.825927734375, // Module 7
+            -0.826660, // Module 7
             0.0  // Module 8
     };
 
@@ -27,10 +27,10 @@ public static SwerveModuleHW getModuleMotors(int id, SwerveCorner corner) {
     }
 
     public enum SwerveCorner {
-        FRONT_LEFT(0),
+        FRONT_LEFT(180),
         FRONT_RIGHT(90),
         BACK_LEFT(270),
-        BACK_RIGHT(180);
+        BACK_RIGHT(0);
 
         private final double offset;
         SwerveCorner(double offset) {
@@ -61,9 +61,9 @@ private SwerveModuleHW(int id, SwerveCorner corner, String canbus) {
             driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
             driveConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
 
-
             turnConfig.CurrentLimits.SupplyCurrentLimit = 30.0;
             turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+            turnConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
             canCoderconfig.MagnetSensor.MagnetOffset = moduleOffsetsRots[id-1];
             canCoderconfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;