diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index a194291..e1e56e6 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -2,7 +2,9 @@ import java.util.Optional; -import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Driver.Drive; +import com.stuypulse.robot.constants.Settings.Driver.Turn; +import com.stuypulse.robot.constants.Settings.Swerve.Drive.*; import com.stuypulse.robot.subsystems.SwerveDrive; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.streams.IStream; @@ -42,11 +44,11 @@ public void initialize() { } private boolean isWithinTurnDeadband() { - return Math.abs(turn.get()) < Swerve.MAX_MODULE_TURN.get(); + return Math.abs(turn.get()) < Turn.DEADBAND.get(); } private boolean isWithinDriveDeadband() { - return Math.abs(speed.get().magnitude()) < Swerve.MAX_MODULE_SPEED.get(); + return Math.abs(speed.get().magnitude()) < Drive.DEADBAND.get(); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 3da49aa..215b7f6 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,6 +5,7 @@ package com.stuypulse.robot.constants; +import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.geometry.Rotation2d; @@ -21,12 +22,13 @@ public interface Settings { double DT = 0.02; public interface Swerve { - double WIDTH = Units.inchesToMeters(26.504); - double LENGTH = Units.inchesToMeters(20.508); + // TODO: Ask Harry for drivetrain dimensions + double WIDTH = Units.inchesToMeters(0); + double LENGTH = Units.inchesToMeters(0); - SmartNumber MODULE_SPEED_DEADBAND = new SmartNumber("Swerve/Module speed deadband", 0.02); - SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed", 5); - SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn", 6.28); + SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02); + SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Maximum module speed (rad per s)", 5); // Ask Harry for max theoretical speed + SmartNumber MAX_MODULE_TURN = new SmartNumber("Swerve/Maximum module turn (m per s)", 6.28); // Ask Harry for max theoretical turn public interface Turn { SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 3.5); @@ -46,34 +48,64 @@ public interface Drive { SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 2.4899); SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.41763); } - + + // measure offsets public interface FrontRight { String ID = "Front Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(357.213206) // recalibrated 4/21 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0) .plus(Rotation2d.fromDegrees(0)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5); } public interface FrontLeft { String ID = "Front Left"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(249.731491) // recalibrated 3/24 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0) .plus(Rotation2d.fromDegrees(270)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5); } public interface BackLeft { String ID = "Back Left"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(125.371964) // recalibrated 4/6 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0) .plus(Rotation2d.fromDegrees(180)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5); } public interface BackRight { String ID = "Back Right"; - Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(7.748473) // recalibrated 5/24 + Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(0) .plus(Rotation2d.fromDegrees(90)); Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5); } - } + + public interface Driver { + public interface Drive { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.03); + + SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.2); + SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 1.5); + + SmartNumber MAX_TELEOP_SPEED = new SmartNumber("Driver Settings/Drive/Max Speed", Swerve.MAX_MODULE_SPEED.get()); + SmartNumber MAX_TELEOP_ACCEL = new SmartNumber("Driver Settings/Drive/Max Accleration", 15); + } + + public interface Turn { + SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); + + SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.15); + SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); + + SmartNumber MAX_TELEOP_TURNING = new SmartNumber("Driver Settings/Turn/Max Turning", 6.0); + + public interface GyroFeedback { + SmartBoolean GYRO_FEEDBACK_ENABLED = new SmartBoolean("Driver Settings/Gyro Feedback/Enabled", true); + + SmartNumber P = new SmartNumber("Driver Settings/Gyro Feedback/kP", 0.5); + SmartNumber I = new SmartNumber("Driver Settings/Gyro Feedback/kI", 0.0); + SmartNumber D = new SmartNumber("Driver Settings/Gyro Feedback/kD", 0.1); + } + } + + } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java index a9e5977..b186d5c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/SwerveDrive.java @@ -28,10 +28,10 @@ public class SwerveDrive extends SubsystemBase { static { instance = new SwerveDrive( - new SL_SwerveModule(FrontRight.ID, FrontRight.MODULE_OFFSET, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.DRIVE), - new SL_SwerveModule(FrontLeft.ID, FrontLeft.MODULE_OFFSET, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.DRIVE), - new SL_SwerveModule(BackLeft.ID, BackLeft.MODULE_OFFSET, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.DRIVE), - new SL_SwerveModule(BackRight.ID, BackRight.MODULE_OFFSET, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.DRIVE) + new SwerveModuleImpl(FrontRight.ID, FrontRight.MODULE_OFFSET, FrontRight.ABSOLUTE_OFFSET, Ports.Swerve.FrontRight.TURN, Ports.Swerve.FrontRight.DRIVE), + new SwerveModuleImpl(FrontLeft.ID, FrontLeft.MODULE_OFFSET, FrontLeft.ABSOLUTE_OFFSET, Ports.Swerve.FrontLeft.TURN, Ports.Swerve.FrontLeft.DRIVE), + new SwerveModuleImpl(BackLeft.ID, BackLeft.MODULE_OFFSET, BackLeft.ABSOLUTE_OFFSET, Ports.Swerve.BackLeft.TURN, Ports.Swerve.BackLeft.DRIVE), + new SwerveModuleImpl(BackRight.ID, BackRight.MODULE_OFFSET, BackRight.ABSOLUTE_OFFSET, Ports.Swerve.BackRight.TURN, Ports.Swerve.BackRight.DRIVE) ); } @@ -39,12 +39,12 @@ public static SwerveDrive getInstance() { return instance; } - private final SL_SwerveModule[] modules; + private final SwerveModuleImpl[] modules; private final FieldObject2d[] moduleObjects; private final SwerveDriveKinematics kinematics; private final AHRS gyro; - public SwerveDrive(SL_SwerveModule... modules) { + public SwerveDrive(SwerveModuleImpl... modules) { this.modules = modules; kinematics = new SwerveDriveKinematics(getModuleOffsets()); gyro = new AHRS(SPI.Port.kMXP); diff --git a/src/main/java/com/stuypulse/robot/subsystems/SL_SwerveModule.java b/src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java similarity index 82% rename from src/main/java/com/stuypulse/robot/subsystems/SL_SwerveModule.java rename to src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java index 608f7a3..158ac77 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/SL_SwerveModule.java +++ b/src/main/java/com/stuypulse/robot/subsystems/SwerveModuleImpl.java @@ -19,28 +19,29 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -public class SL_SwerveModule extends SwerveModule { +public class SwerveModuleImpl extends SwerveModule { // data private final String id; + private final Translation2d translationOffset; + private final Rotation2d angleOffset; private SwerveModuleState targetState; - private Translation2d translationOffset; - private Rotation2d angleOffset; // turn - private CANSparkMax turnMotor; - private SparkMaxAbsoluteEncoder turnEncoder; + private final CANSparkMax turnMotor; + private final SparkMaxAbsoluteEncoder turnEncoder; // drive - private CANSparkMax driveMotor; - private RelativeEncoder driveEncoder; + private final CANSparkMax driveMotor; + private final RelativeEncoder driveEncoder; // controllers - private Controller driveController; - private AngleController turnController; + private final Controller driveController; + private final AngleController turnController; - public SL_SwerveModule(String id, Translation2d translationOffset, Rotation2d angleOffset, int turnID, int driveID) { + public SwerveModuleImpl(String id, Translation2d translationOffset, Rotation2d angleOffset, int turnID, int driveID) { this.id = id; this.translationOffset = translationOffset; + this.angleOffset = angleOffset; turnMotor = new CANSparkMax(turnID, MotorType.kBrushless); driveMotor = new CANSparkMax(turnID, MotorType.kBrushless);