Swerve fixes

src/main/deploy/pathplanner/New Path.path

@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}

src/main/java/frc4388/robot/Constants.java

@@ -53,10 +53,10 @@ public final class Constants {
     public static final int GYRO_ID = 14;
     
     // ofsets are in degrees
-    public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
-    public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
-    public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
-    public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
+    public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
+    public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
+    public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
+    public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
 
     // swerve PID constants
     public static final int SWERVE_SLOT_IDX = 0;

src/main/java/frc4388/robot/RobotMap.java

@@ -91,14 +91,15 @@ public class RobotMap {
     rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
 
-    leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
-    leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
-    rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
-    leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
-    rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
-    rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
+    NeutralMode mode = NeutralMode.Brake;
+    leftFrontSteerMotor.setNeutralMode(mode);
+    leftFrontWheelMotor.setNeutralMode(mode);//Coast
+    rightFrontSteerMotor.setNeutralMode(mode);
+    rightFrontWheelMotor.setNeutralMode(mode);//Coast
+    leftBackSteerMotor.setNeutralMode(mode);
+    leftBackWheelMotor.setNeutralMode(mode);//Coast
+    rightBackSteerMotor.setNeutralMode(mode);
+    rightBackWheelMotor.setNeutralMode(mode);//Coast
 
     leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
     leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
@@ -49,9 +50,9 @@ public class SwerveModule extends SubsystemBase {
     angleMotor.configAllSettings(angleTalonFXConfiguration);
 
     TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
-    driveTalonFXConfiguration.slot0.kP = 0.15;
+    driveTalonFXConfiguration.slot0.kP = 0.1;
     driveTalonFXConfiguration.slot0.kI = 0.0;
-    driveTalonFXConfiguration.slot0.kD = 0.5;
+    driveTalonFXConfiguration.slot0.kD = 0.0;
     driveMotor.configAllSettings(driveTalonFXConfiguration);
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
@@ -72,8 +73,7 @@ public class SwerveModule extends SubsystemBase {
    */
   public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
     Rotation2d currentRotation = getAngle();
-    
-    //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+    // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
     state = SwerveModuleState.optimize(desiredState, currentRotation);
 
     // Find the difference between our current rotational position + our new rotational position
@@ -84,17 +84,23 @@ public class SwerveModule extends SubsystemBase {
     // Convert the CANCoder from it's position reading back to ticks
     double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
     double desiredTicks = currentTicks + deltaTicks;
+
     if (!ignoreAngle){
       angleMotor.set(TalonFXControlMode.Position, desiredTicks);
     }
 
 
     double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
+<<<<<<< Updated upstream
 
     // Add this line to correct for the slight drive movement the angle motor makes when turning in place. 
     // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
     
     driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
+=======
+    // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+    driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get());
+>>>>>>> Stashed changes
   }
 
   /**
@@ -114,5 +120,10 @@ public class SwerveModule extends SubsystemBase {
     driveMotor.set(0);
     angleMotor.set(0);
   }
+  @Override
+  public void periodic(){
+    Rotation2d currentRotation = getAngle();
+    SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+  }
 
 }