diff --git a/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java index bd5d48b..4e4eb82 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/SwerveModule.java @@ -3,17 +3,17 @@ 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.wpilibj2.command.SubsystemBase; -public interface SwerveModule { - public String getId(); - public Translation2d getLocation(); +public abstract class SwerveModule extends SubsystemBase { + public abstract Translation2d getLocation(); - public SwerveModuleState getState(); - public SwerveModulePosition getPosition(); + public abstract SwerveModuleState getState(); + public abstract SwerveModulePosition getPosition(); // in rotations and rotations/s - public double getDistance(); - public double getVelocity(); + public abstract double getDistance(); + public abstract double getVelocity(); - public void setVoltage(double voltage); + public abstract void setVoltage(double voltage); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/VoltageSwerve.java b/src/main/java/com/stuypulse/robot/subsystems/VoltageSwerve.java index ab04250..be6432b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/VoltageSwerve.java +++ b/src/main/java/com/stuypulse/robot/subsystems/VoltageSwerve.java @@ -8,8 +8,8 @@ import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.module.SimVoltageSwerveModule; -import com.stuypulse.robot.subsystems.module.VoltageSwerveModule; +import com.stuypulse.robot.subsystems.module.SwerveModuleSim; +import com.stuypulse.robot.subsystems.module.MaxSwerveModuleImpl; import com.stuypulse.stuylib.math.Angle; import com.stuypulse.stuylib.util.AngleVelocity; @@ -45,17 +45,17 @@ public class VoltageSwerve extends SubsystemBase { public VoltageSwerve() { if (Robot.isReal()) { modules = new SwerveModule[] { - new VoltageSwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE), - new VoltageSwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE), - new VoltageSwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE), - new VoltageSwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE) + new MaxSwerveModuleImpl(FrontRight.ID, FrontRight.MODULE_OFFSET, Ports.Swerve.FrontRight.TURN, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.DRIVE), + new MaxSwerveModuleImpl(FrontLeft.ID, FrontLeft.MODULE_OFFSET, Ports.Swerve.FrontLeft.TURN, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.DRIVE), + new MaxSwerveModuleImpl(BackLeft.ID, BackLeft.MODULE_OFFSET, Ports.Swerve.BackLeft.TURN, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.DRIVE), + new MaxSwerveModuleImpl(BackRight.ID, BackRight.MODULE_OFFSET, Ports.Swerve.BackRight.TURN, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.DRIVE) }; } else { modules = new SwerveModule[] { - new SimVoltageSwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET), - new SimVoltageSwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET), - new SimVoltageSwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET), - new SimVoltageSwerveModule(BackRight.ID, BackRight.MODULE_OFFSET) + new SwerveModuleSim(FrontRight.ID, FrontRight.MODULE_OFFSET), + new SwerveModuleSim(FrontLeft.ID, FrontLeft.MODULE_OFFSET), + new SwerveModuleSim(BackLeft.ID, BackLeft.MODULE_OFFSET), + new SwerveModuleSim(BackRight.ID, BackRight.MODULE_OFFSET) }; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/module/CANSwerveModuleImpl.java b/src/main/java/com/stuypulse/robot/subsystems/module/CANSwerveModuleImpl.java new file mode 100644 index 0000000..365bd50 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/module/CANSwerveModuleImpl.java @@ -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); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/module/VoltageSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/module/MaxSwerveModuleImpl.java similarity index 92% rename from src/main/java/com/stuypulse/robot/subsystems/module/VoltageSwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/module/MaxSwerveModuleImpl.java index c22c643..f543ce7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/module/VoltageSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/module/MaxSwerveModuleImpl.java @@ -13,15 +13,13 @@ import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.math.Angle; -import edu.wpi.first.math.MathUtil; 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; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class VoltageSwerveModule extends SubsystemBase implements SwerveModule { +public class MaxSwerveModuleImpl extends SwerveModule { private interface Turn { double kP = 3.5; @@ -47,8 +45,7 @@ private interface Turn { private double voltage; - public VoltageSwerveModule(String id, Translation2d location, int turnCANId, - Rotation2d angleOffset, int driveCANId) { + public MaxSwerveModuleImpl(String id, Translation2d location, int turnCANId, Rotation2d angleOffset, int driveCANId) { // module data this.id = id; @@ -92,11 +89,6 @@ private void configureDriveMotor() { driveEncoder.setPosition(0); } - @Override - public String getId() { - return id; - } - @Override public Translation2d getLocation() { return location; diff --git a/src/main/java/com/stuypulse/robot/subsystems/module/SimVoltageSwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/module/SwerveModuleSim.java similarity index 94% rename from src/main/java/com/stuypulse/robot/subsystems/module/SimVoltageSwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/module/SwerveModuleSim.java index 397a13a..662ad7b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/module/SimVoltageSwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/module/SwerveModuleSim.java @@ -19,9 +19,8 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.wpilibj.simulation.LinearSystemSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class SimVoltageSwerveModule extends SubsystemBase implements SwerveModule { +public class SwerveModuleSim extends SwerveModule { private static LinearSystem identifyVelocityPositionSystem(double kV, double kA) { if (kV <= 0.0) { @@ -70,7 +69,7 @@ private interface Drive { private double voltage; - public SimVoltageSwerveModule(String id, Translation2d location) { + public SwerveModuleSim(String id, Translation2d location) { // module data this.id = id; this.location = location; @@ -88,11 +87,6 @@ public SimVoltageSwerveModule(String id, Translation2d location) { driveSim = new LinearSystemSim<>(identifyVelocityPositionSystem(Drive.kV, Drive.kA)); } - @Override - public String getId() { - return id; - } - @Override public Translation2d getLocation() { return location;