diff --git a/src/main/deploy/weezer.chrp b/src/main/deploy/weezer.chrp new file mode 100644 index 00000000..dd2f55c1 Binary files /dev/null and b/src/main/deploy/weezer.chrp differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d16ae7d8..06b4b2f2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -79,6 +79,10 @@ public static class SwerveConstants { //ADD 90 degrees to all public static final Translation2d FR_POS = new Translation2d(MODULE_DIST, -MODULE_DIST); public static final Translation2d BL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST); public static final Translation2d BR_POS = new Translation2d(-MODULE_DIST, -MODULE_DIST); + + public static final Translation2d BLUE_SPEAKER_POS = new Translation2d(Units.inchesToMeters(-1.50 + 17.5), Units.inchesToMeters(218.42)); + public static final Translation2d RED_SPEAKER_POS = new Translation2d(Units.inchesToMeters(652.73 - 17.5), Units.inchesToMeters(218.42)); + public static final double SPEAKER_TO_SPEAKER = Units.inchesToMeters(651.23); } public static class IntakeConstants { @@ -162,12 +166,15 @@ public static class LEDConstants { } public static final class VisionConstants { - public static final String POSE_TABLE_KEY = "PoseEstimates"; + public static final String VISION_TABLE_KEY = "Vision"; public static final PhotonCamera FRONT_CAMERA = new PhotonCamera("Arducam_OV9281_USB_Camera_2"); public static final Transform3d FRONT_CAMERA_POSE = new Transform3d( new Translation3d(Units.inchesToMeters(+8), Units.inchesToMeters(4), Units.inchesToMeters(+44)), new Rotation3d(Math.PI, Units.degreesToRadians(16), 0) ); + + public static final PhotonCamera NOTE_CAMERA = new PhotonCamera("Microsoft_LifeCam_HD-3000"); + public static final Transform3d NOTE_CAMERA_POSE = new Transform3d(); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7fd3830e..1346e657 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,12 +24,14 @@ import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; import frc.robot.commands.intake.roller.IntakeRollerOutakeCommand; +import frc.robot.commands.sequences.AutoIntakeSequence; import frc.robot.commands.sequences.ShootModeSequence; import frc.robot.commands.shooter.feed.ShooterFeedLoadCommand; import frc.robot.commands.shooter.feed.ShooterFeedShootCommand; import frc.robot.commands.shooter.pivot.ShooterPivotSetAngleCommand; import frc.robot.commands.shooter.pivot.ShooterPivotVerticalCommand; import frc.robot.commands.swerve.AlignCommand; +import frc.robot.commands.swerve.NoteAlignCommand; import frc.robot.commands.swerve.SwerveStopCommand; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.elevator.ElevatorSubsystem; @@ -38,9 +40,13 @@ import frc.robot.controllers.XboxDriveController; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem; +import frc.robot.subsystems.swerve.SwerveModule; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.TestSingleModuleSwerveSubsystem; import frc.robot.util.ConditionalWaitCommand; +import frc.robot.vision.NoteDetectionWrapper; + +import static frc.robot.Constants.VisionConstants.NOTE_CAMERA; import java.sql.Driver; import java.util.function.BooleanSupplier; @@ -52,6 +58,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -59,6 +66,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -85,6 +93,8 @@ public class RobotContainer { private final ElevatorSubsystem elevatorSubsystem; private final LEDSubsystem ledSubsystem = new LEDSubsystem(); + + private final NoteDetectionWrapper noteDetector; // Configure the trigger bindings @@ -110,10 +120,6 @@ public class RobotContainer { private PIDController xPID; private PIDController yPID; - private final JoystickButton - LBumper = new JoystickButton(mechController, XboxController.Button.kLeftBumper.value), - RBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value), - AButton = new JoystickButton(mechController, XboxController.Button.kA.value); private final GenericEntry xError, yError; @@ -121,47 +127,52 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - //construct Test - // module = new SwerveModule(6, 7, 0); - // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); - baseSwerveSubsystem = new SwerveSubsystem(); - intakePivotSubsystem = new IntakePivotSubsystem(); - shooterFeederSubsystem = new ShooterFeederSubsystem(); + //construct Test + // module = new SwerveModule(6, 7, 0); + // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); + baseSwerveSubsystem = new SwerveSubsystem(); + intakePivotSubsystem = new IntakePivotSubsystem(); + shooterFeederSubsystem = new ShooterFeederSubsystem(); - shooterPivotSubsystem = new ShooterPivotSubsystem(false); - shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); + shooterPivotSubsystem = new ShooterPivotSubsystem(false); + shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(); - climbSubsystem = new ClimbSubsystem(); - - elevatorSubsystem = new ElevatorSubsystem(); + climbSubsystem = new ClimbSubsystem(); + + elevatorSubsystem = new ElevatorSubsystem(); - xPID = new PIDController(4, 0, 0); - yPID = new PIDController(4, 0, 0); + xPID = new PIDController(4, 0, 0); + yPID = new PIDController(4, 0, 0); - traj = Choreo.getTrajectory("2mLine"); + traj = Choreo.getTrajectory("2mLine"); - swerveCrauton = Shuffleboard.getTab("Auton"); + swerveCrauton = Shuffleboard.getTab("Auton"); - xError = swerveCrauton.add("Xerror", 0).withPosition(8, 0).getEntry(); - yError = swerveCrauton.add("Yerror", 0).withPosition(9, 0).getEntry(); - if(DriverStation.getJoystickName(0).equals("Cyborg V.1")){ + xError = swerveCrauton.add("Xerror", 0).withPosition(8, 0).getEntry(); + yError = swerveCrauton.add("Yerror", 0).withPosition(9, 0).getEntry(); + if(DriverStation.getJoystickName(0).equals("Cyborg V.1")){ driveController = new DualJoystickDriveController(); } else { driveController = new XboxDriveController(); } - // Configure the trigger bindings - configureBindings(); // private final SwerveModule module; //construct Test // module = new SwerveModule(6, 7, 0); // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module); + + noteDetector = new NoteDetectionWrapper(NOTE_CAMERA); + + camera1 = new UsbCamera("camera1", 0); camera1.setFPS(60); camera1.setBrightness(45); camera1.setResolution(176, 144); mjpgserver1 = new MjpegServer("m1", 1181); mjpgserver1.setSource(camera1); + + // Configure the trigger bindings + configureBindings(); } @@ -199,14 +210,27 @@ private void configureBindings() { ledSubsystem.setDriverHeading(driveController.getRelativeMode() ? 0 : -swerveSubsystem.getDriverHeading().getRadians()); }, ledSubsystem)); - driveController.getAmpAlign().onTrue(AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem)); + driveController.getAmpAlign().onTrue(new ParallelRaceGroup( + AlignCommand.getAlignCommand(AutoAlignConstants.BLUE_AMP_POSE, swerveSubsystem), + new ConditionalWaitCommand(() -> !driveController.getAmpAlign().getAsBoolean()) + )); + driveController.getNoteAlign().onTrue(new ParallelRaceGroup( + new AutoIntakeSequence(elevatorSubsystem, intakeRollerSubsystem, swerveSubsystem, noteDetector) + .unless(() -> noteDetector.getNote().isEmpty()), + new ConditionalWaitCommand(() -> !driveController.getNoteAlign().getAsBoolean()) + )); driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem)); swerveSubsystem.setDefaultCommand(new RunCommand(() -> { if(driveController.getRelativeMode()){ swerveSubsystem.setRobotRelativeDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower()); } else { + if(driveController.getTurnMode()){ + swerveSubsystem.setAimMode(driveController.getForwardPower(), driveController.getLeftPower()); + } + else{ swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower(), driveController.getRotatePower()); + } } // pivotSubsystem.setFieldPosition(swerveSubsystem.getRobotPosition()); xError.setValue(xPID.getPositionError()); @@ -246,10 +270,6 @@ private void configureBindings() { swerveSubsystem.setDrivePowers(driveController.getForwardPower(), driveController.getLeftPower());//, 1 * (controller.getRightTriggerAxis() - controller.getLeftTriggerAxis())); } , swerveSubsystem)); - - driveController.getFieldResetButton().onTrue(new InstantCommand(() -> { - swerveSubsystem.toggletoRun(); - })); } diff --git a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java b/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java index 4551cc6b..79e88c70 100644 --- a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java +++ b/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java @@ -5,7 +5,7 @@ public class DriveForwardCommand extends Command{ private final SwerveSubsystem swerve; - private int xpower; + private double xpower = .3; public DriveForwardCommand(SwerveSubsystem swerve){ @@ -14,7 +14,7 @@ public DriveForwardCommand(SwerveSubsystem swerve){ } @Override - public void initialize() { + public void execute() { swerve.setRobotRelativeDrivePowers(xpower, 0, 0); } diff --git a/src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java b/src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java new file mode 100644 index 00000000..918975d7 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/AutoIntakeSequence.java @@ -0,0 +1,30 @@ +package frc.robot.commands.sequences; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.auton.DriveForwardCommand; +import frc.robot.commands.elevator.ElevatorToGroundCommand; +import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; +import frc.robot.commands.swerve.NoteAlignCommand; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.intake.IntakeRollersSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.vision.NoteDetectionWrapper; + +public class AutoIntakeSequence extends SequentialCommandGroup { + + + public AutoIntakeSequence( + ElevatorSubsystem elevatorSubsystem, + IntakeRollersSubsystem intakeRollersSubsystem, + SwerveSubsystem swerveSubsystem, + NoteDetectionWrapper noteDetector + ){ + addCommands(new ElevatorToGroundCommand(elevatorSubsystem) + .andThen(new NoteAlignCommand(swerveSubsystem, noteDetector)) + .andThen(new ParallelDeadlineGroup( + new IntakeRollerIntakeCommand(intakeRollersSubsystem).withTimeout(3), + new DriveForwardCommand(swerveSubsystem))) + ); + } +} diff --git a/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java new file mode 100644 index 00000000..da3cf559 --- /dev/null +++ b/src/main/java/frc/robot/commands/swerve/NoteAlignCommand.java @@ -0,0 +1,63 @@ +package frc.robot.commands.swerve; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; + +import java.util.NoSuchElementException; + +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.vision.NoteDetectionWrapper; + +public class NoteAlignCommand extends Command{ + private final SwerveSubsystem swerveSubsystem; + private final NoteDetectionWrapper noteDetector; + + private double noteYawOffsetDegrees; + + public NoteAlignCommand(SwerveSubsystem swerveSubsystem, NoteDetectionWrapper noteDetector) { + this.swerveSubsystem = swerveSubsystem; + this.noteDetector = noteDetector; + + this.addRequirements(swerveSubsystem); + } + + @Override + public void initialize() { + System.out.println("Started NoteAlignCommand"); + + this.noteYawOffsetDegrees = 0; + try { + noteYawOffsetDegrees = noteDetector.getNote().get().getYaw(); + } catch(NoSuchElementException e) { + System.out.println("Tried to align to a note, but none was detected."); + this.end(true); + } + + System.out.println("Rotating to note at offset " + this.noteYawOffsetDegrees + " degrees"); + + } + + @Override + public boolean isFinished() { + return Math.abs(noteYawOffsetDegrees) < 1; + } + + @Override + public void execute() { + try { + noteYawOffsetDegrees = noteDetector.getNote().get().getYaw(); + } catch(NoSuchElementException e) { + + } + + swerveSubsystem.setRobotRelativeDrivePowers(0.,0., -MathUtil.clamp(noteYawOffsetDegrees * .008, -.3, .3)); + } + + @Override + public void end(boolean interrupted) { + swerveSubsystem.setRobotRelativeDrivePowers(0, 0, 0); + System.out.println("Ended NoteAlignCommand"); + } + + +} diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java index e504a1a2..47387b19 100644 --- a/src/main/java/frc/robot/controllers/BaseDriveController.java +++ b/src/main/java/frc/robot/controllers/BaseDriveController.java @@ -31,5 +31,9 @@ public abstract class BaseDriveController { public abstract JoystickButton getAmpAlign(); + public abstract JoystickButton getNoteAlign(); + public abstract JoystickButton getSwerveStop(); + + public abstract Boolean getTurnMode(); } diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java index 69f9354f..a91e533a 100644 --- a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java +++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java @@ -21,8 +21,8 @@ public class DualJoystickDriveController extends BaseDriveController { private final Joystick rightJoystick = new Joystick(1); private final JoystickButton rightTrigger = new JoystickButton(rightJoystick, 1), - right2 = new JoystickButton(rightJoystick, 2), - rightTopLeftButton = new JoystickButton(rightJoystick, 3), + rightBottomButton = new JoystickButton(rightJoystick, 2), + rightTopButton = new JoystickButton(rightJoystick, 3), rightTopRightButton = new JoystickButton(rightJoystick, 4), rightMiddleLeftButton = new JoystickButton(rightJoystick, 5), rightMiddleRightButton = new JoystickButton(rightJoystick, 6), @@ -66,7 +66,7 @@ private double getTurnScaling() { } public JoystickButton getFieldResetButton() { - return right2; + return rightTopButton; } public JoystickButton getRightBumper(){ @@ -82,10 +82,18 @@ public Boolean getRelativeMode(){ } public JoystickButton getAmpAlign(){ - return leftTrigger; + return leftTopLeftButton; } - public JoystickButton getSwerveStop(){ + public JoystickButton getNoteAlign() { return leftMiddleButton; } + + public JoystickButton getSwerveStop(){ + return leftTrigger; + } + public Boolean getTurnMode(){ + return leftTopRightButton.getAsBoolean(); + } + } diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java index e209f1a9..721932b6 100644 --- a/src/main/java/frc/robot/controllers/XboxDriveController.java +++ b/src/main/java/frc/robot/controllers/XboxDriveController.java @@ -56,7 +56,15 @@ public JoystickButton getAmpAlign() { return driveXButton; } + public JoystickButton getNoteAlign() { + return driveYButton; + } + public JoystickButton getSwerveStop() { return driveBButton; } + + public Boolean getTurnMode(){ + return null; + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 37b9c41a..a3e1fff1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.swerve; +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkMax; // import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; @@ -13,6 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import frc.robot.subsystems.swerve.drivemotors.FalconDriveMotor; import frc.robot.subsystems.swerve.drivemotors.SwerveDriveMotor; @@ -103,8 +107,8 @@ public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean vor crimor = new Timer(); crimor.start(); - this.offsetRads = offsetRads; + } /** diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index bddb829e..5781bcbc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -1,12 +1,14 @@ package frc.robot.subsystems.swerve; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Nat; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -23,10 +25,9 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.vision.PhotonWrapper; +import frc.robot.vision.ApriltagWrapper; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.util.Util; -import frc.robot.vision.PhotonWrapper; import frc.robot.Constants; import static frc.robot.Constants.SwerveConstants.*; @@ -38,6 +39,8 @@ import org.photonvision.EstimatedRobotPose; +import javax.swing.text.html.Option; + import com.choreo.lib.Choreo; import com.choreo.lib.ChoreoTrajectory; import com.kauailabs.navx.frc.AHRS; @@ -73,7 +76,10 @@ public class SwerveSubsystem extends BaseSwerveSubsystem{ private final SwerveDrivePoseEstimator poseEstimator; private final SwerveDriveKinematics kinematics; - private final PhotonWrapper photonWrapper; + private final ApriltagWrapper apriltagWrapper; + + //heading lock controller + private final PIDController thetaController; private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); private final NetworkTable networkTable = inst.getTable("Testing"); @@ -103,6 +109,8 @@ public class SwerveSubsystem extends BaseSwerveSubsystem{ // private final GenericEntry FLsteer, FLdrive, FRsteer, FRdrive, BLsteer, BLdrive, BRsteer, BRdrive; private final GenericEntry robotPos; + private boolean isRed = false; //DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + public SwerveSubsystem() { ahrs = new AHRS(SPI.Port.kMXP); @@ -113,6 +121,9 @@ public SwerveSubsystem() { kinematics = new SwerveDriveKinematics(FL_POS, FR_POS, BL_POS, BR_POS); + thetaController = new PIDController(3.5, 0, 0); + thetaController.enableContinuousInput(Math.PI, -Math.PI); + inst.startServer(); crimer = new Timer(); @@ -152,7 +163,7 @@ public SwerveSubsystem() { MatBuilder.fill(Nat.N3(), Nat.N1(), 0.1, 0.1, 0.01) ); - photonWrapper = new PhotonWrapper(FRONT_CAMERA, FRONT_CAMERA_POSE); + apriltagWrapper = new ApriltagWrapper(FRONT_CAMERA, FRONT_CAMERA_POSE); // Configure AutoBuilder AutoBuilder.configureHolonomic( this::getRobotPosition, @@ -183,7 +194,7 @@ public SwerveSubsystem() { public void periodic() { - robotPos.setValue(getRobotPosition().getX()); + robotPos.setValue(Units.radiansToDegrees(thetaController.getPositionError())); // System.out.println(" Error " + Util.twoDecimals(frontRightModule.getDriveError())); // System.out.print(" Setpoint " + Util.twoDecimals(frontRightModule.getDriveSetpoint())); // System.out.print(" Vel " + Util.twoDecimals(frontRightModule.getDriveVelocity())); @@ -208,7 +219,7 @@ public void periodic() { // BRsteer.setValue(backRightModule.getSteerAmpDraws()); // BRdrive.setValue(backRightModule.getDriveAmpDraws()); - Optional visionEstimate = photonWrapper.getRobotPose( + Optional visionEstimate = apriltagWrapper.getRobotPose( new Pose3d(field.getRobotPose()) ); @@ -315,6 +326,47 @@ public void setChassisSpeeds(double xSpeed, double ySpeed, double angleSpeed){ // System.out.println(speeds.vxMetersPerSecond); } + public void setDrivePowerswithHeadingLock(double xPower, double yPower, Rotation2d targetAngles){ + Rotation2d currentRotation = getRobotPosition().getRotation(); + double turnSpeed = thetaController.calculate(currentRotation.getRadians(), targetAngles.getRadians()); + double turnPower = MathUtil.clamp(turnSpeed / MAX_OMEGA, -1.0, 1.0); + + setDrivePowers(xPower, yPower, turnPower); + } + + public void setAimMode(double xPower, double yPower) { + double shootAngleRadians = getShootAngle(isRed); + + setDrivePowerswithHeadingLock(xPower, yPower, Rotation2d.fromRadians(shootAngleRadians)); + } + + public double getShootAngle(boolean isRed) { + double xDistance = getXfromSpeaker(isRed); + double yDistance = getYfromSpeaker(); + + double rawAngle = Math.atan2(yDistance, xDistance); + + /* atan2() returns a value from -PI to PI, so the angle must be offset by 180 deg if the speaker is in + the negative x direction (such as when the robot is on the field and aiming at the blue speaker). */ + return rawAngle; + } + + public double getYfromSpeaker(){ + return BLUE_SPEAKER_POS.getY() - getRobotPosition().getY(); + } + + public double getXfromSpeaker(boolean isRed){ + return getSpeakerPosition(isRed).getX() - getRobotPosition().getX(); + } + + public Translation2d getSpeakerPosition(boolean isRed) { + if (isRed) { + return RED_SPEAKER_POS; + } else { + return BLUE_SPEAKER_POS; + } + } + public void setSwerveModuleStates(SwerveModuleState[] states){ this.states = states; } @@ -376,9 +428,4 @@ public void resetAhrs(){ ahrs.zeroYaw(); } - public SequentialCommandGroup choreoSwerveCommand(ChoreoTrajectory traj) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'choreoSwerveCommand'"); - } - } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java index ecc5bca4..c275df0b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java @@ -68,4 +68,7 @@ public double getAmpDraw(){ return motor.getSupplyCurrent().getValueAsDouble(); } + public TalonFX getTalonMotor(){ + return motor; + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java index b44ae973..25cb336e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.swerve.drivemotors; +import com.ctre.phoenix6.hardware.TalonFX; + public interface SwerveDriveMotor { public void setVelocity(double velocity); @@ -21,4 +23,6 @@ public interface SwerveDriveMotor { public double getSetpoint(); public double getAmpDraw(); + + public TalonFX getTalonMotor(); } diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java index 3fd03168..3551c0a5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java +++ b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.swerve.drivemotors; +import com.ctre.phoenix6.hardware.TalonFX; import com.revrobotics.CANSparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; @@ -74,7 +75,16 @@ public double getSetpoint(){ return lastReference; //STUB } + public double getSetPoint(){ + return 0; //STUB + } + public double getAmpDraw(){ return motor.getOutputCurrent(); //STUB } + + public TalonFX getTalonMotor(){ + return null; + } + } diff --git a/src/main/java/frc/robot/vision/PhotonWrapper.java b/src/main/java/frc/robot/vision/ApriltagWrapper.java similarity index 84% rename from src/main/java/frc/robot/vision/PhotonWrapper.java rename to src/main/java/frc/robot/vision/ApriltagWrapper.java index cc059c71..f403192a 100644 --- a/src/main/java/frc/robot/vision/PhotonWrapper.java +++ b/src/main/java/frc/robot/vision/ApriltagWrapper.java @@ -24,10 +24,10 @@ import static frc.robot.Constants.VisionConstants.*; /** - * The PhotonWrapper class gets robot pose estimates from a given camera and publishes robot pose estimations over + * The ApriltagWrapper class gets robot pose estimates from a given camera and publishes robot pose estimations over * NetworkTables. */ -public class PhotonWrapper { +public class ApriltagWrapper { private final PhotonPoseEstimator poseEstimator; private final String name; @@ -41,7 +41,7 @@ public class PhotonWrapper { * @param camera The PhotonVision camera object. * @param cameraPose The camera's 3d transformation relative to the robot. */ - public PhotonWrapper(PhotonCamera camera, Transform3d cameraPose) { + public ApriltagWrapper(PhotonCamera camera, Transform3d cameraPose) { this.name = camera.getName(); /* Pose Estimation Setup */ @@ -62,12 +62,12 @@ public PhotonWrapper(PhotonCamera camera, Transform3d cameraPose) { /* NetworkTables Setup */ NetworkTableInstance inst = NetworkTableInstance.getDefault(); - NetworkTable poseTable = inst.getTable(POSE_TABLE_KEY); + NetworkTable visionTable = inst.getTable(VISION_TABLE_KEY); - BooleanTopic debugEnabledTopic = poseTable.getBooleanTopic("debugEnabled"); - DoubleTopic xPosTopic = poseTable.getDoubleTopic("xPos" + name); - DoubleTopic yPosTopic = poseTable.getDoubleTopic("yPos" + name); - DoubleTopic headingTopic = poseTable.getDoubleTopic("heading" + name); + BooleanTopic debugEnabledTopic = visionTable.getBooleanTopic("debugEnabled"); + DoubleTopic xPosTopic = visionTable.getDoubleTopic("xPos" + name); + DoubleTopic yPosTopic = visionTable.getDoubleTopic("yPos" + name); + DoubleTopic headingTopic = visionTable.getDoubleTopic("heading" + name); this.debugEnabled = debugEnabledTopic.subscribe(false); this.xPosPub = xPosTopic.publish(PubSubOption.keepDuplicates(true)); diff --git a/src/main/java/frc/robot/vision/NoteDetectionWrapper.java b/src/main/java/frc/robot/vision/NoteDetectionWrapper.java new file mode 100644 index 00000000..d7016b71 --- /dev/null +++ b/src/main/java/frc/robot/vision/NoteDetectionWrapper.java @@ -0,0 +1,45 @@ +package frc.robot.vision; + +import static frc.robot.Constants.VisionConstants.*; + +import java.util.Optional; + +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.BooleanTopic; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; + +public class NoteDetectionWrapper { + private final PhotonCamera camera; + + private final BooleanPublisher noteDetectedPub; + + public NoteDetectionWrapper(PhotonCamera camera) { + this.camera = camera; + + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + NetworkTable visionTable = inst.getTable(VISION_TABLE_KEY); + + BooleanTopic noteDetectedTopic = visionTable.getBooleanTopic("noteDetected"); + this.noteDetectedPub = noteDetectedTopic.publish(PubSubOption.keepDuplicates(true)); + } + + public Optional getNote() { + PhotonPipelineResult result = camera.getLatestResult(); + if (!result.hasTargets()) { + noteDetectedPub.set(false); + return Optional.empty(); + } + + PhotonTrackedTarget target = result.getBestTarget(); + + noteDetectedPub.set(true); + return Optional.of(target); + } + +}