diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..f0a712e --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,58 @@ +{ + "NetworkTables": { + "transitory": { + "Robot": { + "drive": { + "frontRight": { + "SwerveModulePosition##v_/Robot/drive/frontRight/position": { + "Rotation2d##v_angle": { + "open": true + }, + "open": true + }, + "SwerveModuleState##v_/Robot/drive/frontRight/state": { + "Rotation2d##v_angle": { + "open": true + }, + "open": true + } + }, + "open": true, + "rearRight": { + "SwerveModulePosition##v_/Robot/drive/rearRight/position": { + "Rotation2d##v_angle": { + "open": true + }, + "open": true + }, + "module": { + "open": true + }, + "open": true + } + }, + "open": true + } + }, + "types": { + "/FMSInfo": "FMSInfo", + "/Robot/autos": "String Chooser", + "/Robot/drive/field2d": "Field2d", + "/Robot/drive/frontLeft/driveFeedback": "PIDController", + "/Robot/drive/frontLeft/turnFeedback": "PIDController", + "/Robot/drive/frontRight/driveFeedback": "PIDController", + "/Robot/drive/frontRight/turnFeedback": "PIDController", + "/Robot/drive/rearLeft/driveFeedback": "PIDController", + "/Robot/drive/rearLeft/turnFeedback": "PIDController", + "/Robot/drive/rearRight/driveFeedback": "PIDController", + "/Robot/drive/rearRight/turnFeedback": "PIDController", + "/SmartDashboard/drive quasistatic forward": "Command" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)" + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.vscode/launch.json b/.vscode/launch.json index e5e165b..4c42c95 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,27 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "java", + "name": "Main", + "request": "launch", + "mainClass": "org.sciborgs1155.robot.Main", + "projectName": "Hydrogen" + }, + { + "type": "java", + "name": "FlexModule", + "request": "launch", + "mainClass": "org.sciborgs1155.robot.drive.FlexModule", + "projectName": "Hydrogen" + }, + { + "type": "java", + "name": "TalonSwerveModule", + "request": "launch", + "mainClass": "org.sciborgs1155.robot.drive.TalonSwerveModule", + "projectName": "Hydrogen" + }, { "type": "java", "name": "FailureManagement", diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..a945a65 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,10 +1,25 @@ { + "FMS": { + "window": { + "visible": false + } + }, + "Joysticks": { + "window": { + "visible": false + } + }, + "System Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 65, - "incKey": 68 + "decKey": 68, + "incKey": 65 }, { "decKey": 87, @@ -15,9 +30,14 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 76, + "incKey": 74 } ], - "axisCount": 3, + "axisCount": 6, "buttonCount": 4, "buttonKeys": [ 90, @@ -41,16 +61,13 @@ }, { "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, + {}, { "decKey": 73, "incKey": 75 } ], - "axisCount": 2, + "axisCount": 4, "buttonCount": 4, "buttonKeys": [ 77, @@ -88,5 +105,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + {}, + { + "guid": "Keyboard0" + } ] } diff --git a/simgui.json b/simgui.json index 277f295..05b9908 100644 --- a/simgui.json +++ b/simgui.json @@ -1,31 +1,65 @@ { + "HALProvider": { + "Other Devices": { + "window": { + "visible": false + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/Robot/autos": "String Chooser", - "/SmartDashboard/PIDController[1]": "PIDController" + "/Robot/drive/field2d": "Field2d", + "/Robot/drive/frontLeft/driveFeedback": "PIDController", + "/Robot/drive/frontLeft/turnFeedback": "PIDController", + "/Robot/drive/frontRight/driveFeedback": "PIDController", + "/Robot/drive/frontRight/turnFeedback": "PIDController", + "/Robot/drive/rearLeft/driveFeedback": "PIDController", + "/Robot/drive/rearLeft/turnFeedback": "PIDController", + "/Robot/drive/rearRight/driveFeedback": "PIDController", + "/Robot/drive/rearRight/turnFeedback": "PIDController", + "/SmartDashboard/drive quasistatic forward": "Command" + }, + "windows": { + "/Robot/drive/field2d": { + "window": { + "visible": true + } + }, + "/SmartDashboard/drive quasistatic forward": { + "window": { + "visible": true + } + } } }, "NetworkTables": { "transitory": { "Robot": { - "open": true - }, - "Shuffleboard": { - "open": true - }, - "SmartDashboard": { - "Failures": { - "open": true, - "string[]##v_/SmartDashboard/Failures/warnings": { + "drive": { + "frontRight": { + "SwerveModulePosition##v_/Robot/drive/frontRight/position": { + "Rotation2d##v_angle": { + "open": true + }, + "open": true + }, + "SwerveModuleState##v_/Robot/drive/frontRight/state": { + "Rotation2d##v_angle": { + "open": true + }, + "open": true + }, + "driveFeedback": { + "open": true + }, "open": true - } + }, + "open": true }, "open": true } } - }, - "NetworkTables Info": { - "visible": true } } diff --git a/src/main/java/org/sciborgs1155/lib/SparkUtils.java b/src/main/java/org/sciborgs1155/lib/SparkUtils.java index 841d8f8..af845d6 100644 --- a/src/main/java/org/sciborgs1155/lib/SparkUtils.java +++ b/src/main/java/org/sciborgs1155/lib/SparkUtils.java @@ -4,7 +4,6 @@ import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; -import com.revrobotics.SparkPIDController; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Time; import edu.wpi.first.units.Units; @@ -109,19 +108,4 @@ public static void configureFrameStrategy( public static void configureFollowerFrameStrategy(CANSparkBase spark) { configureFrameStrategy(spark, Set.of(), Set.of(), false); } - - /** - * Enables and sets the minimum and maximum bounds for input wrapping on an onboard Spark Max PID - * controller. - * - * @param controller The onboard PID controller object. - * @param min The minimum position input. - * @param max The maximum position input. - */ - public static void enableContinuousPIDInput( - SparkPIDController controller, double min, double max) { - controller.setPositionPIDWrappingEnabled(true); - controller.setPositionPIDWrappingMinInput(min); - controller.setPositionPIDWrappingMaxInput(max); - } } diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java index 3bcb512..fee0217 100644 --- a/src/main/java/org/sciborgs1155/robot/Ports.java +++ b/src/main/java/org/sciborgs1155/robot/Ports.java @@ -5,4 +5,16 @@ public static final class OI { public static final int OPERATOR = 0; public static final int DRIVER = 1; } + + public static final class Drive { + public static final int FRONT_LEFT_DRIVE = 11; + public static final int REAR_LEFT_DRIVE = 10; + public static final int FRONT_RIGHT_DRIVE = 12; + public static final int REAR_RIGHT_DRIVE = 13; + + public static final int FRONT_LEFT_TURNING = 15; + public static final int REAR_LEFT_TURNING = 14; + public static final int FRONT_RIGHT_TURNING = 16; + public static final int REAR_RIGHT_TURNING = 17; + } } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index e62de13..46d0121 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -10,10 +10,12 @@ import monologue.Annotations.Log; import monologue.Logged; import monologue.Monologue; +import org.littletonrobotics.urcl.URCL; import org.sciborgs1155.lib.CommandRobot; import org.sciborgs1155.lib.FaultLogger; import org.sciborgs1155.robot.Ports.OI; import org.sciborgs1155.robot.commands.Autos; +import org.sciborgs1155.robot.drive.Drive; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -28,9 +30,10 @@ public class Robot extends CommandRobot implements Logged { private final CommandXboxController driver = new CommandXboxController(OI.DRIVER); // SUBSYSTEMS + private final Drive drive = Drive.create(); // COMMANDS - @Log.NT Autos autos = new Autos(); + @Log.NT private final Autos autos = new Autos(); /** The robot contains subsystems, OI devices, and commands. */ public Robot() { @@ -41,23 +44,28 @@ public Robot() { /** Configures basic behavior during different parts of the game. */ private void configureGameBehavior() { - if (isSimulation()) { - DriverStation.silenceJoystickConnectionWarning(true); - } - - // Configure logging with DataLogManager, Monologue, and FailureManagement + // Configure logging with DataLogManager, Monologue, FailureManagement, and URCL DataLogManager.start(); Monologue.setupMonologue(this, "/Robot", false, true); addPeriodic(Monologue::updateAll, kDefaultPeriod); FaultLogger.setupLogging(); addPeriodic(FaultLogger::update, 1); + + if (isReal()) { + URCL.start(); + } else { + DriverStation.silenceJoystickConnectionWarning(true); + } } /** * Configures subsystem default commands. Default commands are scheduled when no other command is * running on a subsystem. */ - private void configureSubsystemDefaults() {} + private void configureSubsystemDefaults() { + drive.setDefaultCommand( + drive.drive(() -> -driver.getLeftX(), () -> -driver.getLeftY(), () -> -driver.getRightX())); + } /** Configures trigger -> command bindings */ private void configureBindings() { diff --git a/src/main/java/org/sciborgs1155/robot/drive/Drive.java b/src/main/java/org/sciborgs1155/robot/drive/Drive.java new file mode 100644 index 0000000..2faf233 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/Drive.java @@ -0,0 +1,370 @@ +package org.sciborgs1155.robot.drive; + +import static edu.wpi.first.units.Units.*; +import static org.sciborgs1155.robot.Ports.Drive.*; +import static org.sciborgs1155.robot.drive.DriveConstants.*; + +import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import java.util.List; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import monologue.Annotations.IgnoreLogged; +import monologue.Annotations.Log; +import monologue.Logged; +import org.photonvision.EstimatedRobotPose; +import org.sciborgs1155.robot.Constants; +import org.sciborgs1155.robot.Robot; +import org.sciborgs1155.robot.drive.DriveConstants.Rotation; + +public class Drive extends SubsystemBase implements Logged, AutoCloseable { + + // Modules + private final SwerveModule frontLeft; + private final SwerveModule frontRight; + private final SwerveModule rearLeft; + private final SwerveModule rearRight; + + @IgnoreLogged private final List modules; + + private final AHRS imu = new AHRS(); + + public final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(MODULE_OFFSET); + + // Odometry and pose estimation + private final SwerveDrivePoseEstimator odometry; + + @Log.NT private final Field2d field2d = new Field2d(); + private final FieldObject2d[] modules2d; + + // Rate limiting + private final SlewRateLimiter xLimiter = + new SlewRateLimiter(MAX_ACCEL.in(MetersPerSecondPerSecond)); + private final SlewRateLimiter yLimiter = + new SlewRateLimiter(MAX_ACCEL.in(MetersPerSecondPerSecond)); + + @Log.NT private double speedMultiplier = 1; + + // SysId + private final SysIdRoutine driveRoutine; + private final SysIdRoutine turnRoutine; + + /** + * A factory to create a new drive subsystem based on whether the robot is being ran in simulation + * or not. + */ + public static Drive create() { + return Robot.isReal() + ? new Drive( + new FlexModule(FRONT_LEFT_DRIVE, FRONT_LEFT_TURNING, ANGULAR_OFFSETS.get(0)), + new FlexModule(FRONT_RIGHT_DRIVE, FRONT_RIGHT_TURNING, ANGULAR_OFFSETS.get(1)), + new FlexModule(REAR_LEFT_DRIVE, REAR_LEFT_TURNING, ANGULAR_OFFSETS.get(2)), + new FlexModule(REAR_RIGHT_DRIVE, REAR_RIGHT_TURNING, ANGULAR_OFFSETS.get(3))) + : new Drive(new SimModule(), new SimModule(), new SimModule(), new SimModule()); + } + + /** A swerve drive subsystem containing four {@link ModuleIO} modules. */ + public Drive(ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleIO rearRight) { + this.frontLeft = new SwerveModule(frontLeft, ANGULAR_OFFSETS.get(0), " FL"); + this.frontRight = new SwerveModule(frontRight, ANGULAR_OFFSETS.get(1), "FR"); + this.rearLeft = new SwerveModule(rearLeft, ANGULAR_OFFSETS.get(2), "RL"); + this.rearRight = new SwerveModule(rearRight, ANGULAR_OFFSETS.get(3), " RR"); + + modules = List.of(this.frontLeft, this.frontRight, this.rearLeft, this.rearRight); + modules2d = new FieldObject2d[modules.size()]; + + driveRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + volts -> modules.forEach(m -> m.setDriveVoltage(volts.in(Volts))), null, this)); + + turnRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + volts -> modules.forEach(m -> m.setTurnVoltage(volts.in(Volts))), null, this)); + + odometry = + new SwerveDrivePoseEstimator(kinematics, getHeading(), getModulePositions(), new Pose2d()); + + for (int i = 0; i < modules.size(); i++) { + var module = modules.get(i); + modules2d[i] = field2d.getObject("module-" + module.name); + } + + SmartDashboard.putData("drive quasistatic forward", driveSysIdQuasistatic(Direction.kForward)); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + @Log.NT + public Pose2d getPose() { + return odometry.getEstimatedPosition(); + } + + /** + * Returns the heading of the robot, based on our pigeon. + * + * @return A Rotation2d of our angle. + */ + public Rotation2d getHeading() { + return Robot.isReal() ? imu.getRotation2d() : Rotation2d.fromRadians(simulatedHeading); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + odometry.resetPosition(getHeading(), getModulePositions(), pose); + } + + /** Deadbands and squares inputs */ + private static double scale(double input) { + input = MathUtil.applyDeadband(input, Constants.DEADBAND); + return Math.copySign(input * input, input); + } + + /** + * Drives the robot based on a {@link DoubleSupplier} for field relative x y and omega speeds. + * + * @param vx A supplier for the speed of the robot (-1 to 1) along the x axis (perpendicular to + * the alliance side). + * @param vy A supplier for the speed of the robot (-1 to 1) along the y axis (parallel to the + * alliance side). + * @param vOmega A supplier for the angular speed of the robot (-1 to 1). + * @return The driving command. + */ + public Command drive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vOmega) { + return run( + () -> + driveFieldRelative( + new ChassisSpeeds( + xLimiter.calculate( + MAX_SPEED + .times(scale(vx.getAsDouble())) + .times(speedMultiplier) + .in(MetersPerSecond)), + yLimiter.calculate( + MAX_SPEED + .times(scale(vy.getAsDouble())) + .times(speedMultiplier) + .in(MetersPerSecond)), + MAX_ANGULAR_SPEED + .times(scale(vOmega.getAsDouble())) + .times(speedMultiplier) + .in(RadiansPerSecond)))); + } + + /** + * Drives the robot based on a {@link DoubleSupplier} for field relative x y speeds and an absolute heading. + * + * @param vx A supplier for the speed of the robot (-1 to 1) along the x axis (perpendicular to + * the alliance side). + * @param vy A supplier for the speed of the robot (-1 to 1) along the y axis (parallel to the + * alliance side). + * @param heading A supplier for the field relative heading of the robot. + * @return The driving command. + */ + public Command drive(DoubleSupplier vx, DoubleSupplier vy, Supplier heading) { + var pid = + new ProfiledPIDController( + Rotation.P, + Rotation.I, + Rotation.D, + new TrapezoidProfile.Constraints(MAX_ANGULAR_SPEED, MAX_ANGULAR_ACCEL)); + + return run( + () -> + driveFieldRelative( + ChassisSpeeds.fromFieldRelativeSpeeds( + xLimiter.calculate( + MAX_SPEED + .times(scale(vx.getAsDouble())) + .times(speedMultiplier) + .in(MetersPerSecond)), + yLimiter.calculate( + MAX_SPEED + .times(scale(vy.getAsDouble())) + .times(speedMultiplier) + .in(MetersPerSecond)), + pid.calculate(getHeading().getRadians(), heading.get().getRadians()), + getHeading()))); + } + + /** + * Drives the robot relative to field based on provided {@link ChassisSpeeds} and current heading. + * + * @param speeds The desired field relative chassis speeds. + */ + public void driveFieldRelative(ChassisSpeeds speeds) { + driveRobotRelative(ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getPose().getRotation())); + } + + /** + * Drives the robot based on profided {@link ChassisSpeeds}. + * + *

This method uses {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} to reduce skew. + * + * @param speeds The desired robot relative chassis speeds. + */ + public void driveRobotRelative(ChassisSpeeds speeds) { + setModuleStates( + kinematics.toSwerveModuleStates( + ChassisSpeeds.discretize(speeds, Constants.PERIOD.in(Seconds)))); + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired SwerveModule states. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + if (desiredStates.length != modules.size()) { + throw new IllegalArgumentException("desiredStates must have the same length as modules"); + } + + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, MAX_SPEED.in(MetersPerSecond)); + + for (int i = 0; i < modules.size(); i++) { + modules.get(i).updateDesiredState(desiredStates[i]); + } + } + + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + modules.forEach(SwerveModule::resetEncoders); + } + + /** Zeroes the heading of the robot. */ + public Command zeroHeading() { + return runOnce(imu::reset); + } + + /** Returns the pitch of the drive gyro */ + public double getPitch() { + return imu.getPitch(); + } + + /** Returns the module states. */ + @Log.NT + private SwerveModuleState[] getModuleStates() { + return modules.stream().map(SwerveModule::state).toArray(SwerveModuleState[]::new); + } + + /** Returns the module positions */ + @Log.NT + private SwerveModulePosition[] getModulePositions() { + return modules.stream().map(SwerveModule::position).toArray(SwerveModulePosition[]::new); + } + + /** Updates pose estimation based on provided {@link EstimatedRobotPose} */ + public void updateEstimates(EstimatedRobotPose... poses) { + for (int i = 0; i < poses.length; i++) { + odometry.addVisionMeasurement(poses[i].estimatedPose.toPose2d(), poses[i].timestampSeconds); + field2d.getObject("Cam-" + i + " Est Pose").setPose(poses[i].estimatedPose.toPose2d()); + } + } + + @Override + public void periodic() { + odometry.update(getHeading(), getModulePositions()); + + field2d.setRobotPose(getPose()); + + for (int i = 0; i < modules2d.length; i++) { + var module = modules.get(i); + var transform = new Transform2d(MODULE_OFFSET[i], module.position().angle); + modules2d[i].setPose(getPose().transformBy(transform)); + } + } + + // jank + private double simulatedHeading = 0.0; + + @Override + public void simulationPeriodic() { + simulatedHeading += + kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond + * Constants.PERIOD.in(Seconds); + } + + /** Stops drivetrain */ + public Command stop() { + return runOnce(() -> driveRobotRelative(new ChassisSpeeds())); + } + + /** Sets the drivetrain to an "X" configuration, preventing movement */ + public Command lock() { + var front = new SwerveModuleState(0, Rotation2d.fromDegrees(45)); + var back = new SwerveModuleState(0, Rotation2d.fromDegrees(-45)); + return run(() -> setModuleStates(new SwerveModuleState[] {front, back, back, front})); + } + + /** Sets a new speed multiplier for the robot, this affects max cartesian and angular speeds */ + public Command setSpeedMultiplier(double multiplier) { + return runOnce(() -> speedMultiplier = multiplier); + } + + /** Locks the drive motors. */ + private Command lockDriveMotors() { + return Commands.run(() -> modules.forEach(m -> m.updateDriveSpeed(0))); + } + + /** Locks the turn motors. */ + private Command lockTurnMotors() { + return Commands.run( + () -> modules.forEach(m -> m.updateTurnRotation(Rotation2d.fromDegrees(0)))); + } + + /** Runs the drive quasistatic SysId while locking turn motors. */ + public Command driveSysIdQuasistatic(SysIdRoutine.Direction direction) { + return driveRoutine.quasistatic(direction).deadlineWith(lockTurnMotors()); + } + + /** Runs the drive dynamic SysId while locking turn motors. */ + public Command driveSysIdDynamic(SysIdRoutine.Direction direction) { + return driveRoutine.dynamic(direction).deadlineWith(lockTurnMotors()); + } + + /** Runs the turn quasistatic SysId while locking drive motors. */ + public Command turnSysIdQuasistatic(SysIdRoutine.Direction direction) { + return turnRoutine.quasistatic(direction).deadlineWith(lockDriveMotors()); + } + + /** Runs the turn dynamic SysId while locking drive motors. */ + public Command turnSysIdDynamic(SysIdRoutine.Direction direction) { + return turnRoutine.dynamic(direction).deadlineWith(lockDriveMotors()); + } + + public void close() throws Exception { + frontLeft.close(); + frontRight.close(); + rearLeft.close(); + rearRight.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java b/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java new file mode 100644 index 0000000..9748202 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java @@ -0,0 +1,121 @@ +package org.sciborgs1155.robot.drive; + +import static edu.wpi.first.units.Units.*; + +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Current; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; +import java.util.List; + +public final class DriveConstants { + public static final Measure> MAX_SPEED = MetersPerSecond.of(4.8); + public static final Measure> MAX_ANGULAR_SPEED = RadiansPerSecond.of(2 * Math.PI); + public static final Measure>> MAX_ACCEL = + MetersPerSecondPerSecond.of(6.5); + public static final Measure>> MAX_ANGULAR_ACCEL = + RadiansPerSecond.per(Second).of(2 * Math.PI); + + // Distance between centers of right and left wheels on robot + public static final Measure TRACK_WIDTH = Meters.of(0.5715); + // Distance between front and back wheels on robot + public static final Measure WHEEL_BASE = Meters.of(0.5715); + + public static final Translation2d[] MODULE_OFFSET = { + new Translation2d(WHEEL_BASE.divide(2), TRACK_WIDTH.divide(2)), // front left + new Translation2d(WHEEL_BASE.divide(2), TRACK_WIDTH.divide(-2)), // front right + new Translation2d(WHEEL_BASE.divide(-2), TRACK_WIDTH.divide(2)), // rear left + new Translation2d(WHEEL_BASE.divide(-2), TRACK_WIDTH.divide(-2)) // rear right + }; + + // angular offsets of the modules, since we use absolute encoders + // ignored (used as 0) in simulation because the simulated robot doesn't have offsets + public static final List ANGULAR_OFFSETS = + List.of( + Rotation2d.fromRadians(-Math.PI / 2), // front left + Rotation2d.fromRadians(0), // front right + Rotation2d.fromRadians(Math.PI), // rear left + Rotation2d.fromRadians(Math.PI / 2) // rear right + ); + + public static final class Translation { + public static final double P = 0.6; + public static final double I = 0.0; + public static final double D = 0.0; + } + + public static final class Rotation { + public static final double P = 0.4; + public static final double I = 0.0; + public static final double D = 0.0; + } + + public static final PathConstraints CONSTRAINTS = + new PathConstraints( + MAX_SPEED.in(MetersPerSecond), + MAX_ACCEL.in(MetersPerSecondPerSecond), + MAX_ANGULAR_SPEED.in(RadiansPerSecond), + MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second))); + + public static final class ModuleConstants { + public static final double COUPLING_RATIO = 0; + + public static final class Driving { + // Possible pinion configurations : 12T, 13T, or 14T. + public static final int PINION_TEETH = 14; + + public static final Measure CIRCUMFERENCE = Meters.of(2.0 * Math.PI * 0.0381); + + // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the + // bevel pinion + public static final double GEARING = 1.0 / 45.0 / 22.0 * 15.0 * 14.0; + + public static final Measure POSITION_FACTOR = + Rotations.of(GEARING).times(CIRCUMFERENCE.in(Meters)); + public static final Measure> VELOCITY_FACTOR = POSITION_FACTOR.per(Minute); + + public static final Measure CURRENT_LIMIT = Amps.of(50); + + public static final class PID { + public static final double P = 0.2; + public static final double I = 0.0; + public static final double D = 0.0; + } + + public static final class FF { + public static final double S = 0.3; + public static final double V = 2.7; + public static final double A = 0.25; + } + } + + static final class Turning { + public static final double MOTOR_GEARING = 1.0 / 4.0 / 3.0; + public static final double ENCODER_GEARING = 1; + + public static final Measure POSITION_FACTOR = Rotations.of(ENCODER_GEARING); + public static final Measure> VELOCITY_FACTOR = POSITION_FACTOR.per(Minute); + + public static final boolean ENCODER_INVERTED = true; + + public static final Measure CURRENT_LIMIT = Amps.of(20); + + public static final class PID { + public static final double P = 0.8; + public static final double I = 0.0; + public static final double D = 0.0; + } + + // system constants only used in simulation + public static final class FF { + public static final double S = 0.0; + public static final double V = 0.25; + public static final double A = 0.015; + } + } + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/FlexModule.java b/src/main/java/org/sciborgs1155/robot/drive/FlexModule.java new file mode 100644 index 0000000..78ed668 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/FlexModule.java @@ -0,0 +1,121 @@ +package org.sciborgs1155.robot.drive; + +import static edu.wpi.first.units.Units.*; +import static org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.COUPLING_RATIO; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Set; +import org.sciborgs1155.lib.FaultLogger; +import org.sciborgs1155.lib.SparkUtils; +import org.sciborgs1155.lib.SparkUtils.Data; +import org.sciborgs1155.lib.SparkUtils.Sensor; +import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Driving; +import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Turning; + +public class FlexModule implements ModuleIO { + + private final CANSparkBase driveMotor; // Neo Vortex + private final CANSparkBase turnMotor; // Neo 550 + + private final RelativeEncoder driveEncoder; + private final SparkAbsoluteEncoder turningEncoder; + + private final Rotation2d angularOffset; + + /** + * Constructs a SwerveModule for rev's MAX Swerve. + * + * @param drivePort drive motor port + * @param turnPort turning motor port + */ + public FlexModule(int drivePort, int turnPort, Rotation2d angularOffset) { + driveMotor = new CANSparkFlex(drivePort, MotorType.kBrushless); + driveMotor.restoreFactoryDefaults(); + driveMotor.setInverted(false); + driveMotor.setIdleMode(IdleMode.kBrake); + driveMotor.setSmartCurrentLimit((int) Driving.CURRENT_LIMIT.in(Amps)); + + turnMotor = new CANSparkMax(turnPort, MotorType.kBrushless); + turnMotor.restoreFactoryDefaults(); + turnMotor.setInverted(false); + turnMotor.setIdleMode(IdleMode.kBrake); + turnMotor.setSmartCurrentLimit((int) Turning.CURRENT_LIMIT.in(Amps)); + + driveEncoder = driveMotor.getEncoder(); + driveEncoder.setPositionConversionFactor(Driving.POSITION_FACTOR.in(Radians)); + driveEncoder.setVelocityConversionFactor(Driving.VELOCITY_FACTOR.in(RadiansPerSecond)); + + turningEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle); + turningEncoder.setInverted(Turning.ENCODER_INVERTED); + turningEncoder.setPositionConversionFactor(Turning.POSITION_FACTOR.in(Radians)); + turningEncoder.setVelocityConversionFactor(Turning.VELOCITY_FACTOR.in(RadiansPerSecond)); + + SparkUtils.configureFrameStrategy( + driveMotor, + Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE), + Set.of(Sensor.INTEGRATED), + false); + SparkUtils.configureFrameStrategy( + turnMotor, + Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE), + Set.of(Sensor.DUTY_CYCLE), + false); + + FaultLogger.register(driveMotor); + FaultLogger.register(turnMotor); + + driveMotor.burnFlash(); + turnMotor.burnFlash(); + + resetEncoders(); + + this.angularOffset = angularOffset; + } + + @Override + public void setDriveVoltage(double voltage) { + driveMotor.setVoltage(voltage); + } + + @Override + public void setTurnVoltage(double voltage) { + turnMotor.setVoltage(voltage); + } + + @Override + public double getDrivePosition() { + double driveRot = driveEncoder.getPosition(); + // account for rotation of turn motor on rotation of drive motor + driveRot -= turningEncoder.getPosition() * COUPLING_RATIO; + return driveRot; + } + + @Override + public double getDriveVelocity() { + return driveEncoder.getVelocity(); + } + + @Override + public Rotation2d getRotation() { + return Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset); + } + + @Override + public void resetEncoders() { + driveEncoder.setPosition(0); + } + + @Override + public void close() { + driveMotor.close(); + turnMotor.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java b/src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java new file mode 100644 index 0000000..d61a9d0 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java @@ -0,0 +1,48 @@ +package org.sciborgs1155.robot.drive; + +import edu.wpi.first.math.geometry.Rotation2d; +import monologue.Logged; + +/** Generalized hardware internals for a swerve module */ +public interface ModuleIO extends AutoCloseable, Logged { + /** + * Sets the drive voltage of the module. + * + * @param voltage The voltage to inputted into the drive motor. + */ + public void setDriveVoltage(double voltage); + + /** + * Sets the turn voltage of the module. + * + * @param voltage The voltage to inputted into the turn motor. + */ + public void setTurnVoltage(double voltage); + + /** + * Returns the distance the wheel traveled. + * + * @return The drive encoder position value, in radians. + */ + public double getDrivePosition(); + + /** + * Returns the current velocity of the wheel. + * + * @return The drive encoder velocity value, in radians / seconds. + */ + public double getDriveVelocity(); + + /** + * Returns the angular position of the module. + * + * @return The adjusted turn encoder position value, in radians. + */ + public Rotation2d getRotation(); + + /** Resets all encoders. */ + public void resetEncoders(); + + @Override + public void close(); +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/SimModule.java b/src/main/java/org/sciborgs1155/robot/drive/SimModule.java new file mode 100644 index 0000000..d713f6d --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/SimModule.java @@ -0,0 +1,61 @@ +package org.sciborgs1155.robot.drive; + +import static edu.wpi.first.units.Units.Seconds; +import static org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.*; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import org.sciborgs1155.robot.Constants; + +public class SimModule implements ModuleIO { + + private final DCMotorSim drive = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(Driving.FF.V, Driving.FF.A), + DCMotor.getNeo550(1), + Driving.GEARING); + private final DCMotorSim turn = + new DCMotorSim( + LinearSystemId.createDCMotorSystem(Turning.FF.V, Turning.FF.A), + DCMotor.getNeo550(1), + Turning.MOTOR_GEARING); + + @Override + public void setDriveVoltage(double voltage) { + drive.setInputVoltage(voltage); + drive.update(Constants.PERIOD.in(Seconds)); + } + + @Override + public void setTurnVoltage(double voltage) { + turn.setInputVoltage(voltage); + turn.update(Constants.PERIOD.in(Seconds)); + } + + @Override + public double getDrivePosition() { + return drive.getAngularPositionRad(); + } + + @Override + public double getDriveVelocity() { + return drive.getAngularVelocityRadPerSec(); + } + + @Override + public Rotation2d getRotation() { + return Rotation2d.fromRadians(turn.getAngularPositionRad()); + } + + @Override + public void resetEncoders() { + drive.setState(VecBuilder.fill(0, 0)); + turn.setState(VecBuilder.fill(0, 0)); + } + + @Override + public void close() {} +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/SwerveModule.java b/src/main/java/org/sciborgs1155/robot/drive/SwerveModule.java new file mode 100644 index 0000000..be979f5 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/SwerveModule.java @@ -0,0 +1,125 @@ +package org.sciborgs1155.robot.drive; + +import static org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import monologue.Annotations.Log; +import monologue.Logged; + +/** Class to encapsulate a REV Max Swerve module */ +public class SwerveModule implements Logged, AutoCloseable { + private final ModuleIO module; + + @Log.NT private final PIDController driveFeedback; + @Log.NT private final PIDController turnFeedback; + + private final SimpleMotorFeedforward driveFeedforward; + + private SwerveModuleState setpoint = new SwerveModuleState(); + + public final String name; + + /** + * Constructs a SwerveModule for rev's MAX Swerve using vortexes (flex) or krakens (talon). + * + * @param module talon OR flex swerve module + * @param angularOffset offset from drivetrain + */ + public SwerveModule(ModuleIO module, Rotation2d angularOffset, String name) { + this.module = module; + this.name = name; + driveFeedback = new PIDController(Driving.PID.P, Driving.PID.I, Driving.PID.D); + turnFeedback = new PIDController(Turning.PID.P, Turning.PID.I, Turning.PID.D); + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + + driveFeedforward = new SimpleMotorFeedforward(Driving.FF.S, Driving.FF.V, Driving.FF.A); + + setpoint = new SwerveModuleState(); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + @Log.NT + public SwerveModuleState state() { + return new SwerveModuleState(module.getDriveVelocity(), module.getRotation()); + } + + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + @Log.NT + public SwerveModulePosition position() { + return new SwerveModulePosition(module.getDrivePosition(), module.getRotation()); + } + + /** + * Updates controllers based on an optimized desired state and actuates the module accordingly. + * + *

This method should be called periodically. + * + * @param desiredState The desired state of the module. + */ + public void updateDesiredState(SwerveModuleState desiredState) { + // Optimize the reference state to avoid spinning further than 90 degrees + setpoint = SwerveModuleState.optimize(desiredState, module.getRotation()); + updateDriveSpeed(setpoint.speedMetersPerSecond); + updateTurnRotation(setpoint.angle); + } + + /** + * Updates drive controller based on setpoint. + * + *

This is only used for Sysid. + * + * @param speed The desired speed of the module. + */ + void updateDriveSpeed(double speed) { + double driveFF = driveFeedforward.calculate(speed); + double driveVoltage = driveFF + driveFeedback.calculate(module.getDriveVelocity(), speed); + module.setDriveVoltage(driveVoltage); + } + + /** + * Updates turn controller based on setpoint. + * + *

This is only used for Sysid. + * + * @param rotation The desired rotation of the module. + */ + void updateTurnRotation(Rotation2d rotation) { + double turnVoltage = + turnFeedback.calculate(module.getRotation().getRadians(), rotation.getRadians()); + module.setTurnVoltage(turnVoltage); + } + + @Log.NT + public SwerveModuleState desiredState() { + return setpoint; + } + + public void setDriveVoltage(double voltage) { + module.setDriveVoltage(voltage); + } + + public void setTurnVoltage(double voltage) { + module.setTurnVoltage(voltage); + } + + public void resetEncoders() { + module.resetEncoders(); + } + + @Override + public void close() { + module.close(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java b/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java new file mode 100644 index 0000000..843369f --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/TalonModule.java @@ -0,0 +1,93 @@ +package org.sciborgs1155.robot.drive; + +import static edu.wpi.first.units.Units.*; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkAbsoluteEncoder.Type; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.Set; +import org.sciborgs1155.lib.SparkUtils; +import org.sciborgs1155.lib.SparkUtils.Data; +import org.sciborgs1155.lib.SparkUtils.Sensor; +import org.sciborgs1155.robot.drive.DriveConstants.ModuleConstants.Turning; + +/** Class to encapsulate a CTRE Talon Swerve module */ +public class TalonModule implements ModuleIO { + private final TalonFX driveMotor; + private final CANSparkMax turnMotor; + + private final SparkAbsoluteEncoder turnEncoder; + + public TalonModule(int drivePort, int turnPort) { + driveMotor = new TalonFX(drivePort); + turnMotor = new CANSparkMax(turnPort, MotorType.kBrushless); + + resetEncoders(); + + driveMotor.getPosition().setUpdateFrequency(100); + driveMotor.getVelocity().setUpdateFrequency(100); + + turnMotor.restoreFactoryDefaults(); + turnMotor.setInverted(false); + turnMotor.setIdleMode(IdleMode.kBrake); + turnMotor.setSmartCurrentLimit((int) Turning.CURRENT_LIMIT.in(Amps)); + + turnEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle); + turnEncoder.setInverted(Turning.ENCODER_INVERTED); + turnEncoder.setPositionConversionFactor(Turning.POSITION_FACTOR.in(Radians)); + turnEncoder.setVelocityConversionFactor(Turning.VELOCITY_FACTOR.in(RadiansPerSecond)); + + SparkUtils.configureFrameStrategy( + turnMotor, + Set.of(Data.POSITION, Data.VELOCITY, Data.VOLTAGE), + Set.of(Sensor.DUTY_CYCLE), + false); + + TalonFXConfiguration toApply = new TalonFXConfiguration(); + toApply.MotorOutput.NeutralMode = NeutralModeValue.Brake; + toApply.CurrentLimits.SupplyCurrentLimit = 50; + driveMotor.getConfigurator().apply(toApply); + } + + @Override + public void setDriveVoltage(double voltage) { + driveMotor.setVoltage(voltage); + } + + @Override + public void setTurnVoltage(double voltage) { + turnMotor.setVoltage(voltage); + } + + @Override + public double getDrivePosition() { + return driveMotor.getPosition().getValueAsDouble(); + } + + @Override + public double getDriveVelocity() { + return driveMotor.getVelocity().getValueAsDouble(); + } + + @Override + public Rotation2d getRotation() { + return Rotation2d.fromRadians(turnEncoder.getPosition()); + } + + @Override + public void resetEncoders() { + driveMotor.setPosition(0); + } + + @Override + public void close() { + turnMotor.close(); + driveMotor.close(); + } +} diff --git a/src/test/java/org/sciborgs1155/robot/SwerveTest.java b/src/test/java/org/sciborgs1155/robot/SwerveTest.java new file mode 100644 index 0000000..e96a7be --- /dev/null +++ b/src/test/java/org/sciborgs1155/robot/SwerveTest.java @@ -0,0 +1,20 @@ +package org.sciborgs1155.robot; + +// import static org.sciborgs1155.lib.TestingUtil.*; + +/** Swerve test. Currently incomplete and does nothing. */ +public class SwerveTest { + // Drive drive; + + // @BeforeEach + // public void setup() { + // setupHAL(); + // drive = Drive.create(); + // } + + // @Test + // public void reachSetpoint() { + // run(drive.drive(() -> 1, () -> 1, () -> 1)); + // fastForward(); + // } +} diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json new file mode 100644 index 0000000..d4b7514 --- /dev/null +++ b/vendordeps/URCL.json @@ -0,0 +1,65 @@ +{ + "fileName": "URCL.json", + "name": "URCL", + "version": "2024.0.0", + "frcYear": "2024", + "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", + "mavenUrls": [ + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.0.0" + ], + "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-java", + "version": "2024.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-cpp", + "version": "2024.0.0", + "libName": "URCL", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + }, + { + "groupId": "org.littletonrobotics.urcl", + "artifactId": "URCL-driver", + "version": "2024.0.0", + "libName": "URCLDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena", + "osxuniversal" + ] + } + ] +} \ No newline at end of file