It works

Frc5572 · Jan 27, 2024 · f3d576f · f3d576f
1 parent 2dedcaf
commit f3d576f
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 11 deletions.
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -68,6 +68,10 @@ public void periodic() {
      */
     public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
         desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
+        Logger.recordOutput("/ServeModule" + moduleNumber + "/DesiredState/Angle",
+            desiredState.angle);
+        Logger.recordOutput("/ServeModule" + moduleNumber + "/DesiredState/Speed",
+            desiredState.speedMetersPerSecond);
         io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations()));
         setSpeed(desiredState, isOpenLoop);
     }
@@ -89,7 +93,7 @@ private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
             driveVelocity.FeedForward =
                 driveFeedForward.calculate(desiredState.speedMetersPerSecond);
             io.setDriveMotor(driveVelocity);
-            inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output;
+            inputs.driveMotorSelectedSensorVelocity = driveVelocity.Velocity;
         }
     }
 

diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
@@ -53,7 +53,7 @@ private void configAngleMotor() {
         swerveAngleFXConfig.Slot0.kD = Constants.Swerve.ANGLE_KD;
 
         mAngleMotor.getConfigurator().apply(swerveAngleFXConfig);
-        mAngleMotor.setInverted(Constants.Swerve.ANGLE_MOTOT_INVERT);
+        mAngleMotor.setInverted(Constants.Swerve.ANGLE_MOTOR_INVERT);
     }
 
     private void configDriveMotor() {

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -107,34 +107,34 @@ public static final class Swerve {
         public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
 
         /* Angle Motor PID Values */
-        public static final double ANGLE_KP = 0.6;
+        public static final double ANGLE_KP = 100;
         public static final double ANGLE_KI = 0.0;
-        public static final double ANGLE_KD = 12.0;
+        public static final double ANGLE_KD = 0.0;
         public static final double ANGLE_KF = 0.0;
 
         /* Drive Motor PID Values */
-        public static final double DRIVE_KP = 0.10;
+        public static final double DRIVE_KP = 0.12;
         public static final double DRIVE_KI = 0.0;
         public static final double DRIVE_KD = 0.0;
         public static final double DRIVE_KF = 0.0;
 
         /* Drive Motor Characterization Values */
-        public static final double DRIVE_KS = (0.667 / 12);
+        public static final double DRIVE_KS = (0.667);
         // divide by 12 to convert from volts to percent output for CTRE
-        public static final double DRIVE_KV = (2.44 / 12);
-        public static final double DRIVE_KA = (0.27 / 12);
+        public static final double DRIVE_KV = (2.44);
+        public static final double DRIVE_KA = (0.27);
 
         /* Swerve Profiling Values */
         public static final double MAX_SPEED = 4; // meters per second
-        public static final double MAX_ANGULAR_VELOCITY = 4.0;
+        public static final double MAX_ANGULAR_VELOCITY = 10.0;
 
         /* Neutral Modes */
         public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast;
         public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake;
 
         /* Motor Inverts */
         public static final boolean DRIVE_MOTOR_INVERT = false;
-        public static final boolean ANGLE_MOTOT_INVERT = false;
+        public static final boolean ANGLE_MOTOR_INVERT = false;
 
         /* Angle Encoder Invert */
         public static final SensorDirectionValue CAN_CODER_INVERT =

diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -1,6 +1,7 @@
 package frc.robot.subsystems.swerve;
 
 import java.util.Optional;
+import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -84,7 +85,7 @@ rotation, getFieldRelativeHeading())
      */
     public void setModuleStates(SwerveModuleState[] desiredStates) {
         SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.MAX_SPEED);
-
+        Logger.recordOutput("Swerve/DesiredStates", desiredStates);
         for (SwerveModule mod : swerveMods) {
             mod.setDesiredState(desiredStates[mod.moduleNumber], false);
         }
@@ -116,6 +117,7 @@ public ChassisSpeeds getChassisSpeeds() {
      *
      * @return Array of Swerve Module States
      */
+    @AutoLogOutput(key = "Swerve/CurrentStates")
     public SwerveModuleState[] getModuleStates() {
         SwerveModuleState[] states = new SwerveModuleState[4];
         for (SwerveModule mod : swerveMods) {
@@ -142,6 +144,7 @@ public SwerveModulePosition[] getModulePositions() {
      *
      * @return Pose2d on the field
      */
+    @AutoLogOutput(key = "Odometry/Robot")
     public Pose2d getPose() {
         return swerveOdometry.getPoseMeters();
     }