Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,15 +1,128 @@ | ||
| package frc.lib.util.swerve; | ||
|
|
||
| import com.ctre.phoenix6.configs.CANcoderConfiguration; | ||
| import com.ctre.phoenix6.configs.TalonFXConfiguration; | ||
| import com.ctre.phoenix6.controls.ControlRequest; | ||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import frc.robot.Constants; | ||
|
|
||
| public class SwerveModuleReal implements SwerveModuleIO { | ||
|
Check warning on line 11 in src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
|
||
| private TalonFX mAngleMotor; | ||
| private TalonFX mDriveMotor; | ||
| private CANcoder angleEncoder; | ||
| private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration(); | ||
| private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration(); | ||
| private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration(); | ||
|
|
||
| public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int angleCoderID, | ||
|
Check warning on line 19 in src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
|
||
| Rotation2d angleOffset) { | ||
|
|
||
| mAngleMotor = new TalonFX(angleMotorID, "canivore"); | ||
| mDriveMotor = new TalonFX(driveMotorID, "canivore"); | ||
| angleEncoder = new CANcoder(angleCoderID, "canivore"); | ||
|
|
||
| configAngleEncoder(); | ||
| configMotorDrive(); | ||
| configAngleMotor(); | ||
|
|
||
|
|
||
| } | ||
|
|
||
| private void configAngleMotor() { | ||
| /* Angle Motor Config */ | ||
| /* Motor Inverts and Neutral Mode */ | ||
| swerveAngleFXConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert; | ||
| swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; | ||
|
|
||
| /* Gear Ratio and Wrapping Config */ | ||
| swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.angleGearRatio; | ||
| swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; | ||
|
|
||
| /* Current Limiting */ | ||
| swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = | ||
| Constants.Swerve.angleEnableCurrentLimit; | ||
| swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; | ||
| swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = | ||
| Constants.Swerve.angleCurrentThreshold; | ||
| swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = | ||
| Constants.Swerve.angleCurrentThresholdTime; | ||
|
|
||
| /* PID Config */ | ||
| swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP; | ||
| swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI; | ||
| swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD; | ||
|
|
||
| mAngleMotor.getConfigurator().apply(swerveAngleFXConfig); | ||
|
|
||
| } | ||
|
|
||
| public void configMotorDrive() { | ||
|
Check warning on line 61 in src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
|
||
| /* Drive Motor Config */ | ||
| /* Motor Inverts and Neutral Mode */ | ||
| swerveDriveFXConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert; | ||
| swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode; | ||
|
|
||
| /* Gear Ratio Config */ | ||
| swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; | ||
|
|
||
| /* Current Limiting */ | ||
| swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = | ||
| Constants.Swerve.driveEnableCurrentLimit; | ||
| swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; | ||
| swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = | ||
| Constants.Swerve.driveCurrentThreshold; | ||
| swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = | ||
| Constants.Swerve.driveCurrentThresholdTime; | ||
|
|
||
| /* PID Config */ | ||
| swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP; | ||
| swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI; | ||
| swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD; | ||
|
|
||
| /* Open and Closed Loop Ramping */ | ||
| swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = | ||
| Constants.Swerve.openLoopRamp; | ||
| swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; | ||
|
|
||
| swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = | ||
| Constants.Swerve.closedLoopRamp; | ||
| swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = | ||
| Constants.Swerve.closedLoopRamp; | ||
|
|
||
| mDriveMotor.getConfigurator().apply(swerveDriveFXConfig); | ||
| mDriveMotor.getConfigurator().setPosition(0.0); | ||
| } | ||
|
|
||
| public void configAngleEncoder() { | ||
|
Check warning on line 98 in src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
|
||
| /* Angle Encoder Config */ | ||
| swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; | ||
|
|
||
| angleEncoder.getConfigurator().apply(swerveCANcoderConfig); | ||
| } | ||
|
|
||
| @Override | ||
| public void setAngleMotor(ControlRequest request) { | ||
| mAngleMotor.setControl(request); | ||
| } | ||
|
|
||
| @Override | ||
| public void setDriveMotor(ControlRequest request) { | ||
| mDriveMotor.setControl(request); | ||
| } | ||
|
|
||
| @Override | ||
| public void updateInputs(SwerveModuleInputs inputs) { | ||
| inputs.driveMotorSelectedPosition = mDriveMotor.getPosition().getValueAsDouble(); | ||
| inputs.driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity().getValueAsDouble(); | ||
| inputs.angleMotorSelectedPosition = mAngleMotor.getPosition().getValueAsDouble(); | ||
| inputs.absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition().getValueAsDouble(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setPositionAngleMotor(double absolutePosition) { | ||
| mAngleMotor.setPosition(absolutePosition); | ||
| } | ||
|
|
||
| } | ||
Oops, something went wrong.