Update heading control calculations

team581 · Apr 7, 2022 · 7d0aed7 · 7d0aed7
1 parent e2dfb3c
commit 7d0aed7
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java
@@ -61,14 +61,14 @@ public class DriveSubsystem extends SubsystemBase {
     final var maxChassisSpeedsForward = KINEMATICS.toChassisSpeeds(maxWheelSpeedsForward);
     final var maxWheelSpeedsSpinning =
         new MecanumDriveWheelSpeeds(
-            -Wheel.MAX_WHEEL_VELOCITY,
+            Wheel.MAX_WHEEL_VELOCITY,
             -Wheel.MAX_WHEEL_VELOCITY,
             Wheel.MAX_WHEEL_VELOCITY,
-            Wheel.MAX_WHEEL_VELOCITY);
+            -Wheel.MAX_WHEEL_VELOCITY);
     final var maxChassisSpeedsSpinning = KINEMATICS.toChassisSpeeds(maxWheelSpeedsSpinning);
 
-    MAX_VELOCITY = maxChassisSpeedsForward.vxMetersPerSecond;
-    MAX_ANGULAR_VELOCITY = maxChassisSpeedsSpinning.omegaRadiansPerSecond;
+    MAX_VELOCITY = Math.abs(maxChassisSpeedsForward.vxMetersPerSecond);
+    MAX_ANGULAR_VELOCITY = Math.abs(maxChassisSpeedsSpinning.omegaRadiansPerSecond);
 
     MAX_ACCELERATION = Wheel.MAX_ACCELERATION;
     // TODO: This is the wheel's angular acceleration, do we need a different value for the robot

diff --git a/src/main/java/frc/robot/drive/commands/TeleopDriveCommand.java b/src/main/java/frc/robot/drive/commands/TeleopDriveCommand.java
@@ -12,7 +12,7 @@
 
 public class TeleopDriveCommand extends CommandBase {
   /** The maximum allowed turn rate of the robot during teleop, per second. */
-  public static final Rotation2d MAX_TELEOP_TURN_RATE = Rotation2d.fromDegrees(180);
+  public static final Rotation2d MAX_TELEOP_TURN_RATE = Rotation2d.fromDegrees(360 * 0.75);
 
   private final DriveSubsystem driveSubsystem;
   private final DriveController controller;