generated from StuyPulse/Phil
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add CANSwerveModuleImpl, rename stuff
- Loading branch information
Prog694
committed
Nov 21, 2023
1 parent
94a8689
commit e545c42
Showing
5 changed files
with
156 additions
and
36 deletions.
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
134 changes: 134 additions & 0 deletions
134
src/main/java/com/stuypulse/robot/subsystems/module/CANSwerveModuleImpl.java
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 |
|---|---|---|
| @@ -0,0 +1,134 @@ | ||
| package com.stuypulse.robot.subsystems.module; | ||
|
|
||
| import com.ctre.phoenix.sensors.CANCoder; | ||
| import com.revrobotics.CANSparkMax; | ||
| import com.revrobotics.RelativeEncoder; | ||
| import com.revrobotics.CANSparkMaxLowLevel.MotorType; | ||
| import com.stuypulse.robot.constants.Motors; | ||
| import com.stuypulse.robot.constants.Settings; | ||
| import com.stuypulse.robot.subsystems.SwerveModule; | ||
| import com.stuypulse.stuylib.control.angle.AngleController; | ||
| import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; | ||
| import com.stuypulse.stuylib.math.Angle; | ||
|
|
||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.math.kinematics.SwerveModulePosition; | ||
| import edu.wpi.first.math.kinematics.SwerveModuleState; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
|
|
||
| public class CANSwerveModuleImpl extends SwerveModule { | ||
|
|
||
| private interface Turn { | ||
| double kP = 3.5; | ||
| double kI = 0.0; | ||
| double kD = 0.0; | ||
| } | ||
|
|
||
| // module data | ||
| private String id; | ||
| private Translation2d location; | ||
| private Rotation2d angleOffset; | ||
|
|
||
| // turn | ||
| private CANSparkMax turnMotor; | ||
| private CANCoder absoluteEncoder; | ||
|
|
||
| // drive | ||
| private CANSparkMax driveMotor; | ||
| private RelativeEncoder driveEncoder; | ||
|
|
||
| // controller | ||
| private AngleController turnPID; | ||
|
|
||
| private double voltage; | ||
|
|
||
| public CANSwerveModuleImpl(String id, Translation2d location, int turnCANId, Rotation2d angleOffset, int driveCANId, int turnEncoderId) { | ||
|
|
||
| // module data | ||
| this.id = id; | ||
| this.location = location; | ||
| this.angleOffset = angleOffset; | ||
|
|
||
| // turn | ||
| turnMotor = new CANSparkMax(turnCANId, MotorType.kBrushless); | ||
| turnPID = new AnglePIDController(Turn.kP, Turn.kI, Turn.kD); | ||
| absoluteEncoder = new CANCoder(turnEncoderId); | ||
| configureTurnMotor(angleOffset); | ||
|
|
||
| // drive | ||
| driveMotor = new CANSparkMax(driveCANId, MotorType.kBrushless); | ||
| configureDriveMotor(); | ||
| } | ||
|
|
||
| private void configureTurnMotor(Rotation2d angleOffset) { | ||
| turnMotor.restoreFactoryDefaults(); | ||
|
|
||
| turnMotor.enableVoltageCompensation(12.0); | ||
|
|
||
| Motors.TURN.config(turnMotor); | ||
| } | ||
|
|
||
| private void configureDriveMotor() { | ||
| driveMotor.restoreFactoryDefaults(); | ||
|
|
||
| driveEncoder = driveMotor.getEncoder(); | ||
| // driveEncoder.setPositionConversionFactor(Encoder.Drive.POSITION_CONVERSION); | ||
| // driveEncoder.setVelocityConversionFactor(Encoder.Drive.VELOCITY_CONVERSION); | ||
|
|
||
| driveMotor.enableVoltageCompensation(12.0); | ||
| Motors.DRIVE.config(driveMotor); | ||
| driveEncoder.setPosition(0); | ||
| } | ||
|
|
||
| @Override | ||
| public Translation2d getLocation() { | ||
| return location; | ||
| } | ||
|
|
||
| @Override | ||
| public double getVelocity() { | ||
| return driveEncoder.getVelocity(); | ||
| } | ||
|
|
||
| @Override | ||
| public SwerveModuleState getState() { | ||
| return new SwerveModuleState(getVelocity() * Settings.Swerve.Encoder.Drive.WHEEL_CIRCUMFERENCE, getRotation2d()); | ||
| } | ||
|
|
||
| @Override | ||
| public SwerveModulePosition getPosition() { | ||
| return new SwerveModulePosition(getDistance(), getRotation2d()); | ||
| } | ||
|
|
||
| private Rotation2d getAbsolutePosition() { | ||
| return Rotation2d.fromDegrees(absoluteEncoder.getAbsolutePosition()); | ||
| } | ||
|
|
||
| private Rotation2d getRotation2d() { | ||
| return getAbsolutePosition().minus(angleOffset); | ||
| } | ||
|
|
||
| @Override | ||
| public double getDistance() { | ||
| return driveEncoder.getPosition(); | ||
| } | ||
|
|
||
| @Override | ||
| public void setVoltage(double voltage) { | ||
| this.voltage = voltage; | ||
| } | ||
|
|
||
| public void periodic() { | ||
| turnMotor.set(turnPID.update(Angle.kZero, Angle.fromRotation2d(getRotation2d()))); | ||
| driveMotor.setVoltage(voltage); | ||
|
|
||
| SmartDashboard.putNumber(id + "/Angle", getRotation2d().getDegrees()); | ||
| SmartDashboard.putNumber(id + "/Absolute Angle", getAbsolutePosition().getDegrees()); | ||
|
|
||
| SmartDashboard.putNumber(id + "/Velocity", getVelocity()); | ||
|
|
||
| SmartDashboard.putNumber(id + "/Drive Voltage", voltage); | ||
| } | ||
|
|
||
| } |
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