From 8189141f0ea9ec6907972a7c3c747fb1d7ca3bce Mon Sep 17 00:00:00 2001 From: Yxhej Date: Wed, 17 Jan 2024 16:31:08 -0500 Subject: [PATCH] flywheel base --- build.gradle | 2 +- .../java/org/sciborgs1155/robot/Ports.java | 5 + .../java/org/sciborgs1155/robot/Robot.java | 8 +- .../org/sciborgs1155/robot/drive/Drive.java | 325 ++++++++++++++++++ .../robot/drive/DriveConstants.java | 66 ++++ .../sciborgs1155/robot/drive/IdealModule.java | 52 +++ .../robot/drive/MAXSwerveModule.java | 153 +++++++++ .../sciborgs1155/robot/drive/ModuleIO.java | 41 +++ .../sciborgs1155/robot/drive/SimModule.java | 98 ++++++ .../robot/shooter/FlywheelIO.java | 16 + .../robot/shooter/RealFlywheel.java | 20 ++ .../sciborgs1155/robot/shooter/Shooter.java | 3 + .../robot/shooter/ShooterConstants.java | 10 + .../robot/shooter/SimFlywheel.java | 8 + vendordeps/REVLib.json | 10 +- 15 files changed, 807 insertions(+), 10 deletions(-) create mode 100644 src/main/java/org/sciborgs1155/robot/drive/Drive.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/IdealModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java create mode 100644 src/main/java/org/sciborgs1155/robot/drive/SimModule.java create mode 100644 src/main/java/org/sciborgs1155/robot/shooter/FlywheelIO.java create mode 100644 src/main/java/org/sciborgs1155/robot/shooter/RealFlywheel.java create mode 100644 src/main/java/org/sciborgs1155/robot/shooter/Shooter.java create mode 100644 src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java create mode 100644 src/main/java/org/sciborgs1155/robot/shooter/SimFlywheel.java diff --git a/build.gradle b/build.gradle index 81650d6d..e9c44483 100644 --- a/build.gradle +++ b/build.gradle @@ -76,7 +76,7 @@ dependencies { testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2' // Monologue - implementation 'com.github.shueja:Monologue:v1.0.0-alpha2' + implementation 'com.github.shueja:Monologue:v1.0.0-beta4' } test { diff --git a/src/main/java/org/sciborgs1155/robot/Ports.java b/src/main/java/org/sciborgs1155/robot/Ports.java index 3bcb5126..11492229 100644 --- a/src/main/java/org/sciborgs1155/robot/Ports.java +++ b/src/main/java/org/sciborgs1155/robot/Ports.java @@ -5,4 +5,9 @@ public static final class OI { public static final int OPERATOR = 0; public static final int DRIVER = 1; } + + public static final class Shooter { + public static final int FLYWHEEL = -1; + public static final int ROTATE = -1; + } } diff --git a/src/main/java/org/sciborgs1155/robot/Robot.java b/src/main/java/org/sciborgs1155/robot/Robot.java index 21cdcb2e..79316690 100644 --- a/src/main/java/org/sciborgs1155/robot/Robot.java +++ b/src/main/java/org/sciborgs1155/robot/Robot.java @@ -5,9 +5,9 @@ import edu.wpi.first.wpilibj2.command.ProxyCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import java.util.List; +import monologue.Annotations.Log; import monologue.Logged; import monologue.Monologue; -import monologue.Monologue.LogBoth; import org.sciborgs1155.lib.CommandRobot; import org.sciborgs1155.lib.Fallible; import org.sciborgs1155.lib.SparkUtils; @@ -29,7 +29,7 @@ public class Robot extends CommandRobot implements Logged, Fallible { // SUBSYSTEMS // COMMANDS - @LogBoth Autos autos = new Autos(); + @Log.NT Autos autos = new Autos(); /** The robot contains subsystems, OI devices, and commands. */ public Robot() { @@ -48,8 +48,8 @@ private void configureGameBehavior() { // Configure logging with DataLogManager and Monologue DataLogManager.start(); - Monologue.setupLogging(this, "/Robot"); - addPeriodic(Monologue::update, kDefaultPeriod); + Monologue.setupMonologue(this, "/Robot", false, true); + addPeriodic(Monologue::updateAll, kDefaultPeriod); // Burn flash of all Spark Max at once with delays SparkUtils.safeBurnFlash(); 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 00000000..286d35a9 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/Drive.java @@ -0,0 +1,325 @@ +package org.sciborgs1155.robot.drive; + +import static org.sciborgs1155.lib.PathFlipper.pathForAlliance; +import static org.sciborgs1155.robot.Ports.Drive.*; +import static org.sciborgs1155.robot.drive.DriveConstants.*; + +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.pathplanner.lib.PathPlanner; +import com.pathplanner.lib.PathPlannerTrajectory; +import com.pathplanner.lib.PathPoint; +import com.pathplanner.lib.commands.PPSwerveControllerCommand; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +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.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import io.github.oblarg.oblog.Loggable; +import io.github.oblarg.oblog.annotations.Log; +import java.util.List; +import java.util.function.DoubleSupplier; +import org.photonvision.EstimatedRobotPose; +import org.sciborgs1155.lib.DeferredCommand; +import org.sciborgs1155.lib.failure.Fallible; +import org.sciborgs1155.lib.failure.FaultBuilder; +import org.sciborgs1155.lib.failure.HardwareFault; +import org.sciborgs1155.robot.Constants; +import org.sciborgs1155.robot.Robot; + +public class Drive extends SubsystemBase implements Fallible, Loggable, AutoCloseable { + + @Log private final ModuleIO frontLeft; + @Log private final ModuleIO frontRight; + @Log private final ModuleIO rearLeft; + @Log private final ModuleIO rearRight; + + private final List modules; + + // this should be a generic IMU class, once WPILib implements it + @Log private final WPI_PigeonIMU imu = new WPI_PigeonIMU(PIGEON); + + public final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(MODULE_OFFSET); + + // Odometry and pose estimation + private final SwerveDrivePoseEstimator odometry; + + @Log private final Field2d field2d = new Field2d(); + private final FieldObject2d[] modules2d; + + // Rate limiting + private final SlewRateLimiter xLimiter = new SlewRateLimiter(MAX_ACCEL); + private final SlewRateLimiter yLimiter = new SlewRateLimiter(MAX_ACCEL); + + @Log private double speedMultiplier = 1; + + public static Drive create() { + return Robot.isReal() + ? new Drive( + new MAXSwerveModule( + MODULE_NAMES[0], FRONT_LEFT_DRIVE, FRONT_LEFT_TURNING, ANGULAR_OFFSETS[0]), + new MAXSwerveModule( + MODULE_NAMES[1], FRONT_RIGHT_DRIVE, FRONT_RIGHT_TURNING, ANGULAR_OFFSETS[1]), + new MAXSwerveModule( + MODULE_NAMES[2], REAR_LEFT_DRIVE, REAR_LEFT_TURNING, ANGULAR_OFFSETS[2]), + new MAXSwerveModule( + MODULE_NAMES[3], REAR_RIGHT_DRIVE, REAR_RIGHT_TURNING, ANGULAR_OFFSETS[3])) + : new Drive(new SimModule(), new SimModule(), new SimModule(), new SimModule()); + } + + public Drive(ModuleIO frontLeft, ModuleIO frontRight, ModuleIO rearLeft, ModuleIO rearRight) { + this.frontLeft = frontLeft; + this.frontRight = frontRight; + this.rearLeft = rearLeft; + this.rearRight = rearRight; + + modules = List.of(frontLeft, frontRight, rearLeft, rearRight); + modules2d = new FieldObject2d[modules.size()]; + + odometry = + new SwerveDrivePoseEstimator(kinematics, getHeading(), getModulePositions(), new Pose2d()); + + for (int i = 0; i < modules2d.length; i++) { + modules2d[i] = field2d.getObject("module-" + i); + } + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + 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 imu.getRotation2d(); + } + + /** + * 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 x y and omega velocities */ + public CommandBase drive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vOmega) { + return run( + () -> + drive( + ChassisSpeeds.fromFieldRelativeSpeeds( + xLimiter.calculate(scale(vx.getAsDouble()) * MAX_SPEED * speedMultiplier), + yLimiter.calculate(scale(vy.getAsDouble()) * MAX_SPEED * speedMultiplier), + scale(vOmega.getAsDouble()) * MAX_ANGULAR_SPEED * speedMultiplier, + getHeading()))); + } + + /** + * Drives the robot based on profided {@link ChassisSpeeds}. + * + *

This method uses {@link Pose2d#log(Pose2d)} to reduce skew. + * + * @param speeds The desired chassis speeds. + */ + public void drive(ChassisSpeeds speeds) { + var target = + new Pose2d( + speeds.vxMetersPerSecond * Constants.PERIOD, + speeds.vyMetersPerSecond * Constants.PERIOD, + Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * Constants.PERIOD)); + + var twist = new Pose2d().log(target); + + speeds = + new ChassisSpeeds( + twist.dx / Constants.PERIOD, + twist.dy / Constants.PERIOD, + twist.dtheta / Constants.PERIOD); + + setModuleStates(kinematics.toSwerveModuleStates(speeds)); + } + + /** + * Sets the swerve ModuleStates. + * + * @param desiredStates The desired ModuleIO 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); + + for (int i = 0; i < modules.size(); i++) { + modules.get(i).setDesiredState(desiredStates[i]); + } + } + + /** Resets the drive encoders to currently read a position of 0. */ + public void resetEncoders() { + modules.forEach(ModuleIO::resetEncoders); + } + + /** Zeroes the heading of the robot. */ + public CommandBase zeroHeading() { + return runOnce(imu::reset); + } + + /** Returns the pitch of the drive gyro */ + public double getPitch() { + return imu.getPitch(); + } + + private SwerveModuleState[] getModuleStates() { + return modules.stream().map(ModuleIO::getState).toArray(SwerveModuleState[]::new); + } + + private SwerveModulePosition[] getModulePositions() { + return modules.stream().map(ModuleIO::getPosition).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.getPosition().angle); + modules2d[i].setPose(getPose().transformBy(transform)); + } + } + + @Override + public void simulationPeriodic() { + imu.getSimCollection() + .addHeading( + Units.radiansToDegrees( + kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond) + * Constants.PERIOD); + } + + /** Stops drivetrain */ + public CommandBase stop() { + return runOnce(() -> drive(new ChassisSpeeds())); + } + + /** Sets the drivetrain to an "X" configuration, preventing movement */ + public CommandBase 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 CommandBase setSpeedMultiplier(double multiplier) { + return runOnce(() -> speedMultiplier = multiplier); + } + + /** + * Follows a simple {@link PathPlannerTrajectory} on the field. + * + *

For safer use, see {@link this#followPath(PathPlannerTrajectory, boolean)}. + * + * @param trajectory The trajectory to follow. + * @return The command that follows the trajectory. + */ + public CommandBase follow(PathPlannerTrajectory trajectory) { + return new PPSwerveControllerCommand( + trajectory, + this::getPose, + kinematics, + new PIDController(TRANSLATION.p(), TRANSLATION.i(), TRANSLATION.d()), + new PIDController(TRANSLATION.p(), TRANSLATION.i(), TRANSLATION.d()), + new PIDController(ROTATION.p(), ROTATION.i(), ROTATION.d()), + this::setModuleStates, + false) + .andThen(stop()); + } + + /** + * Follows the specified trajectory using {@link this#follow(PathPlannerTrajectory)} and resets if + * specified + */ + public CommandBase followPath(PathPlannerTrajectory path, boolean resetOdometry) { + var newPath = pathForAlliance(path, DriverStation.getAlliance()); + return (resetOdometry ? pathOdometryReset(newPath) : Commands.none()).andThen(follow(newPath)); + } + + /** Resets odometry to first pose in path, using ppl to reflects if using alliance color */ + public CommandBase pathOdometryReset(PathPlannerTrajectory trajectory) { + var initialState = trajectory.getInitialState(); + Pose2d initialPose = + new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation); + return Commands.runOnce(() -> resetOdometry(initialPose), this); + } + + /** Creates and follows trajectroy for swerve from startPose to desiredPose */ + public CommandBase driveToPose(Pose2d startPose, Pose2d desiredPose) { + Rotation2d heading = desiredPose.minus(startPose).getTranslation().getAngle(); + PathPoint start = new PathPoint(startPose.getTranslation(), heading, startPose.getRotation()); + PathPoint goal = + new PathPoint(desiredPose.getTranslation(), heading, desiredPose.getRotation()); + PathPlannerTrajectory trajectory = PathPlanner.generatePath(CONSTRAINTS, start, goal); + return follow(trajectory); + } + + /** Creates and follows trajectory for swerve from current pose to desiredPose */ + public CommandBase driveToPose(Pose2d desiredPose) { + return new DeferredCommand(() -> driveToPose(getPose(), desiredPose), this); + } + + @Override + public List getFaults() { + var builder = new FaultBuilder(); + for (var module : modules) { + builder.register(module.getFaults()); + } + return builder.build(); + } + + public void close() throws Exception { + frontLeft.close(); + frontRight.close(); + rearLeft.close(); + rearRight.close(); + imu.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 00000000..5dd4f357 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/DriveConstants.java @@ -0,0 +1,66 @@ +package org.sciborgs1155.robot.drive; + +import edu.wpi.first.math.geometry.Translation2d; + +public final class DriveConstants { + public static final double MAX_SPEED = 4.8; // m / s + public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // rad / s + public static final double MAX_ACCEL = 6.5; // m / s^2 + + public static final double TRACK_WIDTH = 0.5715; + // Distance between centers of right and left wheels on robot + public static final double WHEEL_BASE = 0.5715; + // Distance between front and back wheels on robot + + public static final Translation2d[] MODULE_OFFSET = { + new Translation2d(WHEEL_BASE / 2, TRACK_WIDTH / 2), // front left + new Translation2d(WHEEL_BASE / 2, -TRACK_WIDTH / 2), // front right + new Translation2d(-WHEEL_BASE / 2, TRACK_WIDTH / 2), // rear left + new Translation2d(-WHEEL_BASE / 2, -TRACK_WIDTH / 2) // rear right + }; + + public static final String[] MODULE_NAMES = { + "front left module", "front right module", "rear left moduke", "rear right module" + }; + + // 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 double[] ANGULAR_OFFSETS = { + -Math.PI / 2, // front left + 0, // front right + Math.PI, // rear left + Math.PI / 2 // rear right + }; + + public static final PIDConstants TRANSLATION = new PIDConstants(0.6, 0, 0); + public static final PIDConstants ROTATION = new PIDConstants(0.4, 0, 0); + + public static final PathConstraints CONSTRAINTS = + new PathConstraints(MAX_SPEED / 1.9, MAX_ACCEL / 1.4); + + public static final class SwerveModule { + public static final class Driving { + public static final double CIRCUMFERENCE = 2.0 * Math.PI * 0.0381; + // Diameter of the wheel in meters (2 * π * R) + + public static final double GEARING = 1.0 / 45.0 / 22.0 * 15.0 * 14.0; + + public static final double CONVERSION = CIRCUMFERENCE * GEARING; + + public static final PIDConstants PID = new PIDConstants(0.11, 0, 0.06); + public static final BasicFFConstants FF = new BasicFFConstants(0.3, 2.7, 0.25); + } + + public static final class Turning { + public static final double MOTOR_GEARING = 1.0 / 4.0 / 3.0; + + public static final double CONVERSION = 2.0 * Math.PI; + + public static final boolean ENCODER_INVERTED = true; + + public static final PIDConstants PID = new PIDConstants(2, 0, 0.1); + // system constants only used in simulation + public static final BasicFFConstants FF = new BasicFFConstants(0, 0.25, 0.015); + } + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/IdealModule.java b/src/main/java/org/sciborgs1155/robot/drive/IdealModule.java new file mode 100644 index 00000000..630a3158 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/IdealModule.java @@ -0,0 +1,52 @@ +package org.sciborgs1155.robot.drive; + +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.List; +import org.sciborgs1155.lib.failure.HardwareFault; +import org.sciborgs1155.robot.Constants; + +/** Ideal swerve module, useful for debugging */ +public class IdealModule implements ModuleIO { + + private SwerveModuleState state = new SwerveModuleState(); + private double distance; + + @Override + public SwerveModuleState getState() { + return state; + } + + @Override + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(distance, state.angle); + } + + @Override + public void setDesiredState(SwerveModuleState desiredState) { + state = SwerveModuleState.optimize(desiredState, state.angle); + distance += state.speedMetersPerSecond * Constants.PERIOD; + } + + @Override + public SwerveModuleState getDesiredState() { + return state; + } + + @Override + public void resetEncoders() {} + + @Override + public void setTurnPID(double kP, double kI, double kD) {} + + @Override + public void setDrivePID(double kP, double kI, double kD) {} + + @Override + public void close() {} + + @Override + public List getFaults() { + return List.of(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java b/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java new file mode 100644 index 00000000..e148dc1d --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/MAXSwerveModule.java @@ -0,0 +1,153 @@ +package org.sciborgs1155.robot.drive; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.ControlType; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxAbsoluteEncoder.Type; +import com.revrobotics.SparkMaxPIDController; +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 java.util.List; +import org.sciborgs1155.lib.SparkUtils; +import org.sciborgs1155.robot.drive.DriveConstants.SwerveModule.Driving; +import org.sciborgs1155.robot.drive.DriveConstants.SwerveModule.Turning; + +/** Class to encapsulate a rev max swerve module */ +public class MAXSwerveModule implements ModuleIO { + + private final CANSparkMax driveMotor; // Regular Neo + private final CANSparkMax turnMotor; // Neo 550 + + private final RelativeEncoder driveEncoder; + private final AbsoluteEncoder turningEncoder; + + private final SparkMaxPIDController driveFeedback; + private final SparkMaxPIDController turnFeedback; + + private final SimpleMotorFeedforward driveFeedforward = + new SimpleMotorFeedforward(Driving.FF.s(), Driving.FF.v(), Driving.FF.a()); + + private final String name; + private final Rotation2d angularOffset; + + private SwerveModuleState setpoint = new SwerveModuleState(); + + /** + * Constructs a SwerveModule for rev's MAX Swerve. + * + * @param drivePort drive motor port + * @param turnPort turning motor port + * @param angularOffset offset from drivetrain + */ + public MAXSwerveModule(String name, int drivePort, int turnPort, double angularOffset) { + this.name = name; + + driveMotor = SparkUtils.create(drivePort); + turnMotor = SparkUtils.create(turnPort); + + driveEncoder = driveMotor.getEncoder(); + turningEncoder = turnMotor.getAbsoluteEncoder(Type.kDutyCycle); + + driveFeedback = driveMotor.getPIDController(); + turnFeedback = turnMotor.getPIDController(); + + driveFeedback.setFeedbackDevice(driveEncoder); + turnFeedback.setFeedbackDevice(turningEncoder); + + turningEncoder.setInverted(Turning.ENCODER_INVERTED); + + setDrivePID(Driving.PID); + setTurnPID(Turning.PID); + + driveEncoder.setPositionConversionFactor(Driving.CONVERSION); + driveEncoder.setVelocityConversionFactor(Driving.CONVERSION / 60.0); + turningEncoder.setPositionConversionFactor(Turning.CONVERSION); + turningEncoder.setVelocityConversionFactor(Turning.CONVERSION / 60.0); + + // set up continuous input for turning + turnFeedback.setPositionPIDWrappingEnabled(true); + turnFeedback.setPositionPIDWrappingMinInput(0); + turnFeedback.setPositionPIDWrappingMaxInput(Turning.CONVERSION); + + SparkUtils.disableFrames(driveMotor, 4, 5, 6); + SparkUtils.disableFrames(turnMotor, 4, 6); + + driveEncoder.setPosition(0); + this.angularOffset = Rotation2d.fromRadians(angularOffset); + } + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + @Override + public SwerveModuleState getState() { + return new SwerveModuleState( + driveEncoder.getVelocity(), + Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset)); + } + + @Override + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + driveEncoder.getPosition(), + Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset)); + } + + @Override + public void setDesiredState(SwerveModuleState desiredState) { + SwerveModuleState correctedDesiredState = new SwerveModuleState(); + correctedDesiredState.speedMetersPerSecond = desiredState.speedMetersPerSecond; + correctedDesiredState.angle = desiredState.angle.plus(angularOffset); + // Optimize the reference state to avoid spinning further than 90 degrees + setpoint = + SwerveModuleState.optimize( + correctedDesiredState, Rotation2d.fromRadians(turningEncoder.getPosition())); + + double driveFF = driveFeedforward.calculate(setpoint.speedMetersPerSecond); + driveFeedback.setReference(setpoint.speedMetersPerSecond, ControlType.kVelocity, 0, driveFF); + turnFeedback.setReference(setpoint.angle.getRadians(), ControlType.kPosition); + } + + @Override + public SwerveModuleState getDesiredState() { + return setpoint; + } + + @Override + public void resetEncoders() { + driveEncoder.setPosition(0); + } + + @Override + public void setTurnPID(PIDConstants constants) { + turnFeedback.setP(constants.p()); + turnFeedback.setI(constants.i()); + turnFeedback.setD(constants.d()); + } + + @Override + public void setDrivePID(PIDConstants constants) { + driveFeedback.setP(constants.p()); + driveFeedback.setI(constants.i()); + driveFeedback.setD(constants.d()); + } + + @Override + public List getFaults() { + return FaultBuilder.create() + .register(name + " drive", driveMotor) + .register(name + " turn", turnMotor) + .build(); + } + + @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 00000000..a64f69b5 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java @@ -0,0 +1,41 @@ +package org.sciborgs1155.robot.drive; + +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import org.sciborgs1155.lib.Fallible; + +/** Generalized SwerveModule with closed loop control */ +public interface ModuleIO extends Fallible, Sendable, AutoCloseable { + /** Returns the current state of the module. */ + public SwerveModuleState getState(); + + /** Returns the current position of the module. */ + public SwerveModulePosition getPosition(); + + /** Sets the desired state for the module. */ + public void setDesiredState(SwerveModuleState desiredState); + + /** Returns the desired state for the module. */ + public SwerveModuleState getDesiredState(); + + /** Zeroes all the drive encoders. */ + public void resetEncoders(); + + /** Sets the turn PID constants for the module. */ + public void setTurnPID(double kP, double kI, double kD); + + /** Sets the drive PID constants for the module. */ + public void setDrivePID(double kP, double kI, double kD); + + @Override + default void initSendable(SendableBuilder builder) { + builder.addDoubleProperty("current velocity", () -> getState().speedMetersPerSecond, null); + builder.addDoubleProperty("current angle", () -> getPosition().angle.getRadians(), null); + builder.addDoubleProperty("current position", () -> getPosition().distanceMeters, null); + builder.addDoubleProperty( + "target velocity", () -> getDesiredState().speedMetersPerSecond, null); + builder.addDoubleProperty("target angle", () -> getDesiredState().angle.getRadians(), null); + } +} 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 00000000..eb7b1058 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/drive/SimModule.java @@ -0,0 +1,98 @@ +package org.sciborgs1155.robot.drive; + +import edu.wpi.first.math.VecBuilder; +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 edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import java.util.List; +import org.sciborgs1155.robot.Constants; +import org.sciborgs1155.robot.drive.DriveConstants.SwerveModule.Driving; +import org.sciborgs1155.robot.drive.DriveConstants.SwerveModule.Turning; + +/** Class to encapsulate a rev max swerve module */ +public class SimModule implements ModuleIO { + + private final DCMotorSim drive = Driving.FF.sim(DCMotor.getNEO(1), Driving.GEARING); + private final DCMotorSim turn = Turning.FF.sim(DCMotor.getNeo550(1), Turning.MOTOR_GEARING); + + private final PIDController driveFeedback = + new PIDController(Driving.PID.p() * 115, Driving.PID.i(), Driving.PID.d() * 115.0 / 1000); + private final PIDController turnFeedback = + new PIDController(Turning.PID.p() * 2, Turning.PID.i(), Turning.PID.d() * 2.0 / 1000); + + private final SimpleMotorFeedforward driveFeedforward = + new SimpleMotorFeedforward(Driving.FF.s(), Driving.FF.v(), Driving.FF.a()); + + private SwerveModuleState setpoint = new SwerveModuleState(); + + public SimModule() { + turnFeedback.enableContinuousInput(0, Turning.CONVERSION); + } + + @Override + public SwerveModuleState getState() { + return new SwerveModuleState( + drive.getAngularVelocityRadPerSec(), + Rotation2d.fromRadians(turn.getAngularVelocityRadPerSec())); + } + + @Override + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + drive.getAngularPositionRad(), Rotation2d.fromRadians(turn.getAngularPositionRad())); + } + + @Override + public void setDesiredState(SwerveModuleState desiredState) { + // Optimize the reference state to avoid spinning further than 90 degrees + setpoint = + SwerveModuleState.optimize( + desiredState, Rotation2d.fromRadians(turn.getAngularPositionRad())); + + final double driveFF = driveFeedforward.calculate(setpoint.speedMetersPerSecond); + + final double driveFB = + driveFeedback.calculate(drive.getAngularVelocityRadPerSec(), setpoint.speedMetersPerSecond); + + final double turnFB = + turnFeedback.calculate(turn.getAngularPositionRad(), setpoint.angle.getRadians()); + + drive.setInputVoltage(driveFB + driveFF); + drive.update(Constants.PERIOD); + turn.setInputVoltage(turnFB); + turn.update(Constants.PERIOD); + } + + @Override + public SwerveModuleState getDesiredState() { + return setpoint; + } + + @Override + public void resetEncoders() { + drive.setState(VecBuilder.fill(0, 0)); + turn.setState(VecBuilder.fill(0, 0)); + } + + @Override + public void setTurnPID(PIDConstants constants) { + turnFeedback.setPID(constants.p(), constants.i(), constants.d()); + } + + @Override + public void setDrivePID(PIDConstants constants) { + driveFeedback.setPID(constants.p(), constants.i(), constants.d()); + } + + @Override + public void close() {} + + @Override + public List getFaults() { + return List.of(); + } +} diff --git a/src/main/java/org/sciborgs1155/robot/shooter/FlywheelIO.java b/src/main/java/org/sciborgs1155/robot/shooter/FlywheelIO.java new file mode 100644 index 00000000..11cc814d --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/shooter/FlywheelIO.java @@ -0,0 +1,16 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.sciborgs1155.robot.shooter; + +/** May be worth splitting between arm and shooter subsystems */ +public interface FlywheelIO extends AutoCloseable { + public void setFlywheelVoltage(); + + public double getRotation(); + + public double getFlywheelSpeed(); + + public double getRotationSpeed(); +} diff --git a/src/main/java/org/sciborgs1155/robot/shooter/RealFlywheel.java b/src/main/java/org/sciborgs1155/robot/shooter/RealFlywheel.java new file mode 100644 index 00000000..bdf43c2f --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/shooter/RealFlywheel.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.sciborgs1155.robot.shooter; + +import static org.sciborgs1155.robot.Ports.Shooter.*; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; + +/** Add your docs here. */ +public class RealFlywheel { + private final CANSparkFlex flywheel = new CANSparkFlex(FLYWHEEL, MotorType.kBrushless); + private final SparkENcod + + public RealFlywheel() { + + } +} diff --git a/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java b/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java new file mode 100644 index 00000000..868ce946 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/shooter/Shooter.java @@ -0,0 +1,3 @@ +package org.sciborgs1155.robot.shooter; + +public class Shooter {} diff --git a/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java b/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java new file mode 100644 index 00000000..2ab84a82 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/shooter/ShooterConstants.java @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.sciborgs1155.robot.shooter; + +/** Add your docs here. */ +public class ShooterConstants { + public static final double GEARING = -1; +} diff --git a/src/main/java/org/sciborgs1155/robot/shooter/SimFlywheel.java b/src/main/java/org/sciborgs1155/robot/shooter/SimFlywheel.java new file mode 100644 index 00000000..01596396 --- /dev/null +++ b/src/main/java/org/sciborgs1155/robot/shooter/SimFlywheel.java @@ -0,0 +1,8 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.sciborgs1155.robot.shooter; + +/** Add your docs here. */ +public class SimFlywheel {} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 7eb37558..0f3520e7 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.0.0", + "version": "2024.2.0", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.0.0" + "version": "2024.2.0" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.0.0", + "version": "2024.2.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.0.0", + "version": "2024.2.0", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.0.0", + "version": "2024.2.0", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,