From ac580a68f716ebeb3398ac7834039d6929f4ed15 Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 29 Jan 2024 16:50:12 -0500 Subject: [PATCH 1/2] Finalized shoot command and logging --- .../lib/drivers/encoders/EncoderIOAnalog.java | 2 +- .../drivers/encoders/EncoderIODigital.java | 2 +- .../lib/drivers/encoders/EncoderIOSim.java | 4 +- .../com/team1701/robot/RobotContainer.java | 10 ----- .../robot/commands/RotateRelativeToRobot.java | 6 +-- .../com/team1701/robot/commands/Shoot.java | 32 ++++++++-------- .../robot/subsystems/shooter/Shooter.java | 37 ++++++++----------- 7 files changed, 40 insertions(+), 53 deletions(-) diff --git a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java index d4289f61..7fe68ea4 100644 --- a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java +++ b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.AnalogEncoder; public class EncoderIOAnalog implements EncoderIO { - private final AnalogEncoder mEncoder; + public final AnalogEncoder mEncoder; public EncoderIOAnalog(int channel) { mEncoder = new AnalogEncoder(channel); diff --git a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java index f7dde814..22689e13 100644 --- a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java +++ b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj.DutyCycleEncoder; public class EncoderIODigital implements EncoderIO { - private final DutyCycleEncoder mEncoder; + public final DutyCycleEncoder mEncoder; public EncoderIODigital(int channel) { mEncoder = new DutyCycleEncoder(Constants.Shooter.kShooterThroughBoreEncoderId); diff --git a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java index 535cbe7e..1698637c 100644 --- a/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java +++ b/src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java @@ -5,10 +5,12 @@ import edu.wpi.first.math.geometry.Rotation2d; public class EncoderIOSim implements EncoderIO { - private final Supplier mRotationSupplier; + public final Supplier mRotationSupplier; + public double rotations; public EncoderIOSim(Supplier rotationSupplier) { mRotationSupplier = rotationSupplier; + rotations = mRotationSupplier.get().getRotations(); } @Override diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java index 9689e110..8c6b3b17 100644 --- a/src/main/java/com/team1701/robot/RobotContainer.java +++ b/src/main/java/com/team1701/robot/RobotContainer.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.util.PathPlannerLogging; import com.team1701.lib.alerts.TriggeredAlert; -import com.team1701.lib.drivers.digitalinputs.DigitalIOSensor; import com.team1701.lib.drivers.encoders.EncoderIO; import com.team1701.lib.drivers.encoders.EncoderIOAnalog; import com.team1701.lib.drivers.gyros.GyroIO; @@ -39,7 +38,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import static com.team1701.robot.commands.DriveCommands.*; @@ -93,8 +91,6 @@ public RobotContainer() { Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), ShooterMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), - new DigitalIOSensor(Constants.Shooter.kShooterEntranceSensorId), - new DigitalIOSensor(Constants.Shooter.kShooterExitSensorId), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); break; case SIMULATION_BOT: @@ -113,10 +109,6 @@ public RobotContainer() { Shooter.createMotorSim(DCMotor.getNeoVortex(1)), Shooter.createMotorSim(DCMotor.getNeoVortex(1)), Shooter.createMotorSim(DCMotor.getNeoVortex(1)), - Shooter.createDigitalSim(() -> - new LoggedDashboardBoolean("SimulatedShooter/EntranceSensorBlocked", false).get()), - Shooter.createDigitalSim(() -> - new LoggedDashboardBoolean("SimulatedShooter/ExitSensorBlocked", false).get()), Shooter.createEncoderSim(() -> new Rotation2d(Units.degreesToRadians( new LoggedTunableNumber("SimulatedThroughBoreEncoder/InitialAngleDegrees", 30) .get()))))); @@ -152,8 +144,6 @@ public RobotContainer() { Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), ShooterMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), - new DigitalIOSensor(Constants.Shooter.kShooterEntranceSensorId), - new DigitalIOSensor(Constants.Shooter.kShooterExitSensorId), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); setupControllerBindings(); diff --git a/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java b/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java index 58d5cdcf..d8491d1c 100644 --- a/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java +++ b/src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java @@ -64,14 +64,14 @@ public class RotateRelativeToRobot extends Command { @Override public void initialize() { mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits); - mRotationalOffset = mRobotState.getPose2d().getRotation(); + mRotationalOffset = mRobotState.getHeading(); Logger.recordOutput(kLoggingPrefix + "requestedRotation/robotRelative", mTargetRelativeRotation); Logger.recordOutput(kLoggingPrefix + "requestedRotation/fieldRelative", mTargetFieldRotation); mRotationController.reset(); - var currentActualRotation = mRobotState.getPose2d().getRotation(); + var currentActualRotation = mRobotState.getHeading(); mTargetFieldRotation = currentActualRotation.plus(mTargetRelativeRotation); var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity(); mRotationState = new TrapezoidProfile.State( @@ -136,7 +136,7 @@ public boolean isFinished() { } public boolean atTargetPose() { - var currentRotation = mRobotState.getPose2d().getRotation(); + var currentRotation = mRobotState.getHeading(); var rotationError = (mTargetRelativeRotation.plus(mRotationalOffset)).minus(currentRotation); return Util.inRange(rotationError.getRadians(), kRotationToleranceRadians.get()) diff --git a/src/main/java/com/team1701/robot/commands/Shoot.java b/src/main/java/com/team1701/robot/commands/Shoot.java index 91a90645..366af599 100644 --- a/src/main/java/com/team1701/robot/commands/Shoot.java +++ b/src/main/java/com/team1701/robot/commands/Shoot.java @@ -5,11 +5,10 @@ import com.team1701.robot.states.RobotState; import com.team1701.robot.subsystems.drive.Drive; import com.team1701.robot.subsystems.shooter.Shooter; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; +import org.littletonrobotics.junction.AutoLogOutput; public class Shoot extends Command { private final Shooter mShooter; @@ -17,31 +16,33 @@ public class Shoot extends Command { private final RobotState mRobotState = new RobotState(); + @AutoLogOutput(key = "Commands/Shoot/calculatedDistanceToTarget") private double distanceToTarget; - private Rotation2d headingToTarget; + + @AutoLogOutput(key = "Commands/Shoot/calculatedShooterAngleFromHorizontal") private Rotation2d shooterAngleFromHorizontal; - private Pose2d currentPose; - private final Translation2d targetSpeaker; + + @AutoLogOutput(key = "Commands/Shoot/robotRelativeRotationDemand") private Rotation2d robotRelativeRotationDemand; + + @AutoLogOutput(key = "Commands/Shoot/shotFired") private boolean shotFired; public Shoot(Shooter shooter, Drive drive, boolean allianceIsBlue) { mShooter = shooter; mDrive = drive; - targetSpeaker = allianceIsBlue - ? FieldConstants.kBlueSpeakerOpeningCenter.toTranslation2d() - : FieldConstants.kRedSpeakerOpeningCenter.toTranslation2d(); addRequirements(shooter, drive); } @Override public void initialize() { shotFired = false; - currentPose = mRobotState.getPose2d(); - distanceToTarget = currentPose.getTranslation().getDistance(targetSpeaker); - headingToTarget = - new Rotation2d(targetSpeaker.getX() - currentPose.getX(), targetSpeaker.getY() - currentPose.getY()); - robotRelativeRotationDemand = headingToTarget.minus(currentPose.getRotation()); + distanceToTarget = mRobotState + .getPose2d() + .getTranslation() + .getDistance(mRobotState.getSpeakerPose().toTranslation2d()); + robotRelativeRotationDemand = + mRobotState.getSpeakerHeading().minus(mRobotState.getPose2d().getRotation()); shooterAngleFromHorizontal = new Rotation2d( distanceToTarget - Constants.Shooter.kShooterAxisOffset, FieldConstants.kBlueSpeakerOpeningCenter.getZ() - Constants.Shooter.kShooterAxisHeight); @@ -50,14 +51,13 @@ public void initialize() { @Override public void execute() { - if (currentPose.getRotation() != headingToTarget) { + if (mRobotState.getPose2d().getRotation() != mRobotState.getSpeakerHeading()) { DriveCommands.rotateRelativeToRobot( mDrive, robotRelativeRotationDemand, Constants.Drive.kFastKinematicLimits, true); } - // TODO add throughBoreEncoder + mShooter.setRotationAngle(shooterAngleFromHorizontal); - // TODO: add check for value change mShooter.setRollerSpeed(Units.rotationsPerMinuteToRadiansPerSecond(Constants.Motors.kMaxKrakenRPM)); // TODO: implement autostart/stop with sensors diff --git a/src/main/java/com/team1701/robot/subsystems/shooter/Shooter.java b/src/main/java/com/team1701/robot/subsystems/shooter/Shooter.java index cf583b09..58d6ddb6 100644 --- a/src/main/java/com/team1701/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/team1701/robot/subsystems/shooter/Shooter.java @@ -2,19 +2,19 @@ import java.util.function.Supplier; -import com.team1701.lib.drivers.digitalinputs.DigitalIO; import com.team1701.lib.drivers.digitalinputs.DigitalIOSim; -import com.team1701.lib.drivers.digitalinputs.DigitalInputsAutoLogged; import com.team1701.lib.drivers.encoders.EncoderIO; import com.team1701.lib.drivers.encoders.EncoderIOSim; import com.team1701.lib.drivers.encoders.EncoderInputsAutoLogged; import com.team1701.lib.drivers.motors.MotorIO; import com.team1701.lib.drivers.motors.MotorIOSim; import com.team1701.lib.drivers.motors.MotorInputsAutoLogged; +import com.team1701.lib.util.TimeLockedBoolean; import com.team1701.robot.Constants; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -23,23 +23,18 @@ public class Shooter extends SubsystemBase { private MotorIO mRotationShooterMotorIO; private EncoderIO mThroughBoreEncoderIO; - private DigitalIO mEntranceSensorIO; - private DigitalIO mExitSensorIO; + + private final double mInitialShooterAngleRadians; private final MotorInputsAutoLogged mUpperShooterMotorInputsAutoLogged = new MotorInputsAutoLogged(); private final MotorInputsAutoLogged mLowerShooterMotorInputsAutoLogged = new MotorInputsAutoLogged(); private final MotorInputsAutoLogged mRotationShooterMotorInputsAutoLogged = new MotorInputsAutoLogged(); - private final DigitalInputsAutoLogged mEntranceSensorInputsAutoLogged = new DigitalInputsAutoLogged(); - private final DigitalInputsAutoLogged mExitSensorInputsAutoLogged = new DigitalInputsAutoLogged(); private final EncoderInputsAutoLogged mEncoderInputsAutoLogged = new EncoderInputsAutoLogged(); - public Shooter( - MotorIO upperMotor, - MotorIO lowerMotor, - MotorIO rotationMotor, - DigitalIO entranceSensor, - DigitalIO exitSensor, - EncoderIO throughBoreEncoder) { + @AutoLogOutput(key = "Shooter/HasPiece") + private TimeLockedBoolean mShooterHasPiece; + + public Shooter(MotorIO upperMotor, MotorIO lowerMotor, MotorIO rotationMotor, EncoderIO throughBoreEncoder) { mUpperShooterMotorIO = upperMotor; mLowerShooterMotorIO = lowerMotor; @@ -61,9 +56,9 @@ public Shooter( 0, Constants.Shooter.kRotationKd.get()); - mEntranceSensorIO = entranceSensor; - mExitSensorIO = exitSensor; mThroughBoreEncoderIO = throughBoreEncoder; + + mInitialShooterAngleRadians = mEncoderInputsAutoLogged.position.getRadians(); } public static MotorIOSim createMotorSim(DCMotor shooterMotor) { @@ -85,16 +80,12 @@ public void periodic() { mLowerShooterMotorIO.updateInputs(mLowerShooterMotorInputsAutoLogged); mRotationShooterMotorIO.updateInputs(mRotationShooterMotorInputsAutoLogged); - mEntranceSensorIO.updateInputs(mEntranceSensorInputsAutoLogged); - mExitSensorIO.updateInputs(mExitSensorInputsAutoLogged); mThroughBoreEncoderIO.updateInputs(mEncoderInputsAutoLogged); Logger.processInputs("Shooter/Motors/Rollers/Upper", mUpperShooterMotorInputsAutoLogged); Logger.processInputs("Shooter/Motors/Rollers/Lower", mLowerShooterMotorInputsAutoLogged); Logger.processInputs("Shooter/Motors/Rotation", mRotationShooterMotorInputsAutoLogged); - Logger.processInputs("Shooter/DigitalSensors/Entrance", mEntranceSensorInputsAutoLogged); - Logger.processInputs("Shooter/DigitalSensors/Exit", mExitSensorInputsAutoLogged); Logger.processInputs("Shooter/Encoder", mRotationShooterMotorInputsAutoLogged); if (Constants.Shooter.kRollerKff.hasChanged(hash) @@ -134,8 +125,12 @@ public void setRollerSpeed(double radiansPerSecond) { } public void setRotationAngle(Rotation2d rotation) { - Logger.recordOutput("Shooter/Motors/Rotation/Demand", rotation); - mRotationShooterMotorIO.setPositionControl(rotation); + var motorRotationDemand = rotation.minus(new Rotation2d(mInitialShooterAngleRadians)); + + Logger.recordOutput("Shooter/Motors/Rotation/RawDemand", rotation); + Logger.recordOutput("Shooter/Motors/Rotation/CalculatedDemand", motorRotationDemand); + + mRotationShooterMotorIO.setPositionControl(motorRotationDemand); } public void stopRollers() { From 6932da6d11a591712b94a5a0b37b7eaf3dc2f8dc Mon Sep 17 00:00:00 2001 From: Daniel Burke Date: Mon, 29 Jan 2024 17:41:57 -0500 Subject: [PATCH 2/2] Consolidated Spark Flex Motor Factories --- .../com/team1701/robot/RobotContainer.java | 32 ++++----- ...actory.java => SparkFlexMotorFactory.java} | 52 +++++++++++++- .../shooter/ShooterMotorFactory.java | 69 ------------------- 3 files changed, 64 insertions(+), 89 deletions(-) rename src/main/java/com/team1701/robot/{subsystems/drive/DriveMotorFactory.java => SparkFlexMotorFactory.java} (62%) delete mode 100644 src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java index 8c6b3b17..d0721847 100644 --- a/src/main/java/com/team1701/robot/RobotContainer.java +++ b/src/main/java/com/team1701/robot/RobotContainer.java @@ -20,15 +20,13 @@ import com.team1701.lib.util.GeometryUtil; import com.team1701.lib.util.LoggedTunableNumber; import com.team1701.robot.Configuration.Mode; +import com.team1701.robot.SparkFlexMotorFactory.ShooterMotorUsage; import com.team1701.robot.commands.AutonomousCommands; import com.team1701.robot.commands.DriveCommands; import com.team1701.robot.states.RobotState; import com.team1701.robot.subsystems.drive.Drive; -import com.team1701.robot.subsystems.drive.DriveMotorFactory; import com.team1701.robot.subsystems.drive.SwerveModule.SwerveModuleIO; import com.team1701.robot.subsystems.shooter.Shooter; -import com.team1701.robot.subsystems.shooter.ShooterMotorFactory; -import com.team1701.robot.subsystems.shooter.ShooterMotorFactory.ShooterMotorUsage; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; @@ -65,31 +63,31 @@ public RobotContainer() { new GyroIOPigeon2(10), new SwerveModuleIO[] { new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(10), - DriveMotorFactory.createSteerMotorIOSparkMax(11), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(10), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(11), new EncoderIOAnalog(0)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(12), - DriveMotorFactory.createSteerMotorIOSparkMax(13), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(12), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(13), new EncoderIOAnalog(1)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(16), - DriveMotorFactory.createSteerMotorIOSparkMax(17), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(16), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(17), new EncoderIOAnalog(3)), new SwerveModuleIO( - DriveMotorFactory.createDriveMotorIOSparkMax(14), - DriveMotorFactory.createSteerMotorIOSparkMax(15), + SparkFlexMotorFactory.createDriveMotorIOSparkMax(14), + SparkFlexMotorFactory.createSteerMotorIOSparkMax(15), new EncoderIOAnalog(2)), }, mRobotState)); // TODO: update IDs shooter = Optional.of(new Shooter( - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); break; @@ -138,11 +136,11 @@ public RobotContainer() { new AprilTagCameraIO() {})); */ this.mShooter = shooter.orElseGet(() -> new Shooter( - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER), - ShooterMotorFactory.createShooterMotorIOSparkFlex( + SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); diff --git a/src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java b/src/main/java/com/team1701/robot/SparkFlexMotorFactory.java similarity index 62% rename from src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java rename to src/main/java/com/team1701/robot/SparkFlexMotorFactory.java index 279fb507..db541dad 100644 --- a/src/main/java/com/team1701/robot/subsystems/drive/DriveMotorFactory.java +++ b/src/main/java/com/team1701/robot/SparkFlexMotorFactory.java @@ -1,15 +1,56 @@ -package com.team1701.robot.subsystems.drive; +package com.team1701.robot; import java.util.function.Supplier; +import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.REVLibError; import com.team1701.lib.alerts.REVAlert; +import com.team1701.lib.drivers.motors.MotorIOSparkFlex; import com.team1701.lib.drivers.motors.MotorIOSparkMax; -import com.team1701.robot.Constants; -public final class DriveMotorFactory { +public class SparkFlexMotorFactory { + public static MotorIOSparkFlex createShooterMotorIOSparkFlex(int deviceId, ShooterMotorUsage motorUse) { + var motor = new CANSparkFlex(deviceId, MotorType.kBrushless); + var encoder = motor.getEncoder(); + var controller = motor.getPIDController(); + var errorAlert = new REVAlert(motor, deviceId); + + motor.setCANTimeout(200); + + // TODO: Update values for actual shooter + configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert); + + configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert); + configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert); + + configureWithRetry(() -> encoder.setPosition(0), errorAlert); + configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert); + configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert); + + switch (motorUse) { + case ROLLER: + configureWithRetry(() -> controller.setP(Constants.Shooter.kRollerKp.get()), errorAlert); + configureWithRetry(() -> controller.setD(Constants.Shooter.kRollerKd.get()), errorAlert); + configureWithRetry(() -> controller.setFF(Constants.Shooter.kRollerKff.get()), errorAlert); + break; + case ROTATION: + configureWithRetry(() -> controller.setP(Constants.Shooter.kRotationKp.get()), errorAlert); + configureWithRetry(() -> controller.setD(Constants.Shooter.kRotationKd.get()), errorAlert); + configureWithRetry(() -> controller.setFF(Constants.Shooter.kRotationKff.get()), errorAlert); + break; + default: + break; + } + + configureWithRetry(() -> motor.burnFlash(), errorAlert); + + motor.setCANTimeout(0); + + return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction); + } + public static MotorIOSparkMax createDriveMotorIOSparkMax(int deviceId) { var motor = new CANSparkMax(deviceId, MotorType.kBrushless); var encoder = motor.getEncoder(); @@ -83,4 +124,9 @@ private static void configureWithRetry(Supplier config, REVAlert fa failureAlert.enable(error); } + + public enum ShooterMotorUsage { + ROLLER, + ROTATION + } } diff --git a/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java b/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java deleted file mode 100644 index 5d9da214..00000000 --- a/src/main/java/com/team1701/robot/subsystems/shooter/ShooterMotorFactory.java +++ /dev/null @@ -1,69 +0,0 @@ -package com.team1701.robot.subsystems.shooter; - -import java.util.function.Supplier; - -import com.revrobotics.CANSparkFlex; -import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.REVLibError; -import com.team1701.lib.alerts.REVAlert; -import com.team1701.lib.drivers.motors.MotorIOSparkFlex; -import com.team1701.robot.Constants; - -public class ShooterMotorFactory { - public static MotorIOSparkFlex createShooterMotorIOSparkFlex(int deviceId, ShooterMotorUsage motorUse) { - var motor = new CANSparkFlex(deviceId, MotorType.kBrushless); - var encoder = motor.getEncoder(); - var controller = motor.getPIDController(); - var errorAlert = new REVAlert(motor, deviceId); - - motor.setCANTimeout(200); - - // TODO: Update values for actual shooter - configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert); - - configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert); - configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert); - - configureWithRetry(() -> encoder.setPosition(0), errorAlert); - configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert); - configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert); - - switch (motorUse) { - case ROLLER: - configureWithRetry(() -> controller.setP(Constants.Shooter.kRollerKp.get()), errorAlert); - configureWithRetry(() -> controller.setD(Constants.Shooter.kRollerKd.get()), errorAlert); - configureWithRetry(() -> controller.setFF(Constants.Shooter.kRollerKff.get()), errorAlert); - break; - case ROTATION: - configureWithRetry(() -> controller.setP(Constants.Shooter.kRotationKp.get()), errorAlert); - configureWithRetry(() -> controller.setD(Constants.Shooter.kRotationKd.get()), errorAlert); - configureWithRetry(() -> controller.setFF(Constants.Shooter.kRotationKff.get()), errorAlert); - break; - default: - break; - } - - configureWithRetry(() -> motor.burnFlash(), errorAlert); - - motor.setCANTimeout(0); - - return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction); - } - - private static void configureWithRetry(Supplier config, REVAlert failureAlert) { - REVLibError error = REVLibError.kUnknown; - for (var i = 0; i < 4; i++) { - error = config.get(); - if (error == REVLibError.kOk) { - return; - } - } - - failureAlert.enable(error); - } - - public enum ShooterMotorUsage { - ROLLER, - ROTATION - } -}