diff --git a/src/main/java/frc/robot/drive/DriveSubsystem.java b/src/main/java/frc/robot/drive/DriveSubsystem.java index ade7781..f1b5714 100644 --- 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 index 0b1728a..f6ae907 100644 --- 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;