Merge branch 'drive-fix'

carokhan · Oct 16, 2024 · c0b5501 · c0b5501
2 parents 223c47c + cd5533e
commit c0b5501
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 21 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -31,7 +31,7 @@
 public final class Constants {
   public static final double loopPeriodSecs = 0.02;
 
-  public static final Mode currentMode = Mode.SIM;
+  public static final Mode currentMode = Mode.REAL;
 
   public static enum Mode {
     /** Running on a real robot. */
@@ -57,8 +57,8 @@ public static class Drive {
       public static final int backRightDrive = 7;
       public static final int backRightTurn = 8;
 
-      public static final boolean frontLeftDriveInvert = true;
-      public static final boolean frontRightDriveInvert = false;
+      public static final boolean frontLeftDriveInvert = false;
+      public static final boolean frontRightDriveInvert = true;
       public static final boolean backLeftDriveInvert = false; // true?
       public static final boolean backRightDriveInvert = true; // false?
 
@@ -72,7 +72,7 @@ public static class Drive {
       public static final int backLeftEncoder = 1;
       public static final int backRightEncoder = 0;
 
-      public static final double frontLeftOffset = -0.5107599496841431;
+      public static final double frontLeftOffset = 0.807;
       public static final double frontRightOffset = -0.5107599496841431;
       public static final double backLeftOffset = 1.8345500230789185;
       public static final double backRightOffset = 2.437732458114624;
@@ -126,8 +126,8 @@ public static class DriveConstants {
     public static final double odometeryFrequency = 250;
     public static final double updateFrequency = 100;
 
-    // public static final double maxLinearVelocity = Units.feetToMeters(20.4);
-    public static final double maxLinearVelocity = Units.feetToMeters(1.4);
+    public static final double maxLinearVelocity = Units.feetToMeters(20.4);
+    // public static final double maxLinearVelocity = Units.feetToMeters(1.4);
     public static final double maxLinearAccel = 8.0;
 
     public static final double maxAngularVelocity = 10;
@@ -139,7 +139,7 @@ public static class DriveConstants {
     public static double kVDriveReal = 1.93;
     public static double kADriveReal = 0.25;
 
-    public static double kPTurnReal = 1.5; // 1.5?
+    public static double kPTurnReal = 3; // 1.5?
     public static double kDTurnReal = 0.0;
 
     public static double kPDriveSim = 0.3;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -177,6 +177,8 @@ private void configureButtonBindings() {
     // right trigger -> climb up
     m_driver.rightTrigger(0.1).onTrue(m_climber.setDutyCycle(1)).onFalse(m_climber.setDutyCycle(0));
 
+    m_driver.y().whileTrue(m_drive.zeroGyro());
+
     // Operator Controller
 
     // D-Pad Up for intake down, rollers forward, until note in feeder beambreak
@@ -251,7 +253,7 @@ private void configureButtonBindings() {
         .a()
         .whileTrue(
             Commands.parallel(
-                    m_pivot.setPivotTarget(() -> Units.degreesToRadians(34)),
+                    m_pivot.setPivotTarget(() -> Units.degreesToRadians(40)),
                     m_shooter.setRPM(() -> -1500, 1.0),
                     m_feeder.setRPM(() -> -1500),
                     m_intake.setRollerRPM(() -> -1000))
@@ -282,7 +284,7 @@ public void robotPeriodic() {
   }
 
   private static double teleopAxisAdjustment(double x) {
-    return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02);
+    return MathUtil.applyDeadband(x, 0.02);
   }
 
   public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -278,6 +278,13 @@ public Rotation2d getRotation() {
     return gyroIO.getYaw();
   }
 
+  public Command zeroGyro() {
+    return this.run(
+        () -> {
+          gyroIO.setYaw(0);
+        });
+  }
+
   @AutoLogOutput(key = "Odometry/Robot")
   public Pose2d getPose() {
     return poseEstimator.getEstimatedPosition();

diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOReal.java
@@ -73,7 +73,7 @@ public class ModuleIOReal implements ModuleIO {
   private final StatusSignal<Double> driveVelocity;
   private final StatusSignal<Double> driveAppliedVolts;
   private final StatusSignal<Double> driveCurrent;
-  // private int multiplier;
+  private int multiplier;
 
   private final VelocityVoltage driveCurrentVelocity =
       new VelocityVoltage(0.0).withEnableFOC(false);
@@ -93,7 +93,7 @@ public ModuleIOReal(int index) {
         absoluteEncoderOffset =
             new Rotation2d(RobotMap.Drive.frontLeftOffset); // MUST BE CALIBRATED
         name = "FrontLeft";
-        // multiplier = -1;
+        multiplier = 1;
         break;
       case 1:
         driveTalon = new TalonFX(RobotMap.Drive.frontRightDrive);
@@ -104,7 +104,7 @@ public ModuleIOReal(int index) {
         absoluteEncoderOffset =
             new Rotation2d(RobotMap.Drive.frontRightOffset); // MUST BE CALIBRATED
         name = "FrontRight";
-        // multiplier = 1;
+        multiplier = 1;
         break;
       case 2:
         driveTalon = new TalonFX(RobotMap.Drive.backLeftDrive);
@@ -114,7 +114,7 @@ public ModuleIOReal(int index) {
         absoluteEncoder = new AnalogEncoder(RobotMap.Drive.backLeftEncoder);
         absoluteEncoderOffset = new Rotation2d(RobotMap.Drive.backLeftOffset); // MUST BE CALIBRATED
         name = "BackLeft";
-        // multiplier = 1;
+        multiplier = 1;
         break;
       case 3:
         driveTalon = new TalonFX(RobotMap.Drive.backRightDrive);
@@ -125,7 +125,7 @@ public ModuleIOReal(int index) {
         absoluteEncoderOffset =
             new Rotation2d(RobotMap.Drive.backRightOffset); // MUST BE CALIBRATED
         name = "BackRight";
-        // multiplier = 1;
+        multiplier = 1;
         break;
       default:
         throw new RuntimeException("Invalid module index");
@@ -163,10 +163,7 @@ public ModuleIOReal(int index) {
 
     turnRelativeEncoder = turnSparkMax.getEncoder();
 
-    turnRelativeEncoder.setPositionConversionFactor(DriveConstants.turnConversion);
     absoluteEncoder.setDistancePerRotation(2 * Math.PI);
-
-    turnRelativeEncoder.setVelocityConversionFactor(DriveConstants.turnConversion * 60);
     turnPID = turnSparkMax.getPIDController();
 
     turnSparkMax.restoreFactoryDefaults();
@@ -292,10 +289,11 @@ public void runDriveVelocitySetpoint(
         && MathUtil.isNear(0.0, driveVelocity.getValueAsDouble(), 0.1)) {
       runDriveVoltage(0.0);
     } else {
-      driveTalon.setControl(
-          driveCurrentVelocity
-              .withVelocity(metersPerSecond)
-              .withFeedForward(metersPerSecondSquared * driveConfig.Slot0.kA));
+      runDriveVoltage(
+          Math.signum(metersPerSecond)
+              * Math.pow((metersPerSecond / DriveConstants.maxLinearVelocity), 2)
+              * 12.0
+              * multiplier);
     }
   }