From 68cbdaa749a23c71c9a11221409ccb3685aca58d Mon Sep 17 00:00:00 2001 From: Alex Resnick Date: Sat, 20 Jan 2024 15:17:50 -0600 Subject: [PATCH] doesn't work 100% --- .../frc/lib/util/swerve/SwerveModule.java | 25 +++++++++++++------ src/main/java/frc/robot/Constants.java | 16 ++++++------ .../java/frc/robot/subsystems/Swerve.java | 21 +++++++--------- 3 files changed, 34 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java index c438ccf..2621e9d 100644 --- a/src/main/java/frc/lib/util/swerve/SwerveModule.java +++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java @@ -3,8 +3,8 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.math.Conversions; import frc.lib.util.ctre.CTREModuleState; import frc.robot.Constants; @@ -22,6 +23,7 @@ public class SwerveModule { public int moduleNumber; private double angleOffset; + private double absoluteOffset = 0.0; private TalonFX angleMotor; private TalonFX driveMotor; private CANcoder angleEncoder; @@ -30,8 +32,8 @@ public class SwerveModule { private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); private CANcoderConfiguration swerveCanCoderConfig = new CANcoderConfiguration(); private DutyCycleOut drivePower = new DutyCycleOut(0); - private VelocityDutyCycle driveSpeed = new VelocityDutyCycle(0).withSlot(0); - private PositionDutyCycle angleMotorPosition = new PositionDutyCycle(0).withSlot(0); + private VelocityVoltage driveSpeed = new VelocityVoltage(0).withSlot(0); + private PositionVoltage angleMotorPosition = new PositionVoltage(0).withSlot(0); // Use Voltage to turn Voltage Compensation on // private VelocityVoltage driveSpeed = new VelocityVoltage(0).withSlot(0); // private PositionVoltage angleMotorPosition = new PositionVoltage(0).withSlot(0); @@ -81,7 +83,11 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) // See Line 186, swerveDriveFXConfig.Feedback.SensorToMechanismRatio double velocity = Conversions.mpsToFalconRPS(desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference, 1); - driveMotor.setControl(driveSpeed.withVelocity(velocity)); + if (Math.abs(velocity) <= 0.01) { + driveMotor.setControl(drivePower.withOutput(0.0)); + } else { + driveMotor.setControl(driveSpeed.withVelocity(velocity)); + } } // Prevent rotating module if speed is less then 1%. Prevents Jittering. @@ -96,12 +102,15 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) lastAngle = angle; } + public void periodic() { + SmartDashboard.putNumber("Mod" + moduleNumber + "", ) + } + /** * */ private void resetToAbsolute() { - double absolutePosition = getCanCoderRaw() / Constants.Swerve.angleGearRatio; - swerveAngleFXConfig.Feedback.FeedbackRotorOffset = absolutePosition; + absoluteOffset = getCanCoderRaw(); angleMotor.getConfigurator().apply(swerveAngleFXConfig); } @@ -152,7 +161,7 @@ private void configAngleMotor() { swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; // swerveAngleFXConfig.Slot0.kS = 0; // swerveAngleFXConfig.Slot0.kV = 0; - swerveAngleFXConfig.Feedback.SensorToMechanismRatio = 1 / Constants.Swerve.angleGearRatio; + swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; // swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; angleMotor.getConfigurator().apply(swerveAngleFXConfig); angleMotor.setInverted(Constants.Swerve.angleMotorInvert); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e71f247..8eda2f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -72,7 +72,7 @@ public static final class Swerve { public static final double closedLoopRamp = 0.4; public static final double driveGearRatio = (8.14 / 1.0); // 8.14:1 - public static final double angleGearRatio = 1 / ((14.0 / 50.0) * (10.0 / 60.0)); // 150/7:1 + public static final double angleGearRatio = -1 / ((14.0 / 50.0) * (10.0 / 60.0)); // 150/7:1 public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(new Translation2d(wheelBase / 2.0, trackWidth / 2.0), @@ -92,22 +92,22 @@ public static final class Swerve { public static final boolean driveEnableCurrentLimit = true; /* Angle Motor PID Values */ - public static final double angleKP = 0.6; + public static final double angleKP = -0.6 * 12.0; public static final double angleKI = 0.0; - public static final double angleKD = 12.0; + public static final double angleKD = 0.0; public static final double angleKF = 0.0; /* Drive Motor PID Values */ - public static final double driveKP = 0.10; + public static final double driveKP = 0.1; public static final double driveKI = 0.0; public static final double driveKD = 0.0; public static final double driveKF = 0.0; /* Drive Motor Characterization Values */ - public static final double driveKS = (0.667 / 12); + public static final double driveKS = (0.667); // divide by 12 to convert from volts to percent output for CTRE - public static final double driveKV = (2.44 / 12); - public static final double driveKA = (0.27 / 12); + public static final double driveKV = (2.44); + public static final double driveKA = (0.27); /* Swerve Profiling Values */ public static final double maxSpeed = 4; // meters per second @@ -119,7 +119,7 @@ public static final class Swerve { /* Motor Inverts */ public static final boolean driveMotorInvert = true; - public static final boolean angleMotorInvert = true; + public static final boolean angleMotorInvert = false; /* Angle Encoder Invert */ public static final boolean canCoderInvert = false; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index de9831d..bb62582 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -1,6 +1,5 @@ package frc.robot.subsystems; -import org.photonvision.PhotonCamera; import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -22,7 +21,7 @@ public class Swerve extends SubsystemBase { public AHRS gyro = new AHRS(Constants.Swerve.navXID); public SwerveDrivePoseEstimator swerveOdometry; - private PhotonCamera cam = new PhotonCamera(Constants.CameraConstants.cameraName); + // private PhotonCamera cam = new PhotonCamera(Constants.CameraConstants.cameraName); public SwerveModule[] swerveMods; private double fieldOffset = gyro.getYaw(); ChassisSpeeds chassisSpeeds; @@ -36,10 +35,10 @@ public Swerve() { SmartDashboard.putData("Field Pos", field); - swerveMods = new SwerveModule[] {new SwerveModule(0, Constants.Swerve.Mod0.constants),}; - // new SwerveModule(1, Constants.Swerve.Mod1.constants), - // new SwerveModule(2, Constants.Swerve.Mod2.constants), - // new SwerveModule(3, Constants.Swerve.Mod3.constants)}; + swerveMods = new SwerveModule[] {new SwerveModule(0, Constants.Swerve.Mod0.constants), + new SwerveModule(1, Constants.Swerve.Mod1.constants), + new SwerveModule(2, Constants.Swerve.Mod2.constants), + new SwerveModule(3, Constants.Swerve.Mod3.constants)}; swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.swerveKinematics, getYaw(), getPositions(), new Pose2d()); @@ -50,12 +49,10 @@ public Swerve() { */ public void wheelsIn() { swerveMods[0].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(45)), false); - // swerveMods[1].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(135)), - // false); - // swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), - // false); - // swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)), - // false); + swerveMods[1].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(135)), false); + swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), false); + swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)), + false); this.setMotorsZero(Constants.Swerve.isOpenLoop, Constants.Swerve.isFieldRelative); }