diff --git a/src/main/deploy/pathplanner/autos/2 Piece Auton Line (Bottom).auto b/src/main/deploy/pathplanner/autos/2 Piece Auton Line (Bottom).auto index 52ade71..3eb937e 100644 --- a/src/main/deploy/pathplanner/autos/2 Piece Auton Line (Bottom).auto +++ b/src/main/deploy/pathplanner/autos/2 Piece Auton Line (Bottom).auto @@ -24,22 +24,9 @@ } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "named", - "data": { - "name": "actuateIn" - } - } - ] + "name": "actuateInFast" } }, { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7b508cf..951c725 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -77,14 +77,18 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen( - new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)) - ); + // brings the actuator out, while intaking in + NamedCommands.registerCommand("actuateOut", new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)); + // brings the actuator in, while intaking and revving up the shooter for a shot, this will take time and is done while driving NamedCommands.registerCommand("actuateIn", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.INTAKE).alongWith( new SpinShooter(_shooterSubsystem, ShooterState.SHOOT, true) )); + // brings the actuator in immediately, this is fast and shooting can't be done immediately + NamedCommands.registerCommand("actuateInFast", new FeedActuate(_intakeSubsystem, ActuatorState.STOWED, FeedMode.NONE, true)); + + // revs and intakes if necessary while aiming, then shoots NamedCommands.registerCommand("shoot", new AutonShoot(_shooterSubsystem, _elevatorSubsystem, _ledSubsystem, _swerveSubsystem, _intakeSubsystem)); // Drive/Operate default commands @@ -119,7 +123,7 @@ public RobotContainer() { // to configure button bindings private void configureBindings() { - Command feedOut = new FeedActuate(_intakeSubsystem, ActuatorState.OUT).onlyWhile(() -> !_intakeSubsystem.atDesiredActuatorState()).andThen( + Command feedOut = new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.NONE, true).andThen( new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.OUTTAKE) ); @@ -144,6 +148,7 @@ private void configureBindings() { _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem)); _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem)); _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); + _driveController.square().onTrue(Commands.runOnce(() -> AutonShoot.canShoot = false)); // TESTING ONLY!!! _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem)); diff --git a/src/main/java/frc/robot/commands/auto/AutonShoot.java b/src/main/java/frc/robot/commands/auto/AutonShoot.java index 12ede7b..6caa243 100644 --- a/src/main/java/frc/robot/commands/auto/AutonShoot.java +++ b/src/main/java/frc/robot/commands/auto/AutonShoot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants.Presets; import frc.robot.commands.intake.FeedActuate; import frc.robot.commands.shooter.SpinShooter; @@ -30,7 +31,11 @@ // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutonShoot extends SequentialCommandGroup { - private static boolean _preloadShot = false; + /** + * Indicates whether the note in the intake can be shot right away, this means that the note is already squished, and + * that the shooter is already revved enough. + */ + public static boolean canShoot = false; /** Creates a new AutonShoot. */ public AutonShoot( @@ -43,14 +48,14 @@ public AutonShoot( // Add your commands in the addCommands() call, e.g. // addCommands(new FooCommand(), new BarCommand()); addCommands( - // new FeedActuate(intake, FeedMode.INTAKE).onlyIf(() -> _preloadShot).withTimeout(1), - - // Parallel command group that aims, revs, and squeezes note. ONLY APPLIES TO PRELOADED NOTE. new ParallelCommandGroup( - new SpinShooter(shooter, ShooterState.SHOOT, true).alongWith(new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1)).onlyIf( - () -> !_preloadShot - ).andThen(() -> _preloadShot = true), - new AutoAim(swerve, shooter, elevator, leds)//.withTimeout(3) + // This command will squeeze the note while revving up the shooter. It will only run if the note can't be shot right away. + new ParallelCommandGroup( + new SpinShooter(shooter, ShooterState.SHOOT, true).andThen(new WaitCommand(2)), + new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1) + ).onlyIf(() -> !canShoot), + + new AutoAim(swerve, shooter, elevator, leds) ), new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1), diff --git a/src/main/java/frc/robot/commands/intake/FeedActuate.java b/src/main/java/frc/robot/commands/intake/FeedActuate.java index c62439e..e79e563 100644 --- a/src/main/java/frc/robot/commands/intake/FeedActuate.java +++ b/src/main/java/frc/robot/commands/intake/FeedActuate.java @@ -2,6 +2,7 @@ package frc.robot.commands.intake; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.auto.AutonShoot; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeSubsystem.ActuatorState; import frc.robot.subsystems.IntakeSubsystem.FeedMode; @@ -12,16 +13,25 @@ public class FeedActuate extends Command { private final ActuatorState _actuatorState; private final FeedMode _feedMode; + private final boolean _runOnce; + /** Creates a new FeedActuate. */ - public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode) { + public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode, boolean runOnce) { _intake = intake; _actuatorState = actuatorState; _feedMode = feedMode; + _runOnce = runOnce; + addRequirements(_intake); } + /** Default FeedActuate that runs forever. */ + public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState, FeedMode feedMode) { + this(intake, actuatorState, feedMode, false); + } + /** FeedActuate to only control actuator state. */ public FeedActuate(IntakeSubsystem intake, ActuatorState actuatorState) { this(intake, actuatorState, FeedMode.NONE); @@ -32,25 +42,21 @@ public FeedActuate(IntakeSubsystem intake, FeedMode feedMode) { this(intake, ActuatorState.NONE, feedMode); } - public FeedActuate(IntakeSubsystem intake) { - this(intake, FeedMode.NONE); - } - // Called when the command is initially scheduled. @Override public void initialize() { _intake.feed(_feedMode); _intake.actuate(_actuatorState); - if (_feedMode == FeedMode.OUTTAKE) _intake.resetHasNote(); + // if not squished (and revved), can't shoot, else can + if (_runOnce) AutonShoot.canShoot = false; + else AutonShoot.canShoot = true; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { _intake.actuate(_actuatorState); - - // if (_feedMode == FeedMode.INTAKE && _intake.hasNote()) _intake.feed(FeedMode.NONE); } // Called once the command ends or is interrupted. @@ -63,6 +69,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return _runOnce && _intake.atDesiredActuatorState(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index d527a3e..f8318f7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -2,22 +2,15 @@ package frc.robot.subsystems; import com.revrobotics.CANSparkLowLevel.MotorType; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.CANSparkBase.SoftLimitDirection; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.Constants.PID; @@ -30,17 +23,8 @@ public class IntakeSubsystem extends SubsystemBase { private final CANSparkMax _feedMotor, _actuatorMotor; private final PIDController _actuatorController = new PIDController(PID.INTAKE_ACTUATE_KP, 0, 0); - // private final ProfiledPIDController _actuatorController = new ProfiledPIDController( - // 0.05, - // 0, - // 0, - // new TrapezoidProfile.Constraints(22, 50) - // ); private final RelativeEncoder _actuatorEncoder; - private final RelativeEncoder _feedEncoder; - - private final Debouncer _feedDebouncer = new Debouncer(0.3, DebounceType.kRising); /** How to feed (in or out). */ public enum FeedMode { @@ -52,7 +36,6 @@ public enum ActuatorState { STOWED, OUT, NONE } - private boolean _hasNote = false; /** Creates a new IntakeSubsystem. */ public IntakeSubsystem() { @@ -62,8 +45,6 @@ public IntakeSubsystem() { _actuatorEncoder = _actuatorMotor.getEncoder(); _actuatorEncoder.setPosition(0); - _feedEncoder = _feedMotor.getEncoder(); - _actuatorController.setTolerance(0.5); NeoConfig.configureNeo(_feedMotor, false); @@ -92,20 +73,6 @@ public double getActuator() { return _actuatorEncoder.getPosition(); } - /** - * Resets the feed's knowledge of the note. - */ - public void resetHasNote() { - _hasNote = false; - } - - /** - * Boolean for whether the intake is has the note. - */ - public boolean hasNote() { - return _hasNote; - } - /** * Actuate the intake at a given speed. Speed gets clamped. */ @@ -142,17 +109,6 @@ public void actuate(ActuatorState actuatorState) { } } - // public void setAngle(double angle){ - // double out = 0; - - // out = MathUtil.clamp( - // _actuatorController.calculate(getActuator(), angle), - // -Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED, - // Constants.Speeds.INTAKE_ACTUATE_MAX_SPEED); - - // _actuatorMotor.set(out); - // } - /** * Feed in/out of the intake. * @@ -180,13 +136,9 @@ public void feed(FeedMode feedMode) { @Override public void periodic() { - boolean stalling = _feedDebouncer.calculate(_feedMotor.getOutputCurrent() > 50); - _hasNote = stalling ? true : _hasNote; - // This method will be called once per scheduler run SmartDashboard.putNumber("ACTUATOR ENCODER", getActuator()); SmartDashboard.putNumber("ACTUATOR PERCENT OUTPUT", _actuatorMotor.get()); SmartDashboard.putNumber("FEED CURRENT OUTPUT", _feedMotor.getOutputCurrent()); - SmartDashboard.putBoolean("HAS NOTE", hasNote()); } } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index a3aeaff..e1e8c49 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -66,10 +66,10 @@ public class SwerveDriveSubsystem extends SubsystemBase { new SwerveModuleState(_backRight.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle())), new SwerveModuleState(_backLeft.getDriveVelocity(), Rotation2d.fromDegrees(_frontLeft.getAngle()))}; - private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, - BNO055.vector_type_t.VECTOR_EULER); + // private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, + // BNO055.vector_type_t.VECTOR_EULER); - // private final AHRS _gyro = new AHRS(); + private final AHRS _gyro = new AHRS(); private VisionSubsystem _visionSubsystem; @@ -319,8 +319,8 @@ public Rotation2d getHeading() { /** Get heading DIRECTLY from the BNO055 gyro as a Rotation2d. */ public Rotation2d getHeadingRaw() { - return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360)); - // return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360)); + // return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getHeading(), 360)); + return Rotation2d.fromDegrees(-Math.IEEEremainder(_gyro.getAngle(), 360)); } /** diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index da36dfc..1306d47 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -1,6 +1,8 @@ /* Copyright (C) 2024 Team 334. All Rights Reserved.*/ package frc.robot.subsystems; +import java.util.Collections; +import java.util.Comparator; import java.util.Optional; import javax.swing.text.html.Option; @@ -183,8 +185,38 @@ public double[] tagAngleOffsets(int ID) { } public double[] getNoteAngles() { - JsonNode target = _intake.getNeuralTarget(); - double[] angles = {target.get("tx").asDouble(), target.get("ty").asDouble()}; + ArrayNode targets = _intake.getNeuralTargets(); + + // Collections.sort( + // targets, new Comparator() { + // @Override + // public int compare(JsonNode noteA, JsonNode noteB) { + // double noteA_area = noteA.get("ta").asDouble(); + // double noteB_area = noteB.get("ta").asDouble(); + + // if (noteA_area > noteB_area) { + // return 1; + // } + + // if (noteB_area > noteA_area) { + // return -1; + // } + + // else { + // return 0; + // } + // } + // } + // ); + + // return {0, 0}; + + JsonNode target = targets.get(0); + + double[] angles = { + target.get("tx").asDouble(), + target.get("ty").asDouble() + }; return angles; } diff --git a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java index 14f4343..87ba8a9 100644 --- a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java +++ b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java @@ -41,13 +41,13 @@ public JsonNode getJson() { } /** - * Returns the neural target from the limelight. + * Returns the neural targets from the limelight. * */ - public JsonNode getNeuralTarget() { + public ArrayNode getNeuralTargets() { ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector"); - return targets.get(0); + return targets; } /**