demo changes

Author: bobopop787

src/main/java/frc/robot/Constants.java

@@ -47,11 +47,11 @@ public static class SwerveConstants { // all swerve on CAN ID range 1-9
 		public static final double kTurnAbsPositionConversionFactor = Units.rotationsToRadians(1);
 
 		public static final double kMaxWheelSpeed = 8; // m/s
-		public static final double kMagVelLimit = 6; // m/s
+		public static final double kMagVelLimit = 2; // m/s
 		public static final double kDirVelLimit = 10; // rad/s
-		public static final double kRotVelLimit = 6; // rad/s
+		public static final double kRotVelLimit = 4; // rad/s
 		public static final double kMagAccelLimit = 48; // m/s^2
-		public static final double kRotAccelLimit = 30; // rad/s^2
+		public static final double kRotAccelLimit = 60; // rad/s^2
 
 		public static final double kDefaultTestTurn = 0;
 		public static final double kDefaultTestDrive = 0;

src/main/java/frc/robot/RobotContainer.java

@@ -142,6 +142,8 @@ private void configureBindings() {
 		// altController.rightBumper().onFalse(intake.runRoller(0));
 		// altController.rightTrigger().onTrue(intake.runRoller(16));
 		// altController.rightTrigger().onFalse(intake.runRoller(0));
+
+		altController.rightBumper().onTrue(swerveDrive.toggleEnabled());
 	}
 
 	public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -277,7 +277,7 @@ public void periodic() {
                 indexSpeedFactor = 0.3;
                 break;
             case SPEAKER:
-                speedFactor = -1;
+                speedFactor = -0.35;
                 indexSpeedFactor = speedFactor;
                 break;
             case AMP:

src/main/java/frc/robot/subsystems/swerve/SDSSwerveModule.java

@@ -128,6 +128,7 @@ public void setDesiredSwerveState(SwerveModuleState state, boolean optimize) {
                         turnAbsEncoder.getPosition()
                     )
                 );
+                // correctedState.speedMetersPerSecond *= correctedState.angle.minus(new Rotation2d(turnAbsEncoder.getPosition())).minus(chassisAngularOffset).getCos();
             } else {
                 correctedState = SwerveModuleState.optimize(
                     correctedState,

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -92,7 +92,10 @@ public class SwerveDrive extends SubsystemBase {
     private Field2d field;
     private Field2d limelightField;
 
+    private boolean enabled;
+
     public SwerveDrive() {
+        enabled = true;
         initComponents();
         initMathModels();
         initSimulations();
@@ -263,10 +266,21 @@ public Command runUpdateConstants() {
         });
     }
 
+    public Command toggleEnabled() {
+        return runOnce(() -> {
+            System.out.println(enabled);
+            enabled = !enabled;
+        });
+    }
+
     public Command runDriveInputs(
         DoubleSupplier rawXSpeed, DoubleSupplier rawYSpeed, DoubleSupplier rawRotSpeed,
         BooleanSupplier robotCentric, BooleanSupplier noOptimize, boolean rateLimited) {
         return run(() -> {
+            if(!enabled) {
+                adjustedDriveInputs(0, 0, 0, robotCentric.getAsBoolean(), noOptimize.getAsBoolean(), rateLimited);
+                return;
+            }
             double adjXSpeed = MathUtil.applyDeadband(rawXSpeed.getAsDouble(), 0.2);
             double adjYSpeed = MathUtil.applyDeadband(-rawYSpeed.getAsDouble(), 0.2);
 
@@ -393,9 +407,10 @@ private void addPresetPositionDriveInputs(double xSpeed, double ySpeed, double r
 
     public void rawDriveInputs(double rawXSpeed, double rawYSpeed, double rawRotSpeed, boolean noOptimize, boolean robotCentric) {
         SwerveModuleState[] states = SwerveConstants.kSwerveKinematics.toSwerveModuleStates(!robotCentric ?
-            ChassisSpeeds.fromFieldRelativeSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed, getRotation2d()) : // see if gyro is done correctly 
-            new ChassisSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed)
+        ChassisSpeeds.fromFieldRelativeSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed, getRotation2d()) : // see if gyro is done correctly 
+        new ChassisSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed)
         );
+        // System.out.println(states[0].angle.getDegrees() + " " + rawXSpeed);
         // System.out.println(rawXSpeed + " " + rawYSpeed + " " + rawRotSpeed);
         rawModuleInputs(states, noOptimize);
     }