learn more git mad

TitaniumTitans · Jan 27, 2024 · 1e79a75 · 1e79a75
1 parent 284de37
commit 1e79a75
Show file tree

Hide file tree

Showing 5 changed files with 14 additions and 15 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -53,7 +53,7 @@ private DriveConstants() {
     // kP, kI, kD in order
     protected static final double[] DRIVE_FB_GAINS = new double[]{0.05, 0.0, 0.0};
     // kP, kI, kD in order
-    protected static final double[] TURN_FB_GAINS = new double[]{7.0, 0.0, 0.0};
+    protected static final double[] TURN_FB_GAINS = new double[]{0.1, 0.0, 0.0};
 
     public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants(
             0,
@@ -84,7 +84,7 @@ private DriveConstants() {
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
             Units.rotationsToDegrees(-0.017578),
-            false,
+            true,
             ModuleConstants.GearRatios.L3
     );
 
@@ -95,7 +95,7 @@ private DriveConstants() {
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
             Units.rotationsToDegrees(-0.263916),
-            false,
+            true,
             ModuleConstants.GearRatios.L3
     );
   }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -120,8 +120,8 @@ private void configureButtonBindings() {
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(
                 m_driveSubsystem,
-            () -> -controller.getLeftY(),
             () -> -controller.getLeftX(),
+            () -> -controller.getLeftY(),
             () -> -controller.getRightX()));
     controller.x().onTrue(Commands.runOnce(m_driveSubsystem::stopWithX, m_driveSubsystem));
     controller

diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java
@@ -53,7 +53,7 @@ public Module(ModuleIO io) {
         case REAL, REPLAY -> {
           m_driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13);
           m_driveFeedback = new PIDController(0.05, 0.0, 0.0);
-          m_turnFeedback = new PIDController(7.0, 0.0, 0.0);
+          m_turnFeedback = new PIDController(0.1, 0.0, 0.0);
         }
         case SIM -> {
           m_driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13);
@@ -90,8 +90,7 @@ public void periodic() {
 
     // Run closed loop turn control
     if (m_angleSetpoint != null) {
-      m_io.setTurnPositionDegs(
-          m_turnFeedback.calculate(getAngle().getRadians(), m_angleSetpoint.getRadians()));
+      m_io.setTurnPositionDegs(m_angleSetpoint.getDegrees());
 
       // Run closed loop drive control
       // Only allowed if closed loop turn control is running
@@ -104,10 +103,10 @@ public void periodic() {
         double adjustSpeedSetpoint = m_speedSetpoint * Math.cos(m_turnFeedback.getPositionError());
 
         // Run drive controller
-        double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS;
-        m_io.setDriveVelocityMPS(
-            m_driveFeedforward.calculate(velocityRadPerSec)
-                + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
+        m_io.setDriveVelocityMPS(adjustSpeedSetpoint);
+//        m_io.setDriveVelocityMPS(
+//            m_driveFeedforward.calculate(velocityRadPerSec)
+//                + m_driveFeedback.calculate(m_inputs.getDriveVelocityRadPerSec(), velocityRadPerSec));
       }
     }
 

diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleConstants.java
@@ -47,7 +47,7 @@ public ModuleConstants(int id,
             DEFAULT_WHEEL_RADIUS_METERS * 2 * Math.PI,
             inverted,
             !inverted,
-            !inverted,
+            false,
             Rotation2d.fromDegrees(offsetDegs),
             id,
             ids[0], ids[1], ids[2],

diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java
@@ -83,7 +83,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
     driveConfig.MotorOutput.Inverted =
             moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
-    driveConfig.Feedback.RotorToSensorRatio = m_moduleConstants.DRIVE_GEAR_RATIO();
+    driveConfig.Feedback.SensorToMechanismRatio = m_moduleConstants.DRIVE_GEAR_RATIO();
     m_driveTalon.getConfigurator().apply(driveConfig);
     setDriveBrakeMode(true);
 
@@ -106,7 +106,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
             moduleConstants.TURN_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;
     turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
-    turnConfig.Feedback.RotorToSensorRatio = m_moduleConstants.TURNING_GEAR_RATIO();
+    turnConfig.Feedback.SensorToMechanismRatio = m_moduleConstants.TURNING_GEAR_RATIO();
     m_turnTalon.getConfigurator().apply(turnConfig);
     setTurnBrakeMode(true);
 
@@ -191,7 +191,7 @@ public void updateInputs(ModuleIOInputs inputs) {
     inputs.setTurnAbsolutePosition(Rotation2d.fromRotations(m_turnAbsolutePosition.getValueAsDouble())
         .minus(m_moduleConstants.ENCODER_OFFSET()));
     inputs.setTurnPosition(Rotation2d.fromRotations((
-            m_turnPosition.getValueAsDouble() / m_moduleConstants.TURNING_GEAR_RATIO())));
+            m_turnPosition.getValueAsDouble())));
     inputs.setTurnVelocityRadPerSec(
             Units.rotationsToRadians(m_turnVelocity.getValueAsDouble()) / m_moduleConstants.TURNING_GEAR_RATIO());
     inputs.setTurnAppliedVolts(m_turnAppliedVolts.getValueAsDouble());