Merge branch 'main' into main

BroncBotz3481 · Jan 17, 2024 · 1bd2745 · 1bd2745
2 parents dba9849 + 7df18bf
commit 1bd2745
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 32 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/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;
   }
 }
diff --git a/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/swervedrive/drivebase/AbsoluteDriveAdv.java
@@ -26,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;
 
   /**
@@ -66,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.
@@ -75,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(),
@@ -131,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.

diff --git a/src/main/java/swervelib/encoders/CANCoderSwerve.java b/src/main/java/swervelib/encoders/CANCoderSwerve.java
@@ -120,7 +120,11 @@ public double getAbsolutePosition()
       readingError = true;
       readingFaulty.set(true);
       return 0;
-    }else readingFaulty.set(false);
+    }else
+    {
+      readingFaulty.set(false);
+    }
+
     StatusSignal<Double> angle = encoder.getAbsolutePosition().refresh();
 
     // Taken from democat's library.

diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java
@@ -323,7 +323,7 @@ public void setReference(double setpoint, double feedforward, double position)
 
     if (isDriveMotor)
     {
-      motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint));
+      motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward));
     } else
     {
       motor.setControl(m_angleVoltageSetter.withPosition(setpoint));

diff --git a/src/main/java/swervelib/parser/SwerveDriveConfiguration.java b/src/main/java/swervelib/parser/SwerveDriveConfiguration.java
@@ -89,7 +89,19 @@ public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves, SimpleM
    */
   public double getDriveBaseRadiusMeters()
   {
-    Translation2d furthestModule = moduleLocationsMeters[0];
-    return Math.abs(Math.sqrt(Math.pow(furthestModule.getX(), 2) + Math.pow(furthestModule.getY(), 2)));
+    //Find Center of Robot by adding all module offsets together. Should be zero, but incase it isn't
+    Translation2d centerOfModules = moduleLocationsMeters[0].plus(moduleLocationsMeters[1])
+                                    .plus(moduleLocationsMeters[2]).plus(moduleLocationsMeters[3]);
+
+    //Find Largest Radius by checking the distance to the center point 
+    double largestRadius = centerOfModules.getDistance(moduleLocationsMeters[0]);     
+    for(int i=1; i<moduleLocationsMeters.length; i++){
+      if(largestRadius < centerOfModules.getDistance(moduleLocationsMeters[i])){
+        largestRadius = centerOfModules.getDistance(moduleLocationsMeters[i]);
+      }
+    }          
+
+    //Return Largest Radius
+    return largestRadius;
   }
 }