diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7d577f4..34c5163 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,20 +63,36 @@ public static class CoralConstants { } public static class ShooterConstants { - // 2 Neos, 4in shooter wheels, 4:1 gearbox reduction + // 2 Neos, 4in shooter wheels public static final int kLeaderMotorId = 15; public static final int kFollowerMotorId = 16; } public static class TurretConstants { - // 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing, non-continuous (270 - // FOV) - public static final int kLeaderMotorId = 17; - public static final int kFollowerMotorId = 18; + // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous + // 360 deg + public static final int kMotorId = 17; } public static class HoodConstants { // 1 Neo, 0-90 degree variability, 50:1 reduction public static final int kMotorId = 19; } + + // Intake subsystem CAN IDs start at 30 + public static class IntakeConstants { + // SparkFlex controlling the intake flywheel + public static final int kPivotMotorId = 30; + public static final int kRollerMotorId = 31; + } + + // Hopper subsystem CAN IDs start at 40 + public static class HopperConstants { + public static final int kHopperMotorId = 40; + } + + // Kicker subsystem CAN IDs start at 50 + public static class KickerConstants { + public static final int kKickerMotorId = 50; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7f2bff9..1869db3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,12 +17,24 @@ import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; import frc.robot.controls.PoseControls; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.KickerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; import swervelib.SwerveDrive; public class RobotContainer { private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve")); + private final IntakeSubsystem intake = new IntakeSubsystem(); + private final HopperSubsystem hopper = new HopperSubsystem(); + private final KickerSubsystem kicker = new KickerSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); + + private final Superstructure superstructure = new Superstructure(shooter, null, null, intake, hopper, kicker); + private final SendableChooser autoChooser; /** @@ -53,8 +65,8 @@ public RobotContainer() { private void configureBindings() { // Set up controllers - DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null); - OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null); + DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, superstructure); + OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, superstructure); PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase); } diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java index 2b181b9..05d77ff 100644 --- a/src/main/java/frc/robot/controls/DriverControls.java +++ b/src/main/java/frc/robot/controls/DriverControls.java @@ -88,6 +88,7 @@ public static void configure(int port, SwerveSubsystem drivetrain, Superstructur } else if (Robot.isSimulation()) { controller.back().whileTrue(fireAlgae(drivetrain)); } else { + controller.y().onTrue((Commands.runOnce(drivetrain::zeroGyro))); controller.leftBumper().whileTrue(Commands.runOnce(drivetrain::lock, drivetrain).repeatedly()); } } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index ef4c3bd..bc0dd60 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -1,5 +1,7 @@ package frc.robot.controls; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.SwerveSubsystem; @@ -8,5 +10,30 @@ public class OperatorControls { public static void configure(int port, SwerveSubsystem drivetrain, Superstructure superstructure) { CommandXboxController controller = new CommandXboxController(port); + + // Intake controls - A to intake, B to eject + controller.a().whileTrue(superstructure.intakeCommand()); + controller.b().whileTrue(superstructure.ejectCommand()); + + // Hopper controls - X to run hopper forward, Y to run backward + controller.x().whileTrue(superstructure.hopperFeedCommand()); + controller.y().whileTrue(superstructure.hopperReverseCommand()); + + // Shooter controls - Right bumper to shoot + controller.rightBumper().whileTrue(superstructure.shootCommand()); + controller.leftBumper().whileTrue(superstructure.stopShootingCommand()); + + // Kicker controls + controller.back().whileTrue(superstructure.kickerFeedCommand()); + controller.start().whileTrue(superstructure.kickerStopCommand()); + + // Intake pivot controls. Setpoints need to be tested and finalized. + + // 0 for default + // -45 for collection + // +25 just because. We can add more setpoints if necessary. + // controller.povUp().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(25))); + // controller.povRight().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(0))); + // controller.povDown().whileTrue(superstructure.setIntakePivotAngle(Degrees.of(-45))); } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java new file mode 100644 index 0000000..3a39fcb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -0,0 +1,84 @@ +package frc.robot.subsystems; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class HopperSubsystem extends SubsystemBase { + + private static final double HOPPER_SPEED = 1.0; + + // Nova motor controller with NEO motor + private ThriftyNova hopperNova = new ThriftyNova(Constants.HopperConstants.kHopperMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("HopperMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction + .withMotorInverted(false) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(40)); + + private SmartMotorController smc = new NovaWrapper(hopperNova, DCMotor.getNEO(1), smcConfig); + + private final FlyWheelConfig hopperConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Hopper", TelemetryVerbosity.HIGH); + + private FlyWheel hopper = new FlyWheel(hopperConfig); + + public HopperSubsystem() { + } + + /** + * Command to run the hopper forward while held. + */ + public Command feedCommand() { + return hopper.set(HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Feed"); + } + + /** + * Command to run the hopper in reverse while held. + */ + public Command reverseCommand() { + return hopper.set(-HOPPER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Hopper.Reverse"); + } + + /** + * Command to stop the hopper. + */ + public Command stopCommand() { + return hopper.set(0).withName("Hopper.Stop"); + } + + @Override + public void periodic() { + hopper.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + hopper.simIterate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..e2be8e8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,126 @@ +package frc.robot.subsystems; + +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.DegreesPerSecond; +import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond; +import static edu.wpi.first.units.Units.Feet; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.ArmConfig; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.positional.Arm; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class IntakeSubsystem extends SubsystemBase { + + private static final double INTAKE_SPEED = 0.1; + + // ThriftyNova controlling the intake roller + private ThriftyNova rollerNova = new ThriftyNova(Constants.IntakeConstants.kRollerMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("IntakeMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) // Direct drive, adjust if geared + .withMotorInverted(false) + .withIdleMode(MotorMode.COAST) + .withStatorCurrentLimit(Amps.of(40)); + + private SmartMotorController smc = new NovaWrapper(rollerNova, DCMotor.getNeoVortex(1), smcConfig); + + private final FlyWheelConfig intakeConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Intake", TelemetryVerbosity.HIGH); + + private FlyWheel intake = new FlyWheel(intakeConfig); + + // 5:1, 5:1, 60/18 reduction + private SmartMotorControllerConfig intakePivotSmartMotorConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.CLOSED_LOOP) + .withClosedLoopController(200, 0, 0, DegreesPerSecond.of(360), DegreesPerSecondPerSecond.of(360)) + .withFeedforward(new ArmFeedforward(0, 0, 0.1)) + .withTelemetry("IntakePivotMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(5, 5, 60.0 / 18))) + .withMotorInverted(true) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(10)) + .withClosedLoopRampRate(Seconds.of(0.1)); + + private ThriftyNova pivotMotor = new ThriftyNova(Constants.IntakeConstants.kPivotMotorId); + + private SmartMotorController intakePivotController = new NovaWrapper(pivotMotor, DCMotor.getNeoVortex(1), + intakePivotSmartMotorConfig); + + private final ArmConfig intakePivotConfig = new ArmConfig(intakePivotController) + .withSoftLimits(Degrees.of(-95), Degrees.of(45)) // TODO: Find and set proper limits once setpoints and range is + // known + .withHardLimit(Degrees.of(-100), Degrees.of(50)) + .withStartingPosition(Degrees.of(-90)) + .withLength(Feet.of(1)) + .withMass(Pounds.of(2)) // Reis says: 2 pounds, not a lot + .withTelemetry("IntakePivot", TelemetryVerbosity.HIGH); + + private Arm intakePivot = new Arm(intakePivotConfig); + + public IntakeSubsystem() { + } + + /** + * Command to run the intake while held. + */ + public Command intakeCommand() { + return intake.set(INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Run"); + } + + /** + * Command to eject while held. + */ + public Command ejectCommand() { + return intake.set(-INTAKE_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Intake.Eject"); + } + + public Command setPivotAngle(Angle angle) { + return intakePivot.setAngle(angle).withName("IntakePivot.SetAngle"); + } + + public Command rezero() { + return Commands.runOnce(() -> pivotMotor.setEncoderPosition(0), this).withName("IntakePivot.Rezero"); + } + + @Override + public void periodic() { + intake.updateTelemetry(); + intakePivot.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + intake.simIterate(); + intakePivot.simIterate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/KickerSubsystem.java b/src/main/java/frc/robot/subsystems/KickerSubsystem.java new file mode 100644 index 0000000..477a1bf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/KickerSubsystem.java @@ -0,0 +1,76 @@ +package frc.robot.subsystems; + +import com.thethriftybot.ThriftyNova; + +import edu.wpi.first.math.system.plant.DCMotor; +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import yams.gearing.GearBox; +import yams.gearing.MechanismGearing; +import yams.mechanisms.config.FlyWheelConfig; +import yams.mechanisms.velocity.FlyWheel; +import yams.motorcontrollers.SmartMotorController; +import yams.motorcontrollers.SmartMotorControllerConfig; +import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; +import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; +import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; +import yams.motorcontrollers.local.NovaWrapper; + +public class KickerSubsystem extends SubsystemBase { + + private static final double KICKER_SPEED = 1.0; + + // Nova motor controller with NEO motor + private ThriftyNova kickerNova = new ThriftyNova(Constants.KickerConstants.kKickerMotorId); + + private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withControlMode(ControlMode.OPEN_LOOP) + .withTelemetry("KickerMotor", TelemetryVerbosity.HIGH) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) // 4:1 gear reduction + .withMotorInverted(false) + .withIdleMode(MotorMode.BRAKE) + .withStatorCurrentLimit(Amps.of(20)); + + private SmartMotorController smc = new NovaWrapper(kickerNova, DCMotor.getNEO(1), smcConfig); + + private final FlyWheelConfig kickerConfig = new FlyWheelConfig(smc) + .withDiameter(Inches.of(4)) + .withMass(Pounds.of(0.5)) + .withUpperSoftLimit(RPM.of(6000)) + .withLowerSoftLimit(RPM.of(-6000)) + .withTelemetry("Kicker", TelemetryVerbosity.HIGH); + + private FlyWheel kicker = new FlyWheel(kickerConfig); + + public KickerSubsystem() { + } + + /** + * Command to run the kicker forward while held, stops when released. + */ + public Command feedCommand() { + return kicker.set(KICKER_SPEED).finallyDo(() -> smc.setDutyCycle(0)).withName("Kicker.Feed"); + } + + /** + * Command to stop the kicker. + */ + public Command stopCommand() { + return kicker.set(0).withName("Kicker.Stop"); + } + + @Override + public void periodic() { + kicker.updateTelemetry(); + } + + @Override + public void simulationPeriodic() { + kicker.simIterate(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index da43d57..8ac75de 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import com.thethriftybot.ThriftyNova; import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -27,35 +26,35 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; +import yams.motorcontrollers.local.NovaWrapper; public class ShooterSubsystem extends SubsystemBase { - // 2 Neos, 4in shooter wheels, 4:1 gearbox reduction - private SparkMax leaderSpark = new SparkMax(Constants.ShooterConstants.kLeaderMotorId, MotorType.kBrushless); - private SparkMax followerSpark = new SparkMax(Constants.ShooterConstants.kFollowerMotorId, MotorType.kBrushless); + // 2 Neos, 4in shooter wheels + private final ThriftyNova leaderNova = new ThriftyNova(Constants.ShooterConstants.kLeaderMotorId); + private final ThriftyNova followerNova = new ThriftyNova(Constants.ShooterConstants.kFollowerMotorId); - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withFollowers(Pair.of(followerSpark, false)) + private final SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) + .withFollowers(Pair.of(followerNova, false)) .withControlMode(ControlMode.CLOSED_LOOP) .withClosedLoopController(0.1, 0, 0) .withFeedforward(new SimpleMotorFeedforward(0, 0.5, 0)) .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(4))) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) .withMotorInverted(false) .withIdleMode(MotorMode.COAST) .withStatorCurrentLimit(Amps.of(40)); - private SmartMotorController smc = new SparkWrapper(leaderSpark, DCMotor.getNEO(2), smcConfig); + private final SmartMotorController smc = new NovaWrapper(leaderNova, DCMotor.getNEO(2), smcConfig); private final FlyWheelConfig shooterConfig = new FlyWheelConfig(smc) .withDiameter(Inches.of(4)) .withMass(Pounds.of(1)) - .withUpperSoftLimit(RPM.of(6000)) - .withLowerSoftLimit(RPM.of(0)) + .withUpperSoftLimit(RotationsPerSecond.of(6000)) + .withLowerSoftLimit(RotationsPerSecond.of(0)) .withTelemetry("Shooter", TelemetryVerbosity.HIGH); - private FlyWheel shooter = new FlyWheel(shooterConfig); + private final FlyWheel shooter = new FlyWheel(shooterConfig); public ShooterSubsystem() { } @@ -65,7 +64,7 @@ public Command setSpeed(AngularVelocity speed) { } public Command spinUp() { - return shooter.setSpeed(RotationsPerSecond.of(50)); + return shooter.setSpeed(RotationsPerSecond.of(500)); } public Command stop() { diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 9e6adce..b7daef5 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Superstructure coordinates the shooter, turret, and hood subsystems + * Superstructure coordinates the shooter, turret, hood, and intake subsystems * for unified control during shooting operations. */ public class Superstructure extends SubsystemBase { @@ -20,6 +20,9 @@ public class Superstructure extends SubsystemBase { private final ShooterSubsystem shooter; private final TurretSubsystem turret; private final HoodSubsystem hood; + private final IntakeSubsystem intake; + private final HopperSubsystem hopper; + private final KickerSubsystem kicker; // Default values for "ready" state private static final AngularVelocity DEFAULT_SHOOTER_SPEED = RPM.of(4000); @@ -41,10 +44,14 @@ public class Superstructure extends SubsystemBase { private Angle targetTurretAngle = Degrees.of(0); private Angle targetHoodAngle = Degrees.of(0); - public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood) { + public Superstructure(ShooterSubsystem shooter, TurretSubsystem turret, HoodSubsystem hood, IntakeSubsystem intake, + HopperSubsystem hopper, KickerSubsystem kicker) { this.shooter = shooter; this.turret = turret; this.hood = hood; + this.intake = intake; + this.hopper = hopper; + this.kicker = kicker; // Create triggers for checking if mechanisms are at their targets this.isShooterAtSpeed = new Trigger( @@ -188,6 +195,74 @@ public Angle getTargetHoodAngle() { return targetHoodAngle; } + /** + * Command to run the intake while held. + */ + public Command intakeCommand() { + return intake.intakeCommand().withName("Superstructure.intake"); + } + + /** + * Command to eject while held. + */ + public Command ejectCommand() { + return intake.ejectCommand().withName("Superstructure.eject"); + } + + /** + * Command to run the hopper forward while held. + */ + public Command hopperFeedCommand() { + return hopper.feedCommand().withName("Superstructure.feed"); + } + + /** + * Command to run the hopper in reverse while held. + */ + public Command hopperReverseCommand() { + return hopper.reverseCommand().withName("Superstructure.hopperReverse"); + } + + /** + * Command to run the kicker forward while held, stops when released. + */ + public Command kickerFeedCommand() { + return kicker.feedCommand().withName("Superstructure.kickerFeed"); + } + + /** + * Command to run the kicker stop while held, stops when released. + */ + public Command kickerStopCommand() { + return kicker.stopCommand().withName("Superstructure.kickerStop"); + } + + /** + * Command to set the intake pivot angle. + */ + public Command setIntakePivotAngle(Angle angle) { + return intake.setPivotAngle(angle).withName("Superstructure.setIntakePivotAngle"); + } + + /** + * Command to shoot - spins up shooter. + */ + public Command shootCommand() { + return shooter.spinUp().withName("Superstructure.shoot"); + } + + /** + * Command to stop shooting - stops shooter. + */ + public Command stopShootingCommand() { + return shooter.stop().withName("Superstructure.stopShooting"); + } + + // Re-zero intake pivot if needed + public Command rezeroIntakePivotCommand() { + return Commands.runOnce(() -> intake.rezero(), intake).withName("Superstructure.rezeroIntakePivot"); + } + @Override public void periodic() { // Superstructure doesn't need periodic updates - subsystems handle their own diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 01e044b..82ab632 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -76,6 +76,8 @@ public class SwerveSubsystem extends SubsystemBase { private Limelight limelight; private LimelightPoseEstimator poseEstimator; + private final boolean IS_LIMELIGHT_ENABLED = false; + /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -129,50 +131,53 @@ public SwerveSubsystem(File directory) { SwerveIMU gyro = swerveDrive.getGyro(); - limelight = new Limelight("limelight"); - limelight.getSettings() - .withLimelightLEDMode(LEDMode.PipelineControl) - .withCameraOffset(new Pose3d( - Inches.of(14.00).in(Meters), - Inches.of(4.750).in(Meters), - Inches.of(17.25).in(Meters), - Rotation3d.kZero)) - .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.01) - .save(); - - RobotModeTriggers.disabled().onTrue(Commands.runOnce(() -> { - System.out.println("Setting LL IMU Assist Alpha to 0.001"); - + if (IS_LIMELIGHT_ENABLED) { + limelight = new Limelight("limelight"); limelight.getSettings() - // .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.001) + .withLimelightLEDMode(LEDMode.PipelineControl) + .withCameraOffset(new Pose3d( + Inches.of(14.00).in(Meters), + Inches.of(4.750).in(Meters), + Inches.of(17.25).in(Meters), + Rotation3d.kZero)) + .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.01) .save(); - }).ignoringDisable(true)); - Command onEnable = Commands.runOnce(() -> { - System.out.println("Setting LL IMU Assist Alpha to 0.01"); + RobotModeTriggers.disabled().onTrue(Commands.runOnce(() -> { + System.out.println("Setting LL IMU Assist Alpha to 0.001"); + + limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.001) + .save(); + }).ignoringDisable(true)); + + Command onEnable = Commands.runOnce(() -> { + System.out.println("Setting LL IMU Assist Alpha to 0.01"); + + limelight.getSettings() + // .withImuMode(ImuMode.InternalImuMT1Assist) + .withImuAssistAlpha(0.01) + .save(); + }); + + RobotModeTriggers.teleop().onTrue(onEnable); + RobotModeTriggers.autonomous().onTrue(onEnable); + RobotModeTriggers.test().onTrue(onEnable); + // Required for megatag2 in periodic() function before fetching pose. limelight.getSettings() - // .withImuMode(ImuMode.InternalImuMT1Assist) - .withImuAssistAlpha(0.01) + .withRobotOrientation( + new Orientation3d(gyro.getRotation3d(), + new AngularVelocity3d( + DegreesPerSecond.of(0), + DegreesPerSecond.of(0), + DegreesPerSecond.of(0)))) .save(); - }); - RobotModeTriggers.teleop().onTrue(onEnable); - RobotModeTriggers.autonomous().onTrue(onEnable); - RobotModeTriggers.test().onTrue(onEnable); - - // Required for megatag2 in periodic() function before fetching pose. - limelight.getSettings() - .withRobotOrientation( - new Orientation3d(gyro.getRotation3d(), - new AngularVelocity3d( - DegreesPerSecond.of(0), - DegreesPerSecond.of(0), - DegreesPerSecond.of(0)))) - .save(); - - poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2); + + poseEstimator = limelight.createPoseEstimator(EstimationMode.MEGATAG2); + } setupPathPlanner(); } @@ -181,27 +186,32 @@ public SwerveSubsystem(File directory) { public void periodic() { // swerveDrive.updateOdometry(); // TODO: see if this is needed - // // Get MegaTag2 pose - Optional visionEstimate = poseEstimator.getPoseEstimate(); + if (IS_LIMELIGHT_ENABLED) { + // Get MegaTag2 pose + Optional visionEstimate = poseEstimator.getPoseEstimate(); - // If the pose is present - visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { - if (poseEstimate.tagCount > 0) { - Logger.recordOutput("Limelight/Megatag2Count", poseEstimate.tagCount); + // If the pose is present + visionEstimate.ifPresent((PoseEstimate poseEstimate) -> { + if (poseEstimate.tagCount > 0) { + Logger.recordOutput("Limelight/Megatag2Count", poseEstimate.tagCount); - Logger.recordOutput("FieldSimulation/LLPose", poseEstimate.pose); + Logger.recordOutput("FieldSimulation/LLPose", poseEstimate.pose); - // Add it to the pose estimator. - swerveDrive.addVisionMeasurement( - poseEstimate.pose.toPose2d(), - poseEstimate.timestampSeconds); - // TODO: possibly add stddevs here - // TODO: Instead of providing the limelight's pose as is, replace the rotation - // component with the current pose rotation so the process doesn't take it into - // account? + // Add it to the pose estimator. + swerveDrive.addVisionMeasurement( + poseEstimate.pose.toPose2d(), + poseEstimate.timestampSeconds); - } - }); + // TODO: possibly add stddevs here + // TODO: Instead of providing the limelight's pose as is, replace the + // rotation + // component with the current pose rotation so the process doesn't take it + // into + // account? + + } + }); + } } @Override diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 4cdf9e1..c393aec 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -1,9 +1,7 @@ package frc.robot.subsystems; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; +import com.thethriftybot.ThriftyNova; -import edu.wpi.first.math.Pair; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.system.plant.DCMotor; import static edu.wpi.first.units.Units.Amps; @@ -26,34 +24,34 @@ import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.local.SparkWrapper; +import yams.motorcontrollers.local.NovaWrapper; public class TurretSubsystem extends SubsystemBase { - // 2 Neos, 12in diameter, 25:1 gearbox, 10:1 pivot gearing reduction, - // non-continuous (270 FOV) - // Total reduction: 25 * 10 = 250:1 - private SparkMax leaderSpark = new SparkMax(Constants.TurretConstants.kLeaderMotorId, MotorType.kBrushless); - private SparkMax followerSpark = new SparkMax(Constants.TurretConstants.kFollowerMotorId, MotorType.kBrushless); + private final double MAX_ONE_DIR_FOV = 45; // degrees + + // 1 Neo, 6.875 in diameter, 4:1 gearbox, 10:1 pivot gearing, non-continuous + // 360 deg + // Total reduction: 4 * 10 = 40:1 + private ThriftyNova nova = new ThriftyNova(Constants.TurretConstants.kMotorId); private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withFollowers(Pair.of(followerSpark, false)) .withControlMode(ControlMode.CLOSED_LOOP) .withClosedLoopController(100, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(180)) .withFeedforward(new ArmFeedforward(0, 0, 0.1)) .withTelemetry("TurretMotor", TelemetryVerbosity.HIGH) - .withGearing(new MechanismGearing(GearBox.fromReductionStages(25, 10))) + .withGearing(new MechanismGearing(GearBox.fromReductionStages(4, 10))) .withMotorInverted(false) .withIdleMode(MotorMode.BRAKE) - .withSoftLimit(Degrees.of(-135), Degrees.of(135)) // 270 FOV, centered at 0 + .withSoftLimit(Degrees.of(-MAX_ONE_DIR_FOV), Degrees.of(MAX_ONE_DIR_FOV)) .withStatorCurrentLimit(Amps.of(40)) .withClosedLoopRampRate(Seconds.of(0.1)) .withOpenLoopRampRate(Seconds.of(0.1)); - private SmartMotorController smc = new SparkWrapper(leaderSpark, DCMotor.getNEO(2), smcConfig); + private SmartMotorController smc = new NovaWrapper(nova, DCMotor.getNEO(1), smcConfig); private final PivotConfig turretConfig = new PivotConfig(smc) - .withHardLimit(Degrees.of(-140), Degrees.of(140)) + .withHardLimit(Degrees.of(-MAX_ONE_DIR_FOV - 5), Degrees.of(MAX_ONE_DIR_FOV + 5)) .withStartingPosition(Degrees.of(0)) .withMOI(0.05) .withTelemetry("Turret", TelemetryVerbosity.HIGH);