diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index a1046e5..490a5d6 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -3,8 +3,11 @@ import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; import com.stuypulse.robot.commands.BuzzController; +import com.stuypulse.robot.commands.intake.IntakeShoot; import com.stuypulse.robot.commands.shooter.ShooterAcquireFromIntake; +import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.vision.VisionReloadWhiteList; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.RobotType; import com.stuypulse.robot.subsystems.arm.Arm; import com.stuypulse.robot.subsystems.intake.Intake; @@ -63,6 +66,23 @@ public void robotPeriodic() { CommandScheduler.getInstance().schedule(new ShooterAcquireFromIntake() .andThen(new BuzzController(robot.driver))); } + + if (Arm.getInstance().getVelocity() > Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + && Arm.getInstance().atIntakeShouldShootAngle() + ) { + CommandScheduler.getInstance().schedule(new IntakeShoot() + .until( + () -> Arm.getInstance().getVelocity() < Settings.Intake.ARM_SPEED_THRESHOLD_TO_FEED + || !Arm.getInstance().atIntakeShouldShootAngle() + )); + } + + if (Arm.getInstance().getState() == Arm.State.AMP + && !Shooter.getInstance().hasNote() + && Shooter.getInstance().getFeederState() != Shooter.FeederState.DEACQUIRING + ) { + CommandScheduler.getInstance().schedule(new ShooterManualIntake().until(() -> Arm.getInstance().getState() != Arm.State.AMP)); + } CommandScheduler.getInstance().run(); } diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 09832dc..a0615e4 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -15,7 +15,10 @@ import com.stuypulse.robot.commands.auton.Mobility; import com.stuypulse.robot.commands.auton.BAC.FourPieceBAC; import com.stuypulse.robot.commands.intake.IntakeAcquire; +import com.stuypulse.robot.commands.intake.IntakeAcquireForever; import com.stuypulse.robot.commands.intake.IntakeDeacquire; +import com.stuypulse.robot.commands.intake.IntakeFeed; +import com.stuypulse.robot.commands.intake.IntakeShoot; import com.stuypulse.robot.commands.intake.IntakeStop; import com.stuypulse.robot.commands.leds.LEDDefaultMode; import com.stuypulse.robot.commands.leds.LEDReset; @@ -24,6 +27,7 @@ import com.stuypulse.robot.commands.shooter.ShooterFeederDeacquire; import com.stuypulse.robot.commands.shooter.ShooterFeederShoot; import com.stuypulse.robot.commands.shooter.ShooterFeederStop; +import com.stuypulse.robot.commands.shooter.ShooterManualIntake; import com.stuypulse.robot.commands.shooter.ShooterScoreSpeaker; import com.stuypulse.robot.commands.shooter.ShooterSetRPM; import com.stuypulse.robot.commands.shooter.ShooterStop; @@ -109,6 +113,7 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); + intake.setDefaultCommand(new IntakeStop()); leds.setDefaultCommand(new LEDDefaultMode()); } @@ -127,9 +132,9 @@ private void configureDriverBindings() { // intake field relative driver.getRightTriggerButton() .onTrue(new ArmToFeed()) - .whileTrue(new SwerveDriveDriveToNote(driver)) + // .whileTrue(new SwerveDriveDriveToNote(driver)) .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKING)) + .deadlineWith(new LEDSet(LEDInstructions.FIELD_RELATIVE_INTAKING)) .andThen(new BuzzController(driver)) ); @@ -137,16 +142,15 @@ private void configureDriverBindings() { driver.getLeftTriggerButton() .onTrue(new ArmToFeed()) .whileTrue(new IntakeAcquire() - .deadlineWith(new LEDSet(LEDInstructions.INTAKING)) + .deadlineWith(new LEDSet(LEDInstructions.ROBOT_RELATIVE_INTAKING)) .andThen(new BuzzController(driver)) ) .whileTrue(new SwerveDriveDriveRobotRelative(driver)); // deacquire driver.getDPadLeft() - .onTrue(new IntakeDeacquire()) - .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)) - .onFalse(new IntakeStop()); + .whileTrue(new IntakeDeacquire()) + .whileTrue(new LEDSet(LEDInstructions.DEACQUIRING)); // speaker align and score // score amp @@ -154,124 +158,116 @@ private void configureDriverBindings() { .whileTrue(new ConditionalCommand( new ArmWaitUntilAtTarget() .withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) - .andThen(new ShooterFeederDeacquire()), + .andThen(new ShooterFeederDeacquire().alongWith(new LEDSet(LEDInstructions.AMP_SCORE))), new SwerveDriveDriveAlignedSpeaker(driver) .alongWith(new ArmToSpeaker().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToSpeaker())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) ) - .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN) - .until(() -> swerve.isAlignedToSpeaker()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ), + .alongWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN)), () -> Arm.getInstance().getState() == Arm.State.AMP)) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // ferry align and shoot - // move to back of controller - driver.getDPadRight() + // lob ferry align and shoot + driver.getLeftStickButton() .whileTrue(new SwerveDriveDriveAlignedLobFerry(driver) .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLobFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) ) - .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN) - .until(() -> swerve.isAlignedToLobFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - driver.getDPadDown() + + // low ferry align and shoot + driver.getRightStickButton() .whileTrue(new SwerveDriveDriveAlignedLowFerry(driver) .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToLowFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) ) - .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN) - .until(() -> swerve.isAlignedToLowFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // arm to amp and alignment - driver.getLeftBumper() - .onTrue(new ArmToAmp()) - .onTrue(new SwerveDriveDriveAlignedAmp(driver) - .onlyWhile(() -> - Math.abs(driver.getRightX()) < Settings.Driver.Turn.DISABLE_ALIGNMENT_DEADBAND.getAsDouble() && - Arm.getInstance().getState() == Arm.State.AMP) - .deadlineWith(new LEDSet(LEDInstructions.AMP_WITH_ALIGN))); + // arm to amp + driver.getLeftBumper().onTrue(new ArmToAmp()); // manual speaker at subwoofer - // rebind to a button on the back later driver.getRightMenuButton() .whileTrue(new ArmToSubwooferShot().alongWith(new ShooterSetRPM(Settings.Shooter.SPEAKER)) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) - .andThen(new ShooterFeederShoot())) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) + ) .whileTrue(new LEDSet(LEDInstructions.SPEAKER_MANUAL)) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // manual ferry + // manual lob ferry driver.getTopButton() .whileTrue(new SwerveDriveDriveAlignedManualLobFerry(driver) .alongWith(new ArmToLobFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLobFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) ) - .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL) - .until(() -> swerve.isAlignedToManualLobFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOB_FERRY_ALIGN_MANUAL)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); + // manual low ferry driver.getLeftButton() .whileTrue(new SwerveDriveDriveAlignedManualLowFerry(driver) .alongWith(new ArmToLowFerry().alongWith(new ShooterSetRPM(() -> shooter.getFerrySpeeds())) .andThen(new ArmWaitUntilAtTarget().withTimeout(Settings.Arm.MAX_WAIT_TO_REACH_TARGET) .alongWith(new ShooterWaitForTarget().withTimeout(Settings.Shooter.MAX_WAIT_TO_REACH_TARGET))) .andThen(new WaitUntilCommand(() -> swerve.isAlignedToManualLowFerry())) - .andThen(new ShooterFeederShoot()) + .andThen(new ShooterFeederShoot() + .alongWith(new IntakeShoot().onlyIf(() -> Arm.getInstance().atIntakeShouldShootAngle())) + ) ) - .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL) - .until(() -> swerve.isAlignedToManualLowFerry()) - .andThen(new LEDSet(LEDInstructions.IS_ALIGNED)) - ) + .alongWith(new LEDSet(LEDInstructions.LOW_FERRY_ALIGN_MANUAL)) ) .onFalse(new ConditionalCommand( new ShooterFeederStop(), new ShooterStop(), () -> Settings.Shooter.ALWAYS_KEEP_AT_SPEED)); - // climbing - driver.getRightButton().onTrue(new ArmToPreClimb()); - driver.getBottomButton().onTrue(new ArmToClimbing()); + // human player attention button + driver.getRightButton().whileTrue(new LEDSet(LEDInstructions.ATTENTION)); } private void configureOperatorBindings() { diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java index 335cfbc..4c8ecdb 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java @@ -2,9 +2,9 @@ import com.stuypulse.robot.subsystems.intake.Intake; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class IntakeDeacquire extends InstantCommand { +public class IntakeDeacquire extends Command { private final Intake intake; diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java new file mode 100644 index 0000000..a0afb12 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeShoot.java @@ -0,0 +1,19 @@ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.intake.Intake; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class IntakeShoot extends SequentialCommandGroup { + + public IntakeShoot() { + addCommands( + new InstantCommand(() -> Intake.getInstance().setState(Intake.State.SHOOTING), Intake.getInstance()), + new WaitCommand(Settings.Intake.INTAKE_SHOOT_TIME), + new IntakeStop() + ); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index b741468..2fdd384 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -39,15 +39,8 @@ public LEDDefaultMode() { private LEDInstruction getInstruction() { - switch (arm.getState()) { - case PRE_CLIMB: - return LEDInstructions.ARM_PRECLIMB; - case CLIMBING: - return LEDInstructions.ARM_POSTCLIMB; - case AMP: - return LEDInstructions.AMP_WITHOUT_ALIGN; - default: - break; + if (Arm.getInstance().getState() == Arm.State.AMP) { + return LEDInstructions.ARM_AT_AMP; } if (intake.hasNote() || shooter.hasNote()) { diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java index 5346069..95c297e 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterAcquireFromIntake.java @@ -29,7 +29,7 @@ public ShooterAcquireFromIntake() { public void initialize() { stopWatch.reset(); intake.setState(Intake.State.FEEDING); - shooter.feederIntake(); + shooter.setFeederState(Shooter.FeederState.INTAKING); } @Override @@ -57,7 +57,7 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { - shooter.feederStop(); + shooter.setFeederState(Shooter.FeederState.STOP); intake.setState(Intake.State.STOP); } diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java index ca4cca0..72656e4 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederAcquire.java @@ -15,7 +15,7 @@ public ShooterFeederAcquire() { @Override public void initialize() { - shooter.feederIntake(); + shooter.setFeederState(Shooter.FeederState.INTAKING); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java index d82326e..657e782 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederDeacquire.java @@ -15,6 +15,6 @@ public ShooterFeederDeacquire() { @Override public void initialize() { - shooter.feederDeacquire(); + shooter.setFeederState(Shooter.FeederState.DEACQUIRING); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java index 674c24d..30ef6f2 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederShoot.java @@ -15,6 +15,6 @@ public ShooterFeederShoot() { @Override public void initialize() { - shooter.feederShoot(); + shooter.setFeederState(Shooter.FeederState.SHOOTING); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java index 6201923..788d2dd 100644 --- a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterFeederStop.java @@ -15,7 +15,7 @@ public ShooterFeederStop() { @Override public void initialize() { - shooter.feederStop(); + shooter.setFeederState(Shooter.FeederState.STOP); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java new file mode 100644 index 0000000..d623417 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterManualIntake.java @@ -0,0 +1,30 @@ +package com.stuypulse.robot.commands.shooter; + +import com.stuypulse.robot.subsystems.shooter.Shooter; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ShooterManualIntake extends Command{ + + private final Shooter shooter; + + public ShooterManualIntake() { + shooter = Shooter.getInstance(); + addRequirements(shooter); + } + + @Override + public void initialize() { + shooter.setFeederState(Shooter.FeederState.INTAKING); + } + + @Override + public boolean isFinished() { + return shooter.hasNote(); + } + + @Override + public void end(boolean interrupted) { + shooter.setFeederState(Shooter.FeederState.STOP); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java index 67dc84d..aee4420 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAligned.java @@ -21,6 +21,7 @@ import com.stuypulse.stuylib.util.AngleVelocity; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public abstract class SwerveDriveDriveAligned extends Command { @@ -63,7 +64,7 @@ public SwerveDriveDriveAligned(Gamepad driver) { // make angleVelocity contribute less once distance is less than REDUCED_FF_DIST // so that angular velocity doesn't oscillate x -> x * Math.min(1, getDistanceToTarget() / Assist.REDUCED_FF_DIST), - new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL), + // new RateLimit(Settings.Swerve.MAX_ANGULAR_ACCEL), x -> SLMath.clamp(x, -Settings.Swerve.MAX_ANGULAR_VELOCITY, Settings.Swerve.MAX_ANGULAR_VELOCITY), x -> -x ); @@ -99,5 +100,7 @@ public void execute() { ) ) ); + SmartDashboard.putNumber("Alignment/Distance to Target", getDistanceToTarget()); + SmartDashboard.putNumber("Alignment/Target Angle", getTargetAngle().getDegrees()); } } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java index 6da1716..28dbc8d 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java @@ -17,7 +17,7 @@ public SwerveDriveDriveAlignedSpeaker(Gamepad driver) { protected Rotation2d getTargetAngle() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - return speakerPose.minus(currentPose).getAngle(); + return currentPose.minus(speakerPose).getAngle(); } @Override diff --git a/src/main/java/com/stuypulse/robot/commands/vision/VisionChangeWhiteList.java b/src/main/java/com/stuypulse/robot/commands/vision/VisionChangeWhiteList.java index f37b8b5..de43077 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/VisionChangeWhiteList.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/VisionChangeWhiteList.java @@ -1,6 +1,6 @@ package com.stuypulse.robot.commands.vision; -import com.stuypulse.robot.subsystems.vision.TheiaTagVision; +import com.stuypulse.robot.subsystems.vision.PhotonVision; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -19,6 +19,6 @@ public VisionChangeWhiteList(int... ids) { @Override public void initialize() { - TheiaTagVision.getInstance().setTagWhitelist(ids); + PhotonVision.getInstance().setTagWhitelist(ids); } } diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index a8857af..8f69290 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -25,19 +25,19 @@ public interface Limelight { public CameraConfig[] APRILTAG_CAMERAS = new CameraConfig[] { // TO DO: find positions new CameraConfig( - "samera0", //tower camera + "tower-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(-3.333797), Units.inchesToMeters(23.929362)), - new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(0)) + new Translation3d(Units.inchesToMeters(-11.25), Units.inchesToMeters(+3.333797), Units.inchesToMeters(23.929362)), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15), Units.degreesToRadians(180)) ), - "29", + "11", 3000 - ), //10.6.94.29:5802 + ), //10.6.94.11:5800/#/dashboard new CameraConfig( - "samera1", //electronic plate camera + "plate-cam", new Pose3d( - new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(4.863591), Units.inchesToMeters(19.216471)), + new Translation3d(Units.inchesToMeters(0), Units.inchesToMeters(-4.863591), Units.inchesToMeters(19.216471)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(80), Units.degreesToRadians(0)) ), "96", diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 8c0451b..419b7c2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -277,11 +277,9 @@ public static Translation2d getManualFerryPosition() { } public static Translation2d getAmpCornerPose() { - Translation2d targetPose = Robot.isBlue() - ? new Translation2d(0.0, Field.WIDTH - 1.5) - : new Translation2d(0.0, 1.5); - - return targetPose; + return Robot.isBlue() + ? new Translation2d(2.0, WIDTH - 1.25) + : new Translation2d(LENGTH - 2.0, WIDTH - 1.25); } // MIDLINE: 8.27 diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index e9d9f1c..3644885 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -55,29 +55,25 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction INTAKING = new LEDPulseColor(SLColor.BLUE); - LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.RED); + LEDInstruction FIELD_RELATIVE_INTAKING = new LEDRainbow(); + LEDInstruction ROBOT_RELATIVE_INTAKING = new LEDPulseColor(SLColor.BLUE); + LEDInstruction DEACQUIRING = new LEDPulseColor(SLColor.GOLD); LEDInstruction SPEAKER_ALIGN = GREEN; - LEDInstruction SPEAKER_MANUAL = new LEDPulseColor(SLColor.GREEN); + LEDInstruction SPEAKER_MANUAL = new LEDRainbow(); - LEDInstruction LOW_FERRY_ALIGN = LIME; - LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.LIME); + LEDInstruction LOW_FERRY_ALIGN = PURPLE; + LEDInstruction LOW_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.PURPLE); - LEDInstruction LOB_FERRY_ALIGN = AQUA; - LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.AQUA); + LEDInstruction LOB_FERRY_ALIGN = GREEN; + LEDInstruction LOB_FERRY_ALIGN_MANUAL = new LEDPulseColor(SLColor.GREEN); - LEDInstruction AMP_WITH_ALIGN = VIOLET; - LEDInstruction AMP_WITHOUT_ALIGN = new LEDPulseColor(SLColor.VIOLET); - - LEDInstruction IS_ALIGNED = RAINBOW; + LEDInstruction ARM_AT_AMP = YELLOW; + LEDInstruction AMP_SCORE = GREEN; LEDInstruction ATTENTION = new LED694(0.01, SLColor.BLUE); - LEDInstruction ARM_PRECLIMB = ORANGE; - LEDInstruction ARM_POSTCLIMB = new LEDPulseColor(SLColor.ORANGE); - - LEDInstruction CONTAINS_NOTE = GOLD; + LEDInstruction CONTAINS_NOTE = RED; // TO FUTURE USERS, DONT PUT LEDAlign and LEDAutonChooser (any disabled LEDInstructions) inside // the LEDInstructions interface diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index aaeea2a..1e388bb 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -39,20 +39,20 @@ public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) { /** Classes to store all of the values a motor needs */ public interface Arm { - CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.0); - CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.0); + CANSparkConfig LEFT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 40, 0.25, false); + CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, false); } public interface Intake { - CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.25); - CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kBrake, 500, 0.25); - CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25); + CANSparkConfig LEFT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.25, true); + CANSparkConfig RIGHT_FUNNEL_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); + CANSparkConfig INTAKE_MOTOR_CONFIG = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.25, true); } public interface Shooter { - CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 1.0); - CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 1.0); - CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 1.0); + CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 40, 0.5, true); + CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 40, 0.5, true); + CANSparkConfig FEEDER_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 40, 0.25, true); } /* Configurations */ @@ -91,24 +91,27 @@ public static class CANSparkConfig { public final IdleMode IDLE_MODE; public final int CURRENT_LIMIT_AMPS; public final double OPEN_LOOP_RAMP_RATE; + public final boolean ENABLE_VOLTAGE_COMPENSATION; public CANSparkConfig( boolean inverted, IdleMode idleMode, int currentLimitAmps, - double openLoopRampRate) { + double openLoopRampRate, + boolean enableVoltageCompensation) { this.INVERTED = inverted; this.IDLE_MODE = idleMode; this.CURRENT_LIMIT_AMPS = currentLimitAmps; this.OPEN_LOOP_RAMP_RATE = openLoopRampRate; + this.ENABLE_VOLTAGE_COMPENSATION = enableVoltageCompensation; } - public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) { - this(inverted, idleMode, currentLimitAmps, 0.0); + public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps, boolean enableVoltageCompensation) { + this(inverted, idleMode, currentLimitAmps, 0.0, enableVoltageCompensation); } - public CANSparkConfig(boolean inverted, IdleMode idleMode) { - this(inverted, idleMode, 80); + public CANSparkConfig(boolean inverted, IdleMode idleMode, boolean enableVoltageCompensation) { + this(inverted, idleMode, 80, enableVoltageCompensation); } public void configure(CANSparkBase motor) { @@ -116,6 +119,9 @@ public void configure(CANSparkBase motor) { motor.setIdleMode(IDLE_MODE); motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS); motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE); + if (ENABLE_VOLTAGE_COMPENSATION) { + motor.enableVoltageCompensation(12); + } motor.burnFlash(); } @@ -124,6 +130,9 @@ public void configureAsFollower(CANSparkMax motor, CANSparkMax follows) { motor.setIdleMode(IDLE_MODE); motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS); motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE); + if (ENABLE_VOLTAGE_COMPENSATION) { + motor.enableVoltageCompensation(12); + } motor.follow(follows); motor.burnFlash(); } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index a40a474..2393e5d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -58,20 +58,23 @@ public static RobotType fromString(String serialNum) { public interface Arm { double LENGTH = Units.inchesToMeters(16.5); - SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 400); - SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 425); + SmartNumber MAX_VELOCITY = new SmartNumber("Arm/Max Velocity (deg/s)", SAFE_MODE_ENABLED ? 200 : 900); + SmartNumber MAX_ACCELERATION = new SmartNumber("Arm/Max Acceleration (deg/s^2)", SAFE_MODE_ENABLED ? 200 : 700); - SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 90); + SmartNumber MAX_ANGLE = new SmartNumber("Arm/Max Angle (deg)", 85); SmartNumber MIN_ANGLE = new SmartNumber("Arm/Min Angle (deg)", -90 + 12.25); SmartNumber MAX_ANGLE_ERROR = new SmartNumber("Arm/Max Angle Error", 2.5); - SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 50); + SmartNumber AMP_ANGLE = new SmartNumber("Arm/Amp Angle", 43); SmartNumber LOW_FERRY_ANGLE = new SmartNumber("Arm/Low Ferry Angle", -50); SmartNumber LOB_FERRY_ANGLE = new SmartNumber("Arm/Lob Ferry Angle", 50); SmartNumber PRE_CLIMB_ANGLE = new SmartNumber("Arm/Pre climb angle", 80); SmartNumber POST_CLIMB_ANGLE = new SmartNumber("Arm/Post Climb Angle", MIN_ANGLE.get() + 7); - SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 17); + + SmartNumber FEED_ANGLE = new SmartNumber("Arm/Feed Angle", MIN_ANGLE.get() + 0); + SmartNumber MAX_ACCEPTABLE_FEED_ANGLE = new SmartNumber("Arm/Max Acceptable Feed Angle", FEED_ANGLE.get() + 4); + SmartNumber SUBWOOFER_SHOT_ANGLE = new SmartNumber("Arm/Subwoofer Shot Angle", -33); SmartNumber BUMP_SWITCH_DEBOUNCE_TIME = new SmartNumber("Arm/Bump Switch Debounce Time", 0.02); @@ -99,22 +102,28 @@ public interface Encoder { } public interface Intake { - double INTAKE_ACQUIRE_SPEED = 0.65; + double INTAKE_ACQUIRE_SPEED = 0.72; double INTAKE_DEACQUIRE_SPEED = 1.0; - double INTAKE_FEED_SPEED = 0.65; + double INTAKE_FEED_SPEED = 0.4; + + double MAX_ARM_ANGLE_FOR_INTAKE_SHOOT = Arm.MIN_ANGLE.get() + 25; + double ARM_SPEED_THRESHOLD_TO_FEED = 2.5; // degrees per second + + double INTAKE_SHOOT_SPEED = 0.9; + double INTAKE_SHOOT_TIME = 0.75; double FUNNEL_ACQUIRE = 1.0; double FUNNEL_DEACQUIRE = 1.0; - double IR_DEBOUNCE = .005; + double IR_DEBOUNCE = 0.0; - double HANDOFF_TIMEOUT = 1.5; + double HANDOFF_TIMEOUT = 1.0; double MINIMUM_DEACQUIRE_TIME_WHEN_STUCK = 0.5; } public interface Shooter { - double FEEDER_INTAKE_SPEED = 0.23; + double FEEDER_INTAKE_SPEED = 0.18; double FEEDER_DEAQUIRE_SPEED = 0.5; double FEEDER_SHOOT_SPEED = 1.0; @@ -129,27 +138,27 @@ public interface Shooter { ); // Different falling debounce is used to detect note shooting; - SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.01); - SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.005); + SmartNumber HAS_NOTE_FALLING_DEBOUNCE = new SmartNumber("Shooter/Has Note Falling Debounce", 0.0); //0.01 + SmartNumber HAS_NOTE_RISING_DEBOUNCE = new SmartNumber("Shooter/Has Note Rising Debounce", 0.0); //0.005 // left runs faster than right public interface LEFT { public interface PID { - double kP = 0.00034711; + double kP = 0.0003211; double kI = 0; - double kD = 0.00002; + double kD = 0.0; } public interface FF { double kS = 0; - double kV = 0.00017; + double kV = 0.00015; double kA = 0; } } public interface RIGHT { public interface PID { - double kP = 0.00304711; + double kP = 0.0002; double kI = 0; double kD = 0.0; } @@ -168,8 +177,8 @@ public interface Swerve { double MAX_LINEAR_VELOCITY = SAFE_MODE_ENABLED ? 1.0 : 4.9; double MAX_LINEAR_ACCEL = SAFE_MODE_ENABLED ? 10 : 15; - double MAX_ANGULAR_VELOCITY = SAFE_MODE_ENABLED ? 3.0 : 12.0; - double MAX_ANGULAR_ACCEL = SAFE_MODE_ENABLED ? 25.0 : 100.0; + double MAX_ANGULAR_VELOCITY = SAFE_MODE_ENABLED ? 3.0 : 10.0; + double MAX_ANGULAR_ACCEL = SAFE_MODE_ENABLED ? 25.0 : 200.0; String CAN_BUS_NAME = "swerve"; @@ -404,8 +413,8 @@ public interface Vision { } public interface Buzz { - double BUZZ_DURATION = 0.2; - double BUZZ_INTENSITY = 1; + double BUZZ_DURATION = 1.0; + double BUZZ_INTENSITY = 1.0; } public interface Auton { diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java index adbfd16..a3547b7 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/Arm.java @@ -44,6 +44,11 @@ public State getState() { public abstract boolean atTarget(); + public abstract boolean atValidFeedAngle(); + public abstract boolean atIntakeShouldShootAngle(); + + public abstract double getVelocity(); + @Override public void periodic() { SmartDashboard.putString("Arm/State", state.toString()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java index 415cd6d..005c34a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/arm/ArmImpl.java @@ -69,11 +69,22 @@ protected ArmImpl() { @Override public boolean atTarget() { if (state == State.FEED) { - return getDegrees() < Settings.Arm.FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get(); + return atValidFeedAngle(); } return Math.abs(getTargetDegrees() - getDegrees()) < Settings.Arm.MAX_ANGLE_ERROR.get(); } + @Override + public boolean atValidFeedAngle() { + return getDegrees() > Settings.Arm.FEED_ANGLE.get() - Settings.Arm.MAX_ANGLE_ERROR.get() + && getDegrees() < Settings.Arm.MAX_ACCEPTABLE_FEED_ANGLE.get() + Settings.Arm.MAX_ANGLE_ERROR.get(); + } + + @Override + public boolean atIntakeShouldShootAngle() { + return getDegrees() < Settings.Intake.MAX_ARM_ANGLE_FOR_INTAKE_SHOOT; + } + private double getTargetDegrees() { switch (state) { case AMP: @@ -140,6 +151,11 @@ private double getDegrees() { return 360 * armEncoder.getPosition(); } + @Override + public double getVelocity() { + return 360 / 60 * armEncoder.getVelocity(); + } + private void setVoltage(double voltage) { leftMotor.setVoltage(voltage); rightMotor.setVoltage(voltage); @@ -160,9 +176,13 @@ public void periodic() { setVoltage(-1.5); controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); } + else if (getTargetDegrees() == Settings.Arm.MIN_ANGLE.get() && bumpSwitchTriggered.get()) { + setVoltage(0); + controller.update(Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MIN_ANGLE.get()); + } else { controller.update(SLMath.clamp(getTargetDegrees(), Settings.Arm.MIN_ANGLE.get(), Settings.Arm.MAX_ANGLE.get()), getDegrees()); - if (Shooter.getInstance().isShooting()) { + if (Shooter.getInstance().getFeederState() == Shooter.FeederState.SHOOTING && getDegrees() < Settings.Arm.MAX_ANGLE.get()) { setVoltage(controller.getOutput() + 0.31); } else { @@ -189,6 +209,8 @@ public void periodic() { SmartDashboard.putNumber("Arm/Right Duty Cycle", rightMotor.get()); SmartDashboard.putNumber("Arm/Arm Angle", getDegrees()); - SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin) + SmartDashboard.putNumber("Arm/Shooter Angle", getDegrees() + 96); // shooter is offset 96 degrees counterclockwise from arm (thanks kevin)] + + SmartDashboard.putBoolean("Arm/At Target", atTarget()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 9f6ba13..43b7ddd 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -1,5 +1,6 @@ package com.stuypulse.robot.subsystems.intake; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,6 +20,7 @@ public enum State { DEACQUIRING, ACQUIRING, FEEDING, + SHOOTING, STOP } @@ -37,4 +39,9 @@ public State getState() { } public abstract boolean hasNote(); + + @Override + public void periodic() { + SmartDashboard.putString("Intake/State", state.name()); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 021e797..01948ed 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -44,6 +44,12 @@ private void acquire() { funnelMotorRight.set(+Settings.Intake.FUNNEL_ACQUIRE); } + private void shoot() { + intakeMotor.set(Settings.Intake.INTAKE_SHOOT_SPEED); + funnelMotorLeft.stopMotor(); + funnelMotorRight.stopMotor(); + } + private void deacquire() { intakeMotor.set(-Settings.Intake.INTAKE_DEACQUIRE_SPEED); funnelMotorLeft.set(-Settings.Intake.FUNNEL_DEACQUIRE); @@ -81,6 +87,9 @@ public void periodic() { case FEEDING: feed(); break; + case SHOOTING: + shoot(); + break; case STOP: stop(); break; diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java index fa50c6b..6f5fad3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java @@ -17,11 +17,21 @@ public abstract class Shooter extends SubsystemBase { public static Shooter getInstance() { return instance; } + + public enum FeederState { + INTAKING, + DEACQUIRING, + SHOOTING, + STOP + } + + private FeederState feederState; private final SmartNumber leftTargetRPM; private final SmartNumber rightTargetRPM; public Shooter() { + feederState = FeederState.STOP; leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getLeftRPM() : 0); rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", Settings.Shooter.ALWAYS_KEEP_AT_SPEED ? Settings.Shooter.SPEAKER.getRightRPM() : 0); } @@ -40,21 +50,22 @@ public double getRightTargetRPM() { } public void stop() { - feederStop(); + setFeederState(FeederState.STOP); leftTargetRPM.set(0); rightTargetRPM.set(0); } - public abstract boolean atTargetSpeeds(); + public void setFeederState(FeederState feederState) { + this.feederState = feederState; + } - public abstract ShooterSpeeds getFerrySpeeds(); + public FeederState getFeederState() { + return feederState; + } - public abstract void feederIntake(); - public abstract void feederDeacquire(); - public abstract void feederShoot(); - public abstract void feederStop(); + public abstract boolean atTargetSpeeds(); - public abstract boolean isShooting(); + public abstract ShooterSpeeds getFerrySpeeds(); public abstract boolean hasNote(); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 62ecdeb..65bf49e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -106,33 +106,24 @@ private void setRightShooterRPM(double rpm) { rightController.setReference(rpm, ControlType.kVelocity); } - @Override - public void feederIntake() { - feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED); - isShooting = false; - } - - @Override - public void feederDeacquire() { - feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED); - isShooting = false; - } - - @Override - public void feederShoot() { - feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED); - isShooting = true; - } - - @Override - public void feederStop() { - feederMotor.set(0); - isShooting = false; - } - - @Override - public boolean isShooting() { - return isShooting; + private void setFeederBasedOnState() { + switch (getFeederState()) { + case INTAKING: + feederMotor.set(+Settings.Shooter.FEEDER_INTAKE_SPEED); + break; + case DEACQUIRING: + feederMotor.set(-Settings.Shooter.FEEDER_DEAQUIRE_SPEED); + break; + case SHOOTING: + feederMotor.set(Settings.Shooter.FEEDER_SHOOT_SPEED); + break; + case STOP: + feederMotor.set(0); + break; + default: + feederMotor.set(0); + break; + } } @Override @@ -176,6 +167,8 @@ public void periodic () { setRightShooterRPM(getRightTargetRPM()); } + setFeederBasedOnState(); + SmartDashboard.putNumber("Shooter/Feeder Speed", feederMotor.get()); SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage()); @@ -193,6 +186,8 @@ public void periodic () { SmartDashboard.putNumber("Shooter/Left Current", leftMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Right Current", rightMotor.getOutputCurrent()); SmartDashboard.putNumber("Shooter/Feeder Current", feederMotor.getOutputCurrent()); + + SmartDashboard.putString("Shooter/Feeder State", getFeederState().toString()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java index ea031fd..af30011 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java @@ -172,8 +172,8 @@ public void initFieldObject() { public boolean isAlignedToSpeaker() { Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation(); Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation(); - Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle(); - + // Rotate by 180 because the shooter is on the back of the robot + Rotation2d targetAngle = speakerPose.minus(currentPose).getAngle().rotateBy(Rotation2d.fromDegrees(180)); return Math.abs(getPose().getRotation().minus(targetAngle).getDegrees()) < Settings.Alignment.ANGLE_TOLERANCE.get(); } @@ -215,11 +215,9 @@ private void updateEstimatorWithVisionData(ArrayList outputs) { timestampSum += data.getTimestamp() * data.getArea(); } - // addVisionMeasurement(poseSum.div(areaSum), timestampSum / areaSum, - // DriverStation.isAutonomous() ? VecBuilder.fill(0.9, 0.9, 10) : VecBuilder.fill(0.7, 0.7, 10)); - - addVisionMeasurement(new Pose2d(3, 3, new Rotation2d()), timestampSum / areaSum, + addVisionMeasurement(poseSum.div(areaSum), timestampSum / areaSum, DriverStation.isAutonomous() ? VecBuilder.fill(0.9, 0.9, 10) : VecBuilder.fill(0.7, 0.7, 10)); + } public void setVisionEnabled(boolean enabled) { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/AprilTagVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/AprilTagVision.java index 64f58a9..6c4c9b1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/AprilTagVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/AprilTagVision.java @@ -11,7 +11,7 @@ public abstract class AprilTagVision extends SubsystemBase { private static final AprilTagVision instance; static { - instance = new TheiaTagVision(); + instance = new PhotonVision(); } public static AprilTagVision getInstance() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java new file mode 100644 index 0000000..1e645aa --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/PhotonVision.java @@ -0,0 +1,132 @@ +package com.stuypulse.robot.subsystems.vision; + +import com.stuypulse.robot.constants.Cameras; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.robot.util.vision.VisionData; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.ArrayList; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class PhotonVision extends AprilTagVision { + + private final PhotonCamera[] cameras; + private final boolean[] enabled; + private final PhotonPoseEstimator[] poseEstimators; + private final ArrayList outputs; + + private final FieldObject2d robot; + + protected PhotonVision() { + this.cameras = new PhotonCamera[Cameras.APRILTAG_CAMERAS.length]; + for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) { + cameras[i] = new PhotonCamera(Cameras.APRILTAG_CAMERAS[i].getName()); + } + + enabled = new boolean[Cameras.APRILTAG_CAMERAS.length]; + + for (int i = 0; i < enabled.length; i++) { + enabled[i] = true; + } + + poseEstimators = new PhotonPoseEstimator[Cameras.APRILTAG_CAMERAS.length]; + for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) { + poseEstimators[i] = new PhotonPoseEstimator( + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), + PoseStrategy.AVERAGE_BEST_TARGETS, + Cameras.APRILTAG_CAMERAS[i].getLocation().minus(new Pose3d()) + ); + } + + outputs = new ArrayList(); + + robot = SwerveDrive.getInstance().getField().getObject("Vision Pose"); + } + + /** + * Returns the outputs of the vision system. + * + * @return the outputs of the vision system + */ + @Override + public ArrayList getOutputs() { + return outputs; + } + + /** + * Sets the tag layout of the vision system. + * + * @param ids the tag IDs + */ + @Override + public void setTagWhitelist(int... ids) { + + } + + @Override + public void setCameraEnabled(String name, boolean enabled) { + for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) { + if (cameras[i].getName().equals(name)) { + this.enabled[i] = enabled; + } + } + } + + private int[] getIDs(PhotonPipelineResult pipelineResult) { + ArrayList ids = new ArrayList(); + for (PhotonTrackedTarget target : pipelineResult.getTargets()) { + ids.add(target.getFiducialId()); + } + return ids.stream().mapToInt(i -> i).toArray(); + } + + @Override + public void periodic() { + super.periodic(); + + outputs.clear(); + + for (int i = 0; i < cameras.length; i++) { + final int index = i; + if (enabled[index]) { + PhotonPipelineResult latestResult = cameras[index].getLatestResult(); + Optional estimatedRobotPose = poseEstimators[index].update(latestResult); + estimatedRobotPose.ifPresent( + (EstimatedRobotPose robotPose) -> { + VisionData data = new VisionData(robotPose.estimatedPose, getIDs(latestResult), robotPose.timestampSeconds, latestResult.getBestTarget().getArea()); + outputs.add(data); + updateTelemetry("Vision/" + cameras[index].getName(), data); + } + ); + } + } + + SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0); + } + + private void updateTelemetry(String prefix, VisionData data) { + SmartDashboard.putNumber(prefix + "/Pose X", data.getPose().getX()); + SmartDashboard.putNumber(prefix + "/Pose Y", data.getPose().getY()); + SmartDashboard.putNumber(prefix + "/Pose Z", data.getPose().getZ()); + + SmartDashboard.putNumber(prefix + "/Distance to Tag", data.getDistanceToPrimaryTag()); + + SmartDashboard.putNumber(prefix + "/Pose Rotation", Units.radiansToDegrees(data.getPose().getRotation().getAngle())); + SmartDashboard.putNumber(prefix + "/Timestamp", data.getTimestamp()); + + robot.setPose(data.getPose().toPose2d()); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java deleted file mode 100644 index 11c5cad..0000000 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java +++ /dev/null @@ -1,93 +0,0 @@ -package com.stuypulse.robot.subsystems.vision; - -import com.stuypulse.robot.constants.Cameras; -import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.robot.util.vision.TheiaCamera; -import com.stuypulse.robot.util.vision.VisionData; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.ArrayList; - -public class TheiaTagVision extends AprilTagVision { - - private final TheiaCamera[] cameras; - private final ArrayList outputs; - - private final FieldObject2d robot; - - protected TheiaTagVision() { - this.cameras = new TheiaCamera[Cameras.APRILTAG_CAMERAS.length]; - for (int i = 0; i < Cameras.APRILTAG_CAMERAS.length; i++) { - cameras[i] = new TheiaCamera(Cameras.APRILTAG_CAMERAS[i]); - } - - outputs = new ArrayList(); - - robot = SwerveDrive.getInstance().getField().getObject("Vision Pose"); - } - - /** - * Returns the outputs of the vision system. - * - * @return the outputs of the vision system - */ - @Override - public ArrayList getOutputs() { - return outputs; - } - - /** - * Sets the tag layout of the vision system. - * - * @param ids the tag IDs - */ - @Override - public void setTagWhitelist(int... ids) { - for (TheiaCamera camera : cameras) { - camera.setTagLayout(ids); - } - } - - @Override - public void setCameraEnabled(String name, boolean enabled) { - for (TheiaCamera camera : cameras) { - if (camera.getName().equals(name)) - camera.setEnabled(enabled); - } - } - - @Override - public void periodic() { - super.periodic(); - - outputs.clear(); - - for (TheiaCamera camera : cameras) { - SmartDashboard.putBoolean(camera.getName() + "/Has Data", camera.getVisionData().isPresent()); - - camera.getVisionData().ifPresent( - (VisionData data) -> { - outputs.add(data); - updateTelemetry("Vision/" + camera.getName(), data); - }); - } - - SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0); - } - - private void updateTelemetry(String prefix, VisionData data) { - SmartDashboard.putNumber(prefix + "/Pose X", data.getPose().getX()); - SmartDashboard.putNumber(prefix + "/Pose Y", data.getPose().getY()); - SmartDashboard.putNumber(prefix + "/Pose Z", data.getPose().getZ()); - - SmartDashboard.putNumber(prefix + "/Distance to Tag", data.getDistanceToPrimaryTag()); - - SmartDashboard.putNumber(prefix + "/Pose Rotation", Units.radiansToDegrees(data.getPose().getRotation().getAngle())); - SmartDashboard.putNumber(prefix + "/Timestamp", data.getTimestamp()); - - robot.setPose(data.getPose().toPose2d()); - } -} diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java index 06ddca6..5d3c1e9 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLobFerryInterpolation.java @@ -13,40 +13,67 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - // fake data im just putting here so it doesnt crash lol - {3100,382}, - {3100,292}, - {3100,337}, - {3100,337}, - {3100,290}, - {3100,300}, - {3200,335}, - {3200,349}, - {3200,337}, - {3200,339}, - {3300,379}, - {3300,384}, - {3300,389}, - {3300,373}, - {3300,354}, - {3500,389}, - {3500,331}, - {2500,238}, - {2500,233}, - {2500,259}, - {2000,169}, - {2500,288}, - {2000,164}, - {2000,191}, - {1500,109}, - {1500,109}, - {1500,108}, - {1500,105}, - {1200,79}, - {1200,82}, - {1200,67}, - {1200,69}, - {1200,64}, + {4000, 562}, + {4000, 538}, + {4000, 578}, + {4000, 498}, + {4000, 527}, + {4000, 499}, + {4000, 481}, + {4000, 611}, + {4000, 528}, + {4000, 365}, + {4000, 487}, + {4000, 548}, + {3800, 456}, + {3800, 457}, + {3800, 515}, + {3800, 563}, + {3800, 560}, + {3800, 495}, + {3600, 519}, + {3600, 505}, + {3600, 525}, + {3600, 513}, + {3600, 521}, + {3600, 513}, + {3600, 554}, + {3600, 495}, + {3400, 428}, + {3400, 415}, + {3400, 430}, + {3400, 428}, + {3400, 437}, + {3200, 439}, + {3200, 425}, + {3200, 455}, + {3200, 446}, + {3200, 447}, + {3200, 447}, + {3000, 413}, + {3000, 428}, + {3000, 418}, + {3000, 413}, + {3000, 369}, + {3000, 400}, + {3000, 342}, + {3000, 426}, + {2600, 285}, + {2600, 316}, + {2600, 302}, + {2600, 304}, + {2600, 338}, + {2600, 318}, + {2200, 258}, + {2200, 307}, + {2200, 267}, + {2200, 267}, + {2200, 306}, + {2200, 272}, + {1500, 194}, + {1500, 198}, + {1500, 132}, + {1500, 133}, }; static { diff --git a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java index f1f4180..6fdd7b7 100644 --- a/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java +++ b/src/main/java/com/stuypulse/robot/util/ShooterLowFerryInterpolation.java @@ -13,35 +13,88 @@ public static void main(String[] args) { // RPM, distance (inches) private static final double[][] RPMAndDistance = { - {1000, 55.5}, - {1000, 42.5}, - {1000, 57}, - {1200, 73}, - {2900, 295}, - {2900, 265.5}, - {2900, 279}, - {2900, 289}, - {3000, 306}, - {3000, 317}, - {3000, 311.5}, - {3000, 324}, - {3000, 303}, - {3000, 300}, - {3000, 303.5}, - {3700, 437}, - {3700, 447.5}, - {3700, 419}, - {3700, 416}, - {3700, 400}, - {3700, 381.5}, - {3800, 410.5}, - {3800, 441.25}, - {3900, 457.5}, - {3900, 427}, - {4000, 476}, - {4000, 435}, - {4000, 439}, + {4000, 551}, + {4000, 573}, + {4000,546 }, + {4000, 566}, + {4000, 579}, + {4000, 458}, + {4000, 533}, + {3800, 561}, + {3800, 499}, + {3800, 576}, + {3800, 517}, + {3800, 497}, + {3800, 522}, + {3600, 456}, + {3600, 516}, + {3600, 465}, + {3600, 422}, + {3600, 542}, + {3400, 435}, + {3400, 467}, + {3400, 496}, + {3400, 461}, + {3400, 470}, + {3400, 448}, + {3400, 516}, + {3200, 433}, + {3200, 456}, + {3200, 447}, + {3200, 406}, + {3200, 424}, + {3200, 454}, + {3000, 414}, + {3000, 388}, + {3000, 389}, + {3000, 392}, + {3000, 405}, + {3000, 400}, + {3000, 430}, + {2600, 316}, + {2600, 354}, + {2600, 339}, + {2200, 288}, + {2200, 241}, + {2200, 268}, + {2200, 218}, + {2200, 210}, + {1500, 145}, + {1500, 145}, + {1500, 136}, + {1500, 184}, + {1500, 178}, }; + // private static final double[][] RPMAndDistance = { + // {1000, 55.5}, + // {1000, 42.5}, + // {1000, 57}, + // {1200, 73}, + // {2900, 295}, + // {2900, 265.5}, + // {2900, 279}, + // {2900, 289}, + // {3000, 306}, + // {3000, 317}, + // {3000, 311.5}, + // {3000, 324}, + // {3000, 303}, + // {3000, 300}, + // {3000, 303.5}, + // {3700, 437}, + // {3700, 447.5}, + // {3700, 419}, + // {3700, 416}, + // {3700, 400}, + // {3700, 381.5}, + // {3800, 410.5}, + // {3800, 441.25}, + // {3900, 457.5}, + // {3900, 427}, + // {4000, 476}, + // {4000, 435}, + // {4000, 439}, + // }; static { interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); diff --git a/src/main/java/com/stuypulse/robot/util/vision/AprilTag.java b/src/main/java/com/stuypulse/robot/util/vision/AprilTag.java index ea38c7f..2ad2933 100644 --- a/src/main/java/com/stuypulse/robot/util/vision/AprilTag.java +++ b/src/main/java/com/stuypulse/robot/util/vision/AprilTag.java @@ -31,10 +31,10 @@ public int getID() { * @return the location of the tag on the field */ public Pose3d getLocation() { - if (Robot.isBlue()) { + // if (Robot.isBlue()) { return blueLocation; - } else { - return Field.transformToOppositeAlliance(blueLocation); - } + // } else { + // return Field.transformToOppositeAlliance(blueLocation); + // } } } diff --git a/src/main/java/com/stuypulse/robot/util/vision/TheiaCamera.java b/src/main/java/com/stuypulse/robot/util/vision/TheiaCamera.java deleted file mode 100644 index 2f71e4b..0000000 --- a/src/main/java/com/stuypulse/robot/util/vision/TheiaCamera.java +++ /dev/null @@ -1,229 +0,0 @@ -package com.stuypulse.robot.util.vision; - -import com.stuypulse.robot.constants.Cameras.CameraConfig; -import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.net.PortForwarder; -import edu.wpi.first.networktables.DoubleArrayPublisher; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.DoubleSubscriber; -import edu.wpi.first.networktables.IntegerArraySubscriber; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.Optional; - -/** - * This class handles interactions between the robot code and the Theia AprilTag system through the - * NetworkTables. - */ -public class TheiaCamera { - - private final String name; - private final Pose3d cameraLocation; - - // Default Values - private final int camera_id = 0; - private final int camera_resolution_width = 1600; - private final int camera_resolution_height = 1200; - private final int camera_auto_exposure = 1; - private final int camera_exposure = 10; - private final double camera_gain = 0.0; - private final double camera_brightness = 0.0; - - private final int camera_pixel_count = camera_resolution_height * camera_resolution_width; - - // NetworkTables - private final DoubleSubscriber latencySub; - private final IntegerSubscriber fpsSub; - private final DoubleArraySubscriber poseSub; - private final IntegerArraySubscriber idSub; - private final IntegerSubscriber counterSub; - private final DoubleArraySubscriber areaSub; - private final DoubleArraySubscriber pixelSub; - - private final DoubleArrayPublisher layoutPub; - - private final StructPublisher posePub; - - - private double rawLatency; - private long rawFPS; - private double[] rawPose; - private long[] rawids; - private long rawCounter; - private double[] rawAreas; - private double[] rawPixelCoords; - private long lastCounter; - - private final SmartBoolean enabled; - - public TheiaCamera(String name, Pose3d cameraLocation, String ip, int port) { - this.name = name; - this.cameraLocation = cameraLocation; - - NetworkTable table = NetworkTableInstance.getDefault().getTable(this.name); - - NetworkTable configTable = table.getSubTable("config"); - configTable.getIntegerTopic("camera_id").publish().set(camera_id); - configTable.getIntegerTopic("camera_resolution_width").publish().set(camera_resolution_width); - configTable.getIntegerTopic("camera_resolution_height").publish().set(camera_resolution_height); - configTable.getIntegerTopic("camera_auto_exposure").publish().set(camera_auto_exposure); - configTable.getIntegerTopic("camera_exposure").publish().set(camera_exposure); - configTable.getDoubleTopic("camera_gain").publish().set(camera_gain); - configTable.getDoubleTopic("camera_brightness").publish().set(camera_brightness); - - layoutPub = configTable.getDoubleArrayTopic("fiducial_layout").publish(); - layoutPub.set(Field.getLayoutAsDoubleArray(Field.APRILTAGS)); - - NetworkTable outputTable = table.getSubTable("output"); - latencySub = outputTable.getDoubleTopic("latency").subscribe(0, PubSubOption.periodic(0.02)); - fpsSub = outputTable.getIntegerTopic("fps").subscribe(0, PubSubOption.periodic(0.02)); - poseSub = outputTable.getDoubleArrayTopic("pose").subscribe(new double[] {}, PubSubOption.periodic(0.02)); - idSub = outputTable.getIntegerArrayTopic("tids").subscribe(new long[] {}, PubSubOption.periodic(0.02)); - counterSub = outputTable.getIntegerTopic("update_counter").subscribe(0, PubSubOption.periodic(0.02)); - areaSub = outputTable.getDoubleArrayTopic("areas").subscribe(new double[] {}, PubSubOption.periodic(0.02)); - pixelSub = outputTable.getDoubleArrayTopic("pixel_coords").subscribe(new double[] {}, PubSubOption.periodic(0.02)); - - posePub = NetworkTableInstance.getDefault().getStructTopic("SmartDashboard/Vision/" + getName() + "/Pose3d", Pose3d.struct).publish(); - - enabled = new SmartBoolean(name + "/Enabled", true); - - PortForwarder.add(port, "10.6.94." + ip, 5802); - } - - public TheiaCamera(CameraConfig config) { - this(config.getName(), config.getLocation(), config.getIP(), config.getForwardedPort()); - } - - /** - * Returns the name of the camera. - * - * @return the name of the camera - */ - public String getName() { - return name; - } - - /** - * Returns the FPS of the data that is coming from the camera. - * - * @return the FPS of the data that is coming from the camera - */ - public int getFPS() { - return (int) rawFPS; - } - - private boolean hasData() { - return rawPose.length > 0 && - rawids.length > 0 && - rawAreas.length > 0; - } - - /** Pull the data from the NetworkTables and store it in the class. */ - private void updateData() { - rawLatency = latencySub.get(); - rawFPS = (int) fpsSub.get(); - rawPose = poseSub.get(); - rawids = idSub.get(); - rawCounter = counterSub.get(); - rawAreas = areaSub.get(); - rawPixelCoords = pixelSub.get(); - } - - /** - * Helper class that returns the rawPose as a Pose3d. - * - * @return the rawPose as a Pose3d - */ - private Pose3d getDataAsPose3d() { - return new Pose3d( - new Translation3d(rawPose[0], rawPose[1], rawPose[2]), - new Rotation3d(rawPose[3], rawPose[4], rawPose[5])); - } - - /** - * Returns the pose of the robot relative to the field. - * - * @return the pose of the robot relative to the field - */ - private Pose3d getRobotPose() { - return getDataAsPose3d() - .transformBy(new Transform3d(cameraLocation.getTranslation(), cameraLocation.getRotation()) - .inverse()); - } - - /** - * Returns the IDs of the tags detected. - * - * @return the IDs of the tags detected - */ - private int[] getIDs() { - int[] ids = new int[rawids.length]; - for (int i = 0; i < rawids.length; i++) { - ids[i] = (int) rawids[i]; - } - return ids; - } - - public void setEnabled(boolean enabled) { - this.enabled.set(enabled); - } - - /** - * Returns an Optional holding the vision data from the camera. - * - * @return the vision data from the camera in an Optional - */ - public Optional getVisionData() { - updateData(); - - posePub.set(Field.EMPTY_FIELD_POSE3D); - - if (!hasData()) return Optional.empty(); - if (!enabled.get()) return Optional.empty(); - - double fpgaTime = latencySub.getLastChange() / 1_000_000.0; - double timestamp = fpgaTime - Units.millisecondsToSeconds(rawLatency); - - // if (rawCounter - lastCounter < 1) { - // lastCounter = rawCounter; - - // return Optional.empty(); - // } - - lastCounter = rawCounter; - - VisionData data = new VisionData(getRobotPose(), getIDs(), timestamp, rawAreas[0] / camera_pixel_count); - - if (!data.isValidData()) return Optional.empty(); - - posePub.set(data.getPose()); - - if (rawPixelCoords.length == 2) { - SmartDashboard.putNumber("Vision/" + getName() + "/Primary Tag X", rawPixelCoords[0]); - SmartDashboard.putNumber("Vision/" + getName() + "/Primary Tag Y", rawPixelCoords[1]); - } - - return Optional.of(data); - } - - /** - * Sets the tag layout of the camera. - * - * @param ids the tag IDs - */ - public void setTagLayout(int... ids) { - layoutPub.set(Field.getLayoutAsDoubleArray(Field.getApriltagLayout(ids))); - } -}