diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8bafc7b..f9fc442 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,8 +4,6 @@ package frc.robot; -import com.ctre.phoenix6.SignalLogger; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -24,8 +22,6 @@ public void robotInit() { m_robotContainer = new RobotContainer(); m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99); - - SignalLogger.start(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bf31d9..d786c88 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,79 +4,59 @@ package frc.robot; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.led.LEDSubsystem; public class RobotContainer { - final double MaxSpeed = 6; // 6 meters per second desired top speed - final double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity + private class Controller { + public static final CommandController driver = new CommandController(0); + public static final CommandController operator = new CommandController(1); + } - /* Setting up bindings for necessary control of the swerve drive platform */ - CommandController driver = new CommandController(0); // Driver Controller - CommandController operator = new CommandController(1); // Operator Controller - CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain private final LEDSubsystem ledSubsystem; - SwerveRequest.FieldCentric drive = + + // Set up the base for the drive and drivetrain + final DriveSubsystem drivetrain = DriveConstants.DriveTrain; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withIsOpenLoop(true) - .withDeadband(MaxSpeed * 0.1) - .withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric - // driving in open loop - SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); - SwerveRequest.RobotCentric forwardStraight = - new SwerveRequest.RobotCentric().withIsOpenLoop(true); - SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + .withDeadband(DriveConstants.kMaxAngularRate * 0.1) + .withRotationalDeadband(DriveConstants.kMaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /* Path follower */ - // Command runAuto = drivetrain.getAutoPath("Tests"); - - Telemetry logger = new Telemetry(MaxSpeed); + private Command runAuto = drivetrain.getAutoPath("Tests"); - Pose2d odomStart = new Pose2d(0, 0, new Rotation2d(0, 0)); + private final Telemetry logger = new Telemetry(DriveConstants.kMaxSpeed); private void configureBindings() { ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem)); + drivetrain.registerTelemetry(logger::telemeterize); - drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically + drivetrain.setDefaultCommand( drivetrain .applyRequest( () -> drive - .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with + .withVelocityX( + -Controller.driver.getLeftY() + * DriveConstants.kMaxSpeed) // Drive forward with // negative Y (forward) .withVelocityY( - -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left) + -Controller.driver.getLeftX() + * DriveConstants.kMaxSpeed) // Drive left with negative X (left) .withRotationalRate( - -driver.getRightX() - * MaxAngularRate) // Drive counterclockwise with negative X (left) + -Controller.driver.getRightX() + * DriveConstants + .kMaxAngularRate) // Drive counterclockwise with negative X + // (left) ) .ignoringDisable(true)); - - driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake)); - driver - .yellowButton() - .whileTrue( - drivetrain.applyRequest( - () -> - point.withModuleDirection( - new Rotation2d(-driver.getLeftY(), -driver.getLeftX())))); - - // if (Utils.isSimulation()) { - // drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90))); - // } - drivetrain.registerTelemetry(logger::telemeterize); - driver.POVUp() - .whileTrue( - drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0))); - driver.POVDown() - .whileTrue( - drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0))); } public RobotContainer() { @@ -85,11 +65,6 @@ public RobotContainer() { } public Command getAutonomousCommand() { - /* First put the drivetrain into auto run mode, then run the auto */ - return new InstantCommand(() -> {}); - } - - public boolean seedPoseButtonDown() { - return driver.leftBumper().getAsBoolean(); + return runAuto; } } diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java index 16e9089..bb0861a 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -1,5 +1,6 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState; @@ -10,8 +11,6 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StringPublisher; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,8 +19,6 @@ public class Telemetry { private final double MaxSpeed; - private int logEntry; - private int odomEntry; /** * Construct a telemetry object, with the specified max speed of the robot @@ -30,36 +27,36 @@ public class Telemetry { */ public Telemetry(double maxSpeed) { MaxSpeed = maxSpeed; - logEntry = DataLogManager.getLog().start("odometry", "double[]"); - odomEntry = DataLogManager.getLog().start("odom period", "double"); + SignalLogger.start(); } /* What to publish over networktables for telemetry */ - NetworkTableInstance inst = NetworkTableInstance.getDefault(); + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); /* Robot pose for field positioning */ - NetworkTable table = inst.getTable("Pose"); - DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); /* Robot speeds for general checking */ - NetworkTable driveStats = inst.getTable("Drive"); - DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); - DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); - DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); - DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish(); + private final NetworkTable driveStats = inst.getTable("Drive"); + private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish(); + private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish(); + private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish(); + private final DoublePublisher odomFreq = + driveStats.getDoubleTopic("Odometry Frequency").publish(); /* Keep a reference of the last pose to calculate the speeds */ - Pose2d m_lastPose = new Pose2d(); - double lastTime = Utils.getCurrentTimeSeconds(); + private Pose2d m_lastPose = new Pose2d(); + private double lastTime = Utils.getCurrentTimeSeconds(); /* Mechanisms to represent the swerve module states */ - Mechanism2d[] m_moduleMechanisms = + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), }; /* A direction and length changing ligament for speed representation */ - MechanismLigament2d[] m_moduleSpeeds = + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { m_moduleMechanisms[0] .getRoot("RootSpeed", 0.5, 0.5) @@ -75,7 +72,7 @@ public Telemetry(double maxSpeed) { .append(new MechanismLigament2d("Speed", 0.5, 0)), }; /* A direction changing and length constant ligament for module direction */ - MechanismLigament2d[] m_moduleDirections = + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { m_moduleMechanisms[0] .getRoot("RootDirection", 0.5, 0.5) @@ -110,7 +107,7 @@ public void telemeterize(SwerveDriveState state) { speed.set(velocities.getNorm()); velocityX.set(velocities.getX()); velocityY.set(velocities.getY()); - odomPeriod.set(1.0 / state.OdometryPeriod); + odomFreq.set(1.0 / state.OdometryPeriod); /* Telemeterize the module's states */ for (int i = 0; i < 4; ++i) { @@ -121,12 +118,8 @@ public void telemeterize(SwerveDriveState state) { SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } - DataLogManager.getLog() - .appendDoubleArray( - logEntry, - new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}, - (long) (Timer.getFPGATimestamp() * 1000000)); - DataLogManager.getLog() - .appendDouble(odomEntry, state.OdometryPeriod, (long) (Timer.getFPGATimestamp() * 1000000)); + SignalLogger.writeDoubleArray( + "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()}); + SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds"); } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java deleted file mode 100644 index a66c7d8..0000000 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ /dev/null @@ -1,133 +0,0 @@ -package frc.robot.generated; - -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SwerveModuleSteerFeedbackType; -import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; - -import edu.wpi.first.math.util.Units; -import frc.robot.CommandSwerveDrivetrain; - -public class TunerConstants { - static class CustomSlotGains extends Slot0Configs { - public CustomSlotGains(double kP, double kI, double kD, double kV, double kS) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kV = kV; - this.kS = kS; - } - } - - private static final CustomSlotGains steerGains = new CustomSlotGains(100, 0, 0.05, 0, 0); - private static final CustomSlotGains driveGains = new CustomSlotGains(3, 0, 0, 0, 0); - - private static final double kCoupleRatio = 3.5; - - private static final double kDriveGearRatio = 7.363636364; - private static final double kSteerGearRatio = 15.42857143; - private static final double kWheelRadiusInches = - 2.167; // Estimated at first, then fudge-factored to make odom match record - private static final int kPigeonId = 1; - private static final boolean kSteerMotorReversed = true; - private static final String kCANbusName = "rio"; - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static double kSteerInertia = 0.00001; - private static double kDriveInertia = 0.001; - - private static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withPigeon2Id(kPigeonId) - .withCANbusName(kCANbusName) - .withSupportsPro(true); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withWheelRadius(kWheelRadiusInches) - .withSlipCurrent(30) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSpeedAt12VoltsMps( - 6) // Theoretical free speed is 10 meters per second at 12v applied output - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withFeedbackSource(SwerveModuleSteerFeedbackType.FusedCANcoder) - .withCouplingGearRatio( - kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns - .withSteerMotorInverted(kSteerMotorReversed); - - private static final int kFrontLeftDriveMotorId = 5; - private static final int kFrontLeftSteerMotorId = 4; - private static final int kFrontLeftEncoderId = 2; - private static final double kFrontLeftEncoderOffset = -0.83544921875; - - private static final double kFrontLeftXPosInches = 10.5; - private static final double kFrontLeftYPosInches = 10.5; - private static final int kFrontRightDriveMotorId = 7; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 3; - private static final double kFrontRightEncoderOffset = -0.15234375; - - private static final double kFrontRightXPosInches = 10.5; - private static final double kFrontRightYPosInches = -10.5; - private static final int kBackLeftDriveMotorId = 1; - private static final int kBackLeftSteerMotorId = 0; - private static final int kBackLeftEncoderId = 0; - private static final double kBackLeftEncoderOffset = -0.4794921875; - - private static final double kBackLeftXPosInches = -10.5; - private static final double kBackLeftYPosInches = 10.5; - private static final int kBackRightDriveMotorId = 3; - private static final int kBackRightSteerMotorId = 2; - private static final int kBackRightEncoderId = 1; - private static final double kBackRightEncoderOffset = -0.84130859375; - - private static final double kBackRightXPosInches = -10.5; - private static final double kBackRightYPosInches = -10.5; - - private static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - Units.inchesToMeters(kFrontLeftXPosInches), - Units.inchesToMeters(kFrontLeftYPosInches), - kInvertLeftSide); - private static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - Units.inchesToMeters(kFrontRightXPosInches), - Units.inchesToMeters(kFrontRightYPosInches), - kInvertRightSide); - private static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - Units.inchesToMeters(kBackLeftXPosInches), - Units.inchesToMeters(kBackLeftYPosInches), - kInvertLeftSide); - private static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - Units.inchesToMeters(kBackRightXPosInches), - Units.inchesToMeters(kBackRightYPosInches), - kInvertRightSide); - - public static final CommandSwerveDrivetrain DriveTrain = - new CommandSwerveDrivetrain( - DrivetrainConstants, 250, FrontLeft, FrontRight, BackLeft, BackRight); -} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java new file mode 100644 index 0000000..f0c8855 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -0,0 +1,142 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory; +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType; + +import frc.robot.subsystems.drive.swerve.SwerveModuleLocation; +import frc.robot.subsystems.drive.swerve.SwerveModules; + +public class DriveConstants { + /** Only touch these if you know what you're doing */ + private static final DriveSlotGains steerGains = new DriveSlotGains(100.0, 0, 0.05, 0, 0); + + private static final DriveSlotGains driveGains = new DriveSlotGains(3.0, 0, 0, 0, 0); + + public static final double kMaxSpeed = 6.0; // 6 meters per second desired top speed + public static final double kMaxAngularRate = + Math.PI; // Half a rotation per second max angular velocity + + private static final double kCoupleRatio = 3.5; + + private static final double kWheelRadiusInches = + 2.167; // Estimated at first, then fudge-factored to make odom + // match record + + private static final String kCANbusName = "rio"; + private static final int kPigeonId = 1; + + public static final double kSpeedAt12VoltsMps = 5.0; + public static final double kSimLoopPeriod = 0.005; // 5 ms + + private static final boolean kSteerMotorReversed = true; + public static final boolean kInvertLeftSide = false; + public static final boolean kInvertRightSide = true; + + /** The ratios of the drive and steer gears in the swerve modules */ + private class GearRatio { + private static final double kDriveGearRatio = 7.363636364; + private static final double kSteerGearRatio = 15.42857143; + } + + /** + * The simulated voltage necessary to overcome friction for swerve. This is only used for + * simulation + */ + private class FrictionVoltage { + private static final double kSteerFrictionVoltage = 0.25; + private static final double kDriveFrictionVoltage = 0.25; + } + + /** + * The inertia of the drive and steer motors in the swerve modules. These should be left to + * default + */ + private class Intertia { + private static final double kSteerInertia = 0.00001; + private static final double kDriveInertia = 0.001; + } + + /** + * The CAN IDs of the drive motors in the swerve modules NOTE: Be sure to change these to the + * correct values at the start of the season + */ + public class DriveMotorId { + public static final int kFrontLeftDriveMotorId = 5; + public static final int kFrontRightDriveMotorId = 7; + public static final int kBackLeftDriveMotorId = 1; + public static final int kBackRightDriveMotorId = 3; + } + + /** + * The CAN IDs of the steer motors in the swerve modules NOTE: Be sure to change these to the + * correct values at the start of the season + */ + public class SteerMotorId { + public static final int kFrontLeftSteerMotorId = 4; + public static final int kFrontRightSteerMotorId = 6; + public static final int kBackLeftSteerMotorId = 0; + public static final int kBackRightSteerMotorId = 2; + } + + /** + * The CAN IDs of the encoders in the swerve modules NOTE: Be sure to change these to the correct + * values at the start of the season + */ + public class EncoderId { + public static final int kFrontLeftEncoderId = 2; + public static final int kFrontRightEncoderId = 3; + public static final int kBackLeftEncoderId = 0; + public static final int kBackRightEncoderId = 1; + } + + public class EncoderOffset { + public static final double kFrontLeftEncoderOffset = -0.83544921875; + public static final double kFrontRightEncoderOffset = -0.15234375; + public static final double kBackLeftEncoderOffset = -0.4794921875; + public static final double kBackRightEncoderOffset = -0.84130859375; + } + + /** + * The X and Y positions of the swerve modules relative to the center of the robot in inches NOTE: + * Be sure to change these to the correct values at the start of the season + */ + public class SwerveModulePosition { + public static final SwerveModuleLocation kFrontLeft = new SwerveModuleLocation(10.5, 10.5); + public static final SwerveModuleLocation kFrontRight = new SwerveModuleLocation(10.5, -10.5); + public static final SwerveModuleLocation kBackLeft = new SwerveModuleLocation(-10.5, 10.5); + public static final SwerveModuleLocation kBackRight = new SwerveModuleLocation(-10.5, -10.5); + } + + private static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants().withPigeon2Id(kPigeonId).withCANbusName(kCANbusName); + + public static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(GearRatio.kDriveGearRatio) + .withSteerMotorGearRatio(GearRatio.kSteerGearRatio) + .withWheelRadius(kWheelRadiusInches) + .withSlipCurrent(30) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSpeedAt12VoltsMps( + kSpeedAt12VoltsMps) // Theoretical free speed is 10 meters per second at 12v applied + // output + .withSteerInertia(Intertia.kSteerInertia) + .withDriveInertia(Intertia.kDriveInertia) + .withFeedbackSource(SteerFeedbackType.FusedCANcoder) + .withSteerFrictionVoltage(FrictionVoltage.kSteerFrictionVoltage) + .withDriveFrictionVoltage(FrictionVoltage.kDriveFrictionVoltage) + .withCouplingGearRatio( + kCoupleRatio) // Every 1 rotation of the azimuth results in couple ratio drive turns + .withSteerMotorInverted(kSteerMotorReversed); + + /** Construct the final drivetrain object that we interact with */ + public static final DriveSubsystem DriveTrain = + new DriveSubsystem( + DrivetrainConstants, + SwerveModules.FrontLeft, + SwerveModules.FrontRight, + SwerveModules.BackLeft, + SwerveModules.BackRight); +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java b/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java new file mode 100644 index 0000000..2879a83 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/DriveSlotGains.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.drive; + +import com.ctre.phoenix6.configs.Slot0Configs; + +public class DriveSlotGains extends Slot0Configs { + public DriveSlotGains(double kP, double kI, double kD, double kV, double kS) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.kV = kV; + this.kS = kS; + } +} diff --git a/src/main/java/frc/robot/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java similarity index 51% rename from src/main/java/frc/robot/CommandSwerveDrivetrain.java rename to src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 5211b54..33e576e 100644 --- a/src/main/java/frc/robot/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -1,7 +1,8 @@ -package frc.robot; +package frc.robot.subsystems.drive; import java.util.function.Supplier; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain; import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; @@ -15,32 +16,33 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.Subsystem; -/** - * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem so it can be used - * in command-based projects easily. - */ -public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem { - private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); +public class DriveSubsystem extends SwerveDrivetrain implements Subsystem { + private Notifier m_simNotifier = null; + private double m_lastSimTime; - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants driveTrainConstants, - double OdometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(driveTrainConstants, OdometryUpdateFrequency, modules); - configurePathPlanner(); - } + private SwerveRequest.ApplyChassisSpeeds autoRequest = new SwerveRequest.ApplyChassisSpeeds(); - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { - super(driveTrainConstants, modules); - configurePathPlanner(); + @Override + public void simulationPeriodic() { + updateSimState(0.02, 12); } + /** + * Configures path planner to have a holonomic path follower so it can move in any direction. This + * is meant for holonomic drivetrains AND swerve + */ private void configurePathPlanner() { + double driveBaseRadius = 0; + for (var moduleLocation : m_moduleLocations) { + driveBaseRadius = Math.max(driveBaseRadius, moduleLocation.getNorm()); + } + AutoBuilder.configureHolonomic( () -> this.getState().Pose, // Supplier of current robot pose this::seedFieldRelative, // Consumer for seeding pose against auto @@ -51,13 +53,18 @@ private void configurePathPlanner() { new HolonomicPathFollowerConfig( new PIDConstants(10, 0, 0), new PIDConstants(10, 0, 0), - 1, - 1, - new ReplanningConfig(), - 0.004), - this); // Subsystem for requirements + DriveConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + new ReplanningConfig()), + this); } + /** + * Applies and runs a command on the swerve drivetrain + * + * @param requestSupplier The supplier of the swerve request to apply (the request) + * @return A command which runs the specified request + */ public Command applyRequest(Supplier requestSupplier) { return new RunCommand( () -> { @@ -66,16 +73,16 @@ public Command applyRequest(Supplier requestSupplier) { this); } + /** + * Returns a command that will run the specified pathplanner path + * + * @param pathName The name of the path created in pathplanner + * @return A command which runs the specified path + */ public Command getAutoPath(String pathName) { return new PathPlannerAuto(pathName); } - @Override - public void simulationPeriodic() { - /* Assume */ - updateSimState(0.02, 12); - } - /** * Takes the specified location and makes it the current pose for field-relative maneuvers * @@ -93,7 +100,64 @@ public void seedFieldRelative(Pose2d location) { } } + /** + * Returns the current robot chassis speeds + * + * @return The current robot chassis speeds as a Twist2d object + */ public ChassisSpeeds getCurrentRobotChassisSpeeds() { return m_kinematics.toChassisSpeeds(getState().ModuleStates); } + + /** + * Updates the state of the drivetrain for simulation + * + * @param deltaTime The time since the last update + * @param batteryVoltage The current battery voltage + */ + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; + + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(DriveConstants.kSimLoopPeriod); + } + + /** Add any methods you want to call when the drive subsystem is initialized and called */ + public void initialize() { + configurePathPlanner(); + } + + /** Initialize the drive subsystem when we create an instance of it and configure it * */ + public DriveSubsystem( + SwerveDrivetrainConstants driveTrainConstants, + double OdometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(driveTrainConstants, OdometryUpdateFrequency, modules); + this.initialize(); + + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** Initialize the drive subsystem when we create an instance of it and configure it * */ + public DriveSubsystem( + SwerveDrivetrainConstants driveTrainConstants, SwerveModuleConstants... modules) { + super(driveTrainConstants, modules); + this.initialize(); + + if (Utils.isSimulation()) { + startSimThread(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModuleLocation.java b/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModuleLocation.java new file mode 100644 index 0000000..cd689e7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModuleLocation.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.drive.swerve; + +import edu.wpi.first.math.util.Units; + +public class SwerveModuleLocation { + private double xPosition, yPosition; + + /** + * Stores the x and y position of a swerve module + * + * @param xPosition The X position of the module relative to the center of the robot + * @param yPosition The Y position of the module relative to the center of the robot + */ + public SwerveModuleLocation(double xPosition, double yPosition) { + this.xPosition = xPosition; + this.yPosition = yPosition; + } + + /** + * Returns the X position of the module relative to the center of the robot in inches + * + * @return The raw X position of the module in inches + */ + public double getXPositionRaw() { + return this.xPosition; + } + + /** + * Returns the Y position of the module relative to the center of the robot in inches + * + * @return The raw Y position of the module in inches + */ + public double getYPositionRaw() { + return this.yPosition; + } + + /** + * Returns the X position of the module relative to the center of the robot in meters + * + * @return The calculated X position of the module in meters + */ + public double getXPositionCalculated() { + return Units.inchesToMeters(this.xPosition); + } + + /** + * Returns the Y position of the module relative to the center of the robot in meters + * + * @return The calculated Y position of the module in meters + */ + public double getYPositionCalculated() { + return Units.inchesToMeters(this.yPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModules.java b/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModules.java new file mode 100644 index 0000000..9afff70 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/swerve/SwerveModules.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.drive.swerve; + +import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants; + +import frc.robot.subsystems.drive.DriveConstants; +import frc.robot.subsystems.drive.DriveConstants.DriveMotorId; +import frc.robot.subsystems.drive.DriveConstants.EncoderId; +import frc.robot.subsystems.drive.DriveConstants.EncoderOffset; +import frc.robot.subsystems.drive.DriveConstants.SteerMotorId; +import frc.robot.subsystems.drive.DriveConstants.SwerveModulePosition; + +/** This class houses the constants and assembled swerve module configurations for the robot */ +public class SwerveModules { + public static final SwerveModuleConstants FrontLeft = + DriveConstants.ConstantCreator.createModuleConstants( + SteerMotorId.kFrontLeftSteerMotorId, + DriveMotorId.kFrontLeftDriveMotorId, + EncoderId.kFrontLeftEncoderId, + EncoderOffset.kFrontLeftEncoderOffset, + SwerveModulePosition.kFrontLeft.getXPositionCalculated(), + SwerveModulePosition.kFrontLeft.getXPositionCalculated(), + DriveConstants.kInvertLeftSide); + + public static final SwerveModuleConstants FrontRight = + DriveConstants.ConstantCreator.createModuleConstants( + SteerMotorId.kFrontRightSteerMotorId, + DriveMotorId.kFrontRightDriveMotorId, + EncoderId.kFrontRightEncoderId, + EncoderOffset.kFrontRightEncoderOffset, + SwerveModulePosition.kFrontRight.getXPositionCalculated(), + SwerveModulePosition.kFrontRight.getXPositionCalculated(), + DriveConstants.kInvertRightSide); + + public static final SwerveModuleConstants BackLeft = + DriveConstants.ConstantCreator.createModuleConstants( + SteerMotorId.kBackLeftSteerMotorId, + DriveMotorId.kBackLeftDriveMotorId, + EncoderId.kBackLeftEncoderId, + EncoderOffset.kBackLeftEncoderOffset, + SwerveModulePosition.kBackLeft.getXPositionCalculated(), + SwerveModulePosition.kBackLeft.getXPositionCalculated(), + DriveConstants.kInvertLeftSide); + + public static final SwerveModuleConstants BackRight = + DriveConstants.ConstantCreator.createModuleConstants( + SteerMotorId.kBackRightSteerMotorId, + DriveMotorId.kBackRightDriveMotorId, + EncoderId.kBackRightEncoderId, + EncoderOffset.kBackRightEncoderOffset, + SwerveModulePosition.kBackRight.getXPositionCalculated(), + SwerveModulePosition.kBackRight.getXPositionCalculated(), + DriveConstants.kInvertRightSide); +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index aeaa3ba..a92663b 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.0.0-beta-1", + "version": "2024.0.0-beta-6.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-1" + "version": "2024.0.0-beta-6.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-1", + "version": "2024.0.0-beta-6.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 1f9f441..622e9c7 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-1" + "version": "24.0.0-beta-7" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-1", + "version": "24.0.0-beta-7", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,