Skip to content

Commit

Permalink
doesn't work 100%
Browse files Browse the repository at this point in the history
  • Loading branch information
@legoguy1000
legoguy1000 committed Jan 20, 2024
1 parent 98e5275 commit 68cbdaa
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 28 deletions.
25 changes: 17 additions & 8 deletions src/main/java/frc/lib/util/swerve/SwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,16 @@
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;
import com.ctre.phoenix6.signals.SensorDirectionValue;
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;
Expand All @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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.
Expand All @@ -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);
}

Expand Down Expand Up @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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
Expand All @@ -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;
Expand Down
21 changes: 9 additions & 12 deletions src/main/java/frc/robot/subsystems/Swerve.java
View file
Original file line number Diff line number Diff line change
@@ -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;
Expand All @@ -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;
Expand All @@ -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());
Expand All @@ -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);
}

Expand Down

0 comments on commit 68cbdaa

Please sign in to comment.