From 5009913ea4f3acb60f511910baf763a304ef83c8 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Sun, 22 Mar 2026 16:03:07 -0400 Subject: [PATCH] More automation and new IDs --- lib/RoboLib | 2 +- src/main/java/frc/robot/Constants.java | 28 ++++--- src/main/java/frc/robot/RobotContainer.java | 3 - .../AutoShootCommandChassisTurret.java | 75 +++++++++++++++++++ .../robot/commands/ManualHopperCommand.java | 10 +-- .../frc/robot/subsystems/HopperSubsystem.java | 24 +++--- .../frc/robot/subsystems/IntakeSubsystem.java | 6 ++ .../frc/robot/subsystems/TurretSubsystem.java | 69 +++++++++++++---- 8 files changed, 166 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java diff --git a/lib/RoboLib b/lib/RoboLib index eea7693..eeccb75 160000 --- a/lib/RoboLib +++ b/lib/RoboLib @@ -1 +1 @@ -Subproject commit eea76939a3cbb76aba47874adf15eaf49a8333de +Subproject commit eeccb7531c6bddd4470d1e13804b42b279fdf7ee diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d2535bc..dcdc0d1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,11 +4,7 @@ package frc.robot; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Unit; import edu.wpi.first.units.measure.Angle; -import java.security.PublicKey; -import java.util.Queue; import com.ctre.phoenix6.signals.InvertedValue; @@ -25,14 +21,23 @@ public final class Constants { public static final int kDriverControllerPort = 0; } + public static class CameraConstanst { + public static final String BACK_CAMERA_NAME = "back"; + public static final int BACK_LOCALIZATION_PIPELINE_NUMBER = 0; + public static final int BACK_HUB_TRACKING_PIPELINE_NUMBER = 1; + + public static final String FRONT_CAMERA_NAME = "front"; + public static final int FRONT_LOCALIZATION_PIPELINE_NUMBER = 0; + + } + public static class AdvantageKitConstants{ public enum RobotType{ SIM, REAL, REPLAY } - public static final RobotType CURRENT_MODE = RobotType.REAL; - + public static final RobotType CURRENT_MODE = RobotType.REAL; } // Swerve offsets as of 3/2/26 public static class SwerveDriveOffsets { @@ -72,7 +77,7 @@ public final class Constants { public static final int DEPLOY_MOTOR_ID = 17; public static final boolean DEPLOY_MOTOR_INVERTED = false; // TODO: Test this - public static final int INTAKE_MOTOR_ID = 0; + public static final int INTAKE_MOTOR_ID = 2; public static final InvertedValue INTAKE_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; // TODO: Test this } @@ -80,7 +85,7 @@ public final class Constants { public static final int LAUNCHER_MOTOR_ID = 2; public static final boolean LAUNCHER_MOTOR_INVERTED = true; // TODO: Test this - public static final int TURRET_MOTOR_ID = 0; + public static final int TURRET_MOTOR_ID = 3; public static final InvertedValue TURRET_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int TURRET_LIMIT_CHANNEL = 0; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(265); @@ -88,7 +93,7 @@ public final class Constants { public static final double HOMING_SPEED = .35; public static final double TURRET_GEAR_RATIO = 0; - public static final int HOOD_SERVO_CHANNEL = 9; + public static final int HOOD_SERVO_CHANNEL = 0; public static final boolean HOOD_SERVO_INVERTED = true; // TODO: Test this public static final int HOOD_ENCODER_ID = 0; public static final Angle TARGET_ELEVATION_DIF = edu.wpi.first.units.Units.Degrees.of(.5); @@ -103,10 +108,9 @@ public final class Constants { public static class HopperConstants { public static final int KICKER_MOTOR_ID = 9; - public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; + public static final InvertedValue KICKER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; public static final int SPINDEXER_MOTOR_ID = 8; - public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.CounterClockwise_Positive; - public static final double SPINDEXER_MOTOR_SPEED = .6; + public static final InvertedValue SPINDEXER_MOTOR_DIRECTION = InvertedValue.Clockwise_Positive; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ec268ec..a254508 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,9 +79,6 @@ public class RobotContainer { m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); - m_driverController.b().onTrue(intakeSubsystem.revSpeedCommand()); - m_driverController.b().onTrue(hopperSubsystem.revSpeedCommand()); - m_driverController.b().onTrue(turretSubsystem.revSpeedCommand()); intakeSubsystem.setDefaultCommand(new ManualIntake(intakeSubsystem, m_driverController.getHID())); } diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java new file mode 100644 index 0000000..e25df57 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -0,0 +1,75 @@ +package frc.robot.commands; + +import java.lang.constant.Constable; + +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers; +import org.fairportrobotics.frc.robolib.vision.limelight.LimelightHelpers.LimelightTarget_Fiducial; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.TurretSubsystem; + +public class AutoShootCommandChassisTurret extends Command{ + + // For Chassis Control + private DriveSubsystem driveSubsystem; + + private HopperSubsystem hopperSubsystem; + private IntakeSubsystem intakeSubsystem; + private TurretSubsystem turretSubsystem; + + private PIDController autoCenterController; + + public AutoShootCommandChassisTurret(DriveSubsystem driveSubsystem, HopperSubsystem hopperSubsystem, IntakeSubsystem intakeSubsystem, TurretSubsystem turretSubsystem) + { + this.driveSubsystem = driveSubsystem; + this.hopperSubsystem = hopperSubsystem; + this.intakeSubsystem = intakeSubsystem; + this.turretSubsystem = turretSubsystem; + + autoCenterController = new PIDController(0, 0, 0); + autoCenterController.setTolerance(1); + autoCenterController.setSetpoint(0); + } + + @Override + public void initialize() { + turretSubsystem.setLauncher(2500); + hopperSubsystem.spindexerOn(); + } + + @Override + public void execute() { + // Need to compute shoot speed and angle + + // Get apriltags + // Change to HUB tracking pipeline + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_HUB_TRACKING_PIPELINE_NUMBER); + + LimelightHelpers.getTargetPose_CameraSpace(Constants.CameraConstanst.BACK_CAMERA_NAME); + + if(turretSubsystem.isLauncherUpToSpeed() && autoCenterController.atSetpoint()){ + hopperSubsystem.feedKicker(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + turretSubsystem.setLauncher(0); + + hopperSubsystem.stopKicker(); + hopperSubsystem.spindexerOff(); + + LimelightHelpers.setPipelineIndex(Constants.CameraConstanst.BACK_CAMERA_NAME, Constants.CameraConstanst.BACK_LOCALIZATION_PIPELINE_NUMBER); + } + +} diff --git a/src/main/java/frc/robot/commands/ManualHopperCommand.java b/src/main/java/frc/robot/commands/ManualHopperCommand.java index ac7c03b..0058003 100644 --- a/src/main/java/frc/robot/commands/ManualHopperCommand.java +++ b/src/main/java/frc/robot/commands/ManualHopperCommand.java @@ -23,7 +23,7 @@ public class ManualHopperCommand extends Command { // Called when the command is initially scheduled. @Override public void initialize() { - + m_subsystem.spindexerOn();; } // Called every time the scheduler runs while the command is scheduled. @@ -31,19 +31,19 @@ public class ManualHopperCommand extends Command { public void execute() { if(m_xboxController.getLeftTriggerAxis()>.5 && m_xboxController.getRightTriggerAxis()>.2) { - m_subsystem.feed(); + m_subsystem.feedKicker(); } else { - m_subsystem.stop(); + m_subsystem.stopKicker(); } if(m_xboxController.getRightBumper()) { - m_subsystem.spin(Constants.HopperConstants.SPINDEXER_MOTOR_SPEED); + m_subsystem.spindexerOn(); } if(m_xboxController.getLeftBumper()) { - m_subsystem.spin(0); + m_subsystem.spindexerOff(); } } diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index c9b8612..703336e 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -28,26 +28,20 @@ public class HopperSubsystem extends TestableSubsystem { SetMotorDirection(kickerMotor, Constants.HopperConstants.KICKER_MOTOR_DIRECTION); } - public void feed() {kickerMotor.set(0.5);} + public void feedKicker() {kickerMotor.set(0.5);} - public void reverse() {kickerMotor.set(-0.5);} + public void reverseKicker() {kickerMotor.set(-0.5);} - public void stop() {kickerMotor.set(0);} + public void stopKicker() {kickerMotor.stopMotor();} - public void spin(double speed) {spindexerMotor.set(speed);} + public void spindexerOn() { spindexerMotor.set(0.5);} - public Command revSpeedCommand() - { - return this.runOnce(() -> reverse()); - } - - @PostTest(name = "A friendly name", enabled = true) - public void myFailingPostTest(){ - assertThat(true).isFalse(); - } + public void spindexerOff() { spindexerMotor.stopMotor();} @PostTest(enabled = true) - public void myPassingPostTest(){ - assertThat("Hello World").endWith("World"); + public void hopperSystem_CANDevicesConnected(){ + assertThat(kickerMotor.isConnected()).isTrue().as("Kicker motor not connected!"); + assertThat(spindexerMotor.isConnected()).isTrue().as("Spindexer motor not connected!"); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 692b839..cbd329f 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -61,4 +61,10 @@ public class IntakeSubsystem extends TestableSubsystem { { return this.runOnce(() -> setSpeed(.5)); } + + @PostTest(enabled = true) + public void IntakeSubsystem_CANDevicesConnected(){ + assertThat(intakeMotor.isConnected()).isTrue().as("Intake motor not connected!"); + assertThat(deployMotor.isAlive()).isTrue().as("Deploy controller not connected!"); + } } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 03fee0d..f11c164 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -5,24 +5,28 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; +import org.fairportrobotics.frc.posty.test.PostTest; +import static org.fairportrobotics.frc.posty.assertions.Assertions.*; -import com.ctre.phoenix6.controls.ControlRequest; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityDutyCycle; -import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkMaxConfig; import frc.robot.Constants; -import static org.fairportrobotics.frc.robolib.motors.Utils.*; - import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.Command; @@ -38,21 +42,46 @@ public class TurretSubsystem extends TestableSubsystem { private DigitalInput turretLimitSwitch; private TalonFX turretMotor; private SparkMax launcherMotor; + private SparkClosedLoopController launcherControler; + private Servo hood; private Angle limitPos; private Angle limitNeg; private boolean turretGoingPos; public TurretSubsystem() { - turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL); - //turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID); - //SetMotorDirection(turretMotor, Constants.ShooterConstants.TURRET_MOTOR_DIRECTION); + // Turret configuration + turretLimitSwitch = new DigitalInput(Constants.ShooterConstants.TURRET_LIMIT_CHANNEL); + turretMotor = new TalonFX(Constants.ShooterConstants.TURRET_MOTOR_ID); + turretMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withSlot0( + new Slot0Configs() + .withKP(1.0) + .withKI(0) + .withKD(0) + ) + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + ) + .withCurrentLimits( + new CurrentLimitsConfigs() + ) + ); + + // Launcher configuration launcherMotor = new SparkMax(Constants.ShooterConstants.LAUNCHER_MOTOR_ID, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig() - .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO_2); + .apply((SparkMaxConfig) SparkMaxConfig.Presets.REV_NEO); config.inverted(Constants.ShooterConstants.LAUNCHER_MOTOR_INVERTED); + config.closedLoop.p(1).i(0).d(0).outputRange(0, 5000); + config.closedLoop.allowedClosedLoopError(10, ClosedLoopSlot.kSlot0); + launcherMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + launcherControler = launcherMotor.getClosedLoopController(); + // Hood configuration hood = new Servo(Constants.ShooterConstants.HOOD_SERVO_CHANNEL); currentState = TurretState.INIT; limitNeg = Constants.ShooterConstants.LIMIT_AXIMUTH_NEG; @@ -60,7 +89,11 @@ public class TurretSubsystem extends TestableSubsystem { turretGoingPos = false; } - public void setLauncher(double speed) {launcherMotor.set(speed);} + public void setLauncher(double speed) {launcherControler.setSetpoint(speed, ControlType.kVelocity);} + + public boolean isLauncherUpToSpeed() { + return launcherControler.isAtSetpoint(); + } public void turretControl(double position) { @@ -108,8 +141,7 @@ public class TurretSubsystem extends TestableSubsystem { public Angle getTurretAngle() { - // return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); - return Units.Degrees.of(0.0); + return turretMotor.getPosition().refresh().getValue().times(Constants.ShooterConstants.TURRET_GEAR_RATIO); } @Override @@ -147,4 +179,11 @@ public class TurretSubsystem extends TestableSubsystem { //turretMotor.set(0); }*/ } + + @PostTest() + public void TurretSubsystem_CANDevicesConnected(){ + assertThat(launcherMotor.getDeviceId()).isGreaterThan(0).as("Launcher motor not connected!"); + assertThat(turretMotor.isConnected()).isTrue().as("Turret motor not connected!"); + } + }