Merge branch 'main' of https://github.com/Technologyman00/YAGSL-Example

src/main/deploy/swerve/falcon/modules/physicalproperties.json

@@ -1,7 +1,7 @@
 {
   "conversionFactor": {
-    "angle": 0.01373291015625,
-    "drive": 1.914649238933429E-5
+	"angle": 28.125,
+	"drive": 0.047286787200699704
   },
   "currentLimit": {
     "drive": 40,

src/main/deploy/swerve/falcon/modules/pidfproperties.json

@@ -1,15 +1,15 @@
 {
   "drive": {
-    "p": 0.15,
+    "p": 1,
     "i": 0,
-    "d": 2.0,
+    "d": 0,
     "f": 0,
     "iz": 0
   },
   "angle": {
-    "p": 0.050953,
+    "p": 360,
     "i": 0,
-    "d": 0.118,
+    "d": 0,
     "f": 0,
     "iz": 0
   }

src/main/deploy/swerve/maxSwerve/modules/physicalproperties.json

@@ -0,0 +1,16 @@
+{
+  "conversionFactor": {
+    "drive": 0.047286787200699704,
+    "angle": 360
+  },
+  "currentLimit": {
+    "drive": 40,
+    "angle": 20
+  },
+  "rampRate": {
+    "drive": 0.25,
+    "angle": 0.25
+  },
+  "optimalVoltage": 12,
+  "wheelGripCoefficientOfFriction": 1.19
+}

src/main/deploy/swerve/MaxSwerve-Neo-NavX/swervedrive.json → src/main/deploy/swerve/maxSwerve/swervedrive.json

@@ -1,12 +1,10 @@
 {
-  "maxSpeed": 14.5,
-  "optimalVoltage": 12,
   "imu": {
     "type": "navx_spi",
     "id": 0,
     "canbus": null
   },
-  "invertedIMU": true,
+  "invertedIMU": false,
   "modules": [
     "frontleft.json",
     "frontright.json",

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

@@ -47,6 +47,6 @@ public static class OperatorConstants
     public static final double LEFT_X_DEADBAND = 0.01;
     public static final double LEFT_Y_DEADBAND = 0.01;
     public static final double RIGHT_X_DEADBAND = 0.01;
-    public static final double TURN_CONSTANT = 0.75;
+    public static final double TURN_CONSTANT = 6;
   }
 }

src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java

@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
@@ -27,7 +26,7 @@ public class AbsoluteDriveAdv extends Command
   private final SwerveSubsystem swerve;
   private final DoubleSupplier  vX, vY;
   private final DoubleSupplier headingAdjust;
-  private boolean initRotation = false;
+  private boolean resetHeading = false;
   private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight;
 
   /**
@@ -67,7 +66,7 @@ public AbsoluteDriveAdv(SwerveSubsystem swerve, DoubleSupplier vX, DoubleSupplie
   @Override
   public void initialize()
   {
-    initRotation = true;
+    resetHeading = true;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -76,53 +75,43 @@ public void execute()
   {
     double headingX = 0;
     double headingY = 0;
-    Rotation2d newHeading = Rotation2d.fromRadians(0);
 
     // These are written to allow combinations for 45 angles
     // Face Away from Drivers
     if(lookAway.getAsBoolean()){
-      headingX = 1;
+      headingY = -1;
     }
     // Face Right
     if(lookRight.getAsBoolean()){
-      headingY = 1;
+      headingX = 1;
     }
     // Face Left
     if(lookLeft.getAsBoolean()){
-      headingY = -1;
+      headingX = -1;
     }
     // Face Towards the Drivers
     if(lookTowards.getAsBoolean()){
-      headingX = -1;
-    }
-
-    //Dont overwrite a button press
-    if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){
-      newHeading = Rotation2d.fromRadians(Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble())
-                                                                      .plus(swerve.getHeading());
-      headingX = newHeading.getSin();
-      headingY = newHeading.getCos();
+      headingY = 1;
     }
 
-    ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(),
-                                                         headingX,
-                                                         headingY);
-
     // Prevent Movement After Auto
-    if(initRotation)
+    if(resetHeading)
     {
-      if(headingX == 0 && headingY == 0)
+      if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0)
       {
-        // Get the curretHeading
-        Rotation2d firstLoopHeading = swerve.getHeading();
+        // Get the curret Heading
+        Rotation2d currentHeading = swerve.getHeading();
 
         // Set the Current Heading to the desired Heading
-        desiredSpeeds = swerve.getTargetSpeeds(0, 0, firstLoopHeading.getSin(), firstLoopHeading.getCos());
+        headingX = currentHeading.getSin();
+        headingY = currentHeading.getCos();
       }
-      //Dont Init Rotation Again
-      initRotation = false;
+      //Dont reset Heading Again
+      resetHeading = false;
     }
 
+    ChassisSpeeds desiredSpeeds = swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY);
+
     // Limit velocity to prevent tippy
     Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds);
     translation = SwerveMath.limitVelocity(translation, swerve.getFieldVelocity(), swerve.getPose(),
@@ -132,7 +121,13 @@ public void execute()
     SmartDashboard.putString("Translation", translation.toString());
 
     // Make the robot move
-    swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
+    if(headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0){
+      resetHeading = true;
+      swerve.drive(translation, (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), true);
+    }
+    else{
+      swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true);
+    }
   }
 
   // Called once the command ends or is interrupted.