auto tuning + inverted DT sides

src/main/deploy/pathplanner/paths/Center Sub to Note 2.path

@@ -8,19 +8,19 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.3216735004898608,
+        "x": 1.7449183616315946,
-        "y": 5.541275082550166
+        "y": 5.55472336015897
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.289197721252586,
+        "x": 2.294543620426272,
         "y": 5.541275082550166
       },
       "prevControl": {
-        "x": 1.2891977212525858,
+        "x": 1.2945436204262721,
         "y": 5.541275082550166
       },
       "nextControl": null,

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

@@ -133,7 +133,7 @@ public class RobotContainer {
                 1.0, 
                 1.0
             ).raceWith(
-                new WaitCommand(0.5)
+                new WaitCommand(1.0)
             ).andThen(
                 shooter.angleSpeedsSetpoints(
                     () -> ShooterConstants.kShooterLoadAngle, 
@@ -228,7 +228,7 @@ public class RobotContainer {
             drivetrain.teleopCommand(
                 () -> { return driver.getLeftY() * (invertXYDrive ? -1 : 1); }, 
                 () -> { return driver.getLeftX() * (invertXYDrive ? -1 : 1); }, 
-                () -> { return -driver.getRightX() * (invertXYDrive ? -1 : 1); }, 
+                () -> { return driver.getRightX() * (invertXYDrive ? -1 : 1); }, 
                 OIConstants.kTeleopDriveDeadband
             )
         );
@@ -451,6 +451,6 @@ public class RobotContainer {
   }
 
   public Command getAutonomousCommand() {
-        return drivetrain.zeroHeadingCommand().andThen(autoChooser.getSelected());
+        return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -27,10 +27,10 @@ public final class DrivetrainConstants {
 
     // Angular offsets of the modules relative to the chassis in radians
     
-    public static final double kFrontLeftChassisAngularOffset = Math.PI;
+    public static final double kFrontLeftChassisAngularOffset = 0.0;// Math.PI
-    public static final double kFrontRightChassisAngularOffset = -Math.PI / 2;
+    public static final double kFrontRightChassisAngularOffset = Math.PI / 2;//-Math.PI / 2;
-    public static final double kBackLeftChassisAngularOffset = Math.PI / 2;
+    public static final double kBackLeftChassisAngularOffset = -Math.PI/2;//Math.PI / 2;
-    public static final double kBackRightChassisAngularOffset = 0;
+    public static final double kBackRightChassisAngularOffset = Math.PI; //0;
 
     /*
     public static final double kFrontLeftChassisAngularOffset = -2*Math.PI;
@@ -40,15 +40,15 @@ public final class DrivetrainConstants {
     */
 
     // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 8;
+    public static final int kFrontLeftDrivingCanId = 1;//8
-    public static final int kRearLeftDrivingCanId = 3;
+    public static final int kRearLeftDrivingCanId = 6;//3
-    public static final int kFrontRightDrivingCanId = 6;
+    public static final int kFrontRightDrivingCanId = 3;//6
-    public static final int kRearRightDrivingCanId = 1;
+    public static final int kRearRightDrivingCanId = 8;//1
 
-    public static final int kFrontLeftTurningCanId = 4;
+    public static final int kFrontLeftTurningCanId = 5;//4
-    public static final int kRearLeftTurningCanId = 7;
+    public static final int kRearLeftTurningCanId = 2;//7
-    public static final int kFrontRightTurningCanId = 2;
+    public static final int kFrontRightTurningCanId = 7;//2
-    public static final int kRearRightTurningCanId = 5;
+    public static final int kRearRightTurningCanId = 4;//5
 
     public static final double kTurnToleranceDeg = 0;
     public static final double kTurnRateToleranceDegPerS = 0;

src/main/java/frc/robot/subsystems/Climber.java

@@ -29,10 +29,12 @@ public class Climber extends SubsystemBase {
 
     public Command setSpeedWithSupplier(DoubleSupplier speed) {
         return run(() -> {
-            if(shooterAngle.getAsDouble() <= ShooterConstants.kShooterLoadAngle){
+            if(shooterAngle.getAsDouble() > ShooterConstants.kShooterLoadAngle){
            
                 motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
                  
+            }else{
+                motor1.set(VictorSPXControlMode.PercentOutput, 0.0);
             }
         });
     }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -207,6 +207,8 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
+        m_gyro.setAngleAdjustment(pose.getRotation().getDegrees());
+
         m_poseEstimator.resetPosition(
             Rotation2d.fromDegrees(getGyroAngle()),
             new SwerveModulePosition[] {
@@ -304,21 +306,29 @@ public class Drivetrain extends SubsystemBase {
     }
 
     public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
-        speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
+        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+        setModuleStates(newStates);
+        
+        /*
+        //speeds.omegaRadiansPerSecond = -speeds.omegaRadiansPerSecond;
         SwerveModuleState[] states = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(speeds);
 
         m_frontLeft.setDesiredState(states[0]);
         m_frontRight.setDesiredState(states[1]);
         m_rearLeft.setDesiredState(states[2]);
         m_rearRight.setDesiredState(states[3]);
+        */
     }
 
     public Command teleopCommand(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, double deadband) {
         return run(() -> {
             drive(
-                -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
+                MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
-                -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
+                MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
-                -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), 
+                MathUtil.applyDeadband(rotation.getAsDouble(), deadband), 
                 () -> fieldRelativeControl, 
                 true
             );