diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9b10ffb..f2e21c4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,7 +78,7 @@ public static class GroundIntakeConstants { /** * What power to spin the intake motor with */ - public static final Measure kLowPower = Percent.of(50); + public static final Measure kLowPower = Percent.of(100); } public static class ClimberConstants { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 979c90c..e673772 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.LEDSubsystem; +import frc.robot.subsystems.Vision; import frc.robot.subsystems.LEDSubsystem.Pattern; public class Robot extends TimedRobot { @@ -58,7 +59,7 @@ public void robotInit() { m_robotContainer = new RobotContainer(RobotFrame.COMP); // Start the camera server - CameraServer.startAutomaticCapture(); + // CameraServer.startAutomaticCapture(); // Start the data logger DataLogManager.start(); @@ -82,6 +83,8 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + + SmartDashboard.putNumber("/Vision/Yaw", Vision.getInstance().getTargetYaw().getDegrees()); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 985c42d..95b8e47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,15 +57,15 @@ public RobotContainer(RobotFrame bot) { // Each bot has a different set of subsystems switch (bot) { case COMP: - setupSwerveDrive(bot); + setupIntake(); + setupSwerveDrive(bot, m_intake); setupClimber(); setupShooter(); setupIndexer(); - setupIntake(); setupGuide(); break; case M1C2: - setupSwerveDrive(bot); + setupSwerveDrive(bot, Optional.empty()); break; case DOUGHNUT: setupDifferentialDrive(); @@ -141,11 +141,11 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); } - private void setupSwerveDrive(RobotFrame bot) { + private void setupSwerveDrive(RobotFrame bot, Optional m_intake) { SwerveSubsystem drive = null; try { - drive = new SwerveSubsystem(bot); + drive = new SwerveSubsystem(bot, m_intake); } catch (Exception e) { // End the robot program if we can't initialize the swerve drive. System.err.println("Failed to initialize swerve drive"); @@ -196,6 +196,10 @@ private void setupSwerveDrive(RobotFrame bot) { // Lock Pose m_driverController.L3().whileTrue(drive.cLock()); + // Use cDumbSkedaddle() and go to a note + // m_driverController.triangle().onTrue(drive.cDumbSkedaddle()); + m_driverController.triangle().whileTrue(drive.cDumbSkedaddle()); + m_swerveDrive = Optional.of(drive); } @@ -253,8 +257,8 @@ private void setupIndexer() { private void setupGuide() { var guide = new AmpGuideSubsystem(); - m_driverController.circle().onTrue(guide.cExtend()); - m_driverController.cross().onTrue(guide.cRetract()); + // m_driverController.circle().onTrue(guide.cExtend()); + // m_driverController.cross().onTrue(guide.cRetract()); m_guide = Optional.of(guide); } diff --git a/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java b/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java index cd48164..ec16390 100644 --- a/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/GroundIntakeSubsystem.java @@ -42,7 +42,7 @@ public void periodic() { * * @return true if a note is detected, false otherwise. */ - private boolean hasNote() { + public boolean hasNote() { return irSensor.getVoltage() > 2.5; } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index a695584..82d1c1c 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -2,15 +2,19 @@ import java.io.File; import java.io.IOException; +import java.util.Optional; +import java.util.function.Supplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.Angle; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -20,15 +24,18 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot.RobotFrame; import swervelib.SwerveDrive; import swervelib.parser.SwerveParser; import static edu.wpi.first.math.util.Units.feetToMeters; +import static edu.wpi.first.units.Units.Rotation; public class SwerveSubsystem extends SubsystemBase { private final SwerveDrive m_swerveDrive; + private final Optional m_intake; /** * Creates a new SwerveSubsystem. @@ -36,7 +43,9 @@ public class SwerveSubsystem extends SubsystemBase { * @param visionSubsystem The vision subsystem to use for pose estimation. * @throws IOException If the swerve module configuration file cannot be read. */ - public SwerveSubsystem(RobotFrame bot) throws IOException { + public SwerveSubsystem(RobotFrame bot, Optional intake) throws IOException { + this.m_intake = intake; + String swerveDir; switch (bot) { case COMP: @@ -110,7 +119,8 @@ public void driveRobotRelative(ChassisSpeeds chassisSpeeds) { * @return The current gyro angle as a Rotation2d. */ public Rotation2d getHeading() { - return m_swerveDrive.getYaw(); + // return m_swerveDrive.getYaw(); + return m_swerveDrive.getOdometryHeading(); } /** @@ -157,4 +167,126 @@ public void periodic() { SmartDashboard.putNumber("Swerve/Combined Speed", Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond)); } + + private final PIDController visionPID = new PIDController(0.2, 0, 0.2 / 5.0); + private final TrapezoidProfile m_profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(0.85 * 360, 720)); + + private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + // This is 0.02 because the bot runs at 50hz (0.02 = 1/50), so one tick + double kDt = 0.02; + + private Rotation2d m_last_heading = new Rotation2d(); + + /// Uses a P-controller to minimize the supplier angle + public Command cAutoPickUp(Supplier supplier) { + cAutoPickUp(supplier, true); + } + + public Command cAutoPickUp(Supplier supplier, boolean go) { + SmartDashboard.putData("/Vision/Vision PID", visionPID); + + // visionPID.setTolerance(0.8); + visionPID.enableContinuousInput(-180, 180); + + return runEnd(() -> { + // measurement is the yaw returned from the Photonvision camera + Rotation2d measurement = m_last_heading; + m_last_heading = getHeading(); + + SmartDashboard.putNumber("/Vision/Measurement", measurement.getDegrees()); + + m_goal = new TrapezoidProfile.State(supplier.get().getDegrees() + measurement.getDegrees(), 0); + SmartDashboard.putNumber("/Vision/Goal", m_goal.position); + + Rotation2d m_rot2d_goal = new Rotation2d(m_goal.position); + Rotation2d error = getHeading().minus(m_rot2d_goal); + + m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal); + SmartDashboard.putNumber("/Vision/Setpoint", m_setpoint.position); + + double omega = visionPID.calculate(measurement.getDegrees(), m_setpoint.position); + double rev = -1 * Math.cos(error.getRadians() * (90 / 35) * (Math.PI / 180)); + + ChassisSpeeds speed; + if (go == false) { + speed = new ChassisSpeeds(0, 0, omega); + } else { + speed = new ChassisSpeeds(rev, 0, omega); + } + SmartDashboard.putNumber("/Vision/Turn Rate", omega); + + SmartDashboard.putBoolean("/Vision/Has Target", Vision.getInstance().hasTarget()); + if (!Vision.getInstance().hasTarget()) { + stop(); + return; + } + + driveRobotRelative(speed); + }, this::stop).beforeStarting(() -> { + m_last_heading = getHeading(); + }, this); + } + + public Command cFollow() { + double notepitch = Vision.getInstance().getTargetPitch(); + double angle = 60; + SmartDashboard.putNumber("/SwerveSubsystem/Closeness Angle", angle); + angle = SmartDashboard.getNumber("/SwerveSubsystem/Closeness Angle", angle); + + if (notepitch >= angle && notepitch < 90) { + return cAutoPickUp(Vision.getInstance()::getSmoothYaw, false); + } else { + return cAutoPickUp(Vision.getInstance()::getSmoothYaw); + } + } + + /// Drives forward forever + public Command cDriveReverse() { + return runEnd(() -> { + ChassisSpeeds speed = new ChassisSpeeds(-0.5, 0, 0); + driveRobotRelative(speed); + }, this::stop); + } + + public Command cTestTracking() { + return cAutoPickUp(Vision.getInstance()::getSmoothYaw); + } + + public Command cDumbSkedaddle() { + if (m_intake.isEmpty()) { + return Commands.print("Missing GroundIntake"); + } + + GroundIntakeSubsystem intake = m_intake.get(); + return Commands.race( + cAutoPickUp(Vision.getInstance()::getSmoothYaw), + intake.cRunUntilCaptured()); + } + + public double distTraveled() { + // This will be a method to see the change in distance. + // This will be checked every one second or so. + double sum = 0; + double iter = 0; + + Pose2d pose = getPose(); + + return sum; + } + + public Command cSmartSkedadle() { + + if (m_intake.isEmpty()) { + return Commands.print("Missing GroundIntake"); + } + + GroundIntakeSubsystem intake = m_intake.get(); + + // return Commands.sequence( + // Commands.parallel(cTurnToTarget.getDegrees(Vision.getInstance()::getTargetYaw)), + // Commands.race(Commands.waitSeconds(7.0), cDriveForward(), + // intake.cRunUntilCaptured())); + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..634af67 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,141 @@ +package frc.robot.subsystems; + +import java.time.Clock; +import java.util.Optional; + +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Vision extends SubsystemBase { + // APRIL Tags + // + // Absolute - PoseEstimator + // Relative - GetVisionTarget(int id) + // + // NOTES + // + // Absolute - Pose2d getNearestNote() + // Absolute - ArrayList getAllNotes() + // Absolute - void removeNotes(ArrayList notes) + // Absolute - Omniscient + // + // Relative - Optional yawNearestNote() - NEEDED + // Relative - Optional distanceNearestNote() - Later + // Relative - boolean seeNote() + // + // DriveTrain : faceNote().andThen(driveForward()) + private final PhotonCamera camera; + private static Vision instance = null; + + private Vision() { + camera = new PhotonCamera("Logi_Webcam_C920e (1)"); + } + + public static Vision getInstance() { + if (instance == null) { + instance = new Vision(); + } + return instance; + } + + // Command cSkedaddle() + // + // boolean hasNoteTarget() + + public double getTargetDist() { + PhotonPipelineResult result = camera.getLatestResult(); + boolean hasTargets = result.hasTargets(); + + if (hasTargets == true) { + PhotonTrackedTarget target = result.getBestTarget(); + + double height = 3; + // height of the camera above the ground + double cameraSetAngle = 7; + // cameraSetAngle is the angle between the vertical and 0 pitch of the camera + + double theta = target.getPitch() + cameraSetAngle; + // theta should be zero when it points straight down + // Angle between vertical and the note + + double tardist = (height / (Math.cos(theta))) * (Math.sin(theta)); + + return tardist; + } + + // List targets = result.getTargets(); + // Currently unneccessary + + else { + return -1; + } + } + + // This need to update with each tick... how? + private double m_last_timestamp = 0; + private Rotation2d m_last_heading = new Rotation2d(); + + // private double m_last_timestamp = System.currentTimeMillis(); + + // Precondition: this command only makes sense if there is a target in frame + public Optional getTargetYaw(Rotation2d robotHeading) { + PhotonPipelineResult result = camera.getLatestResult(); + + if (result.getTimestampSeconds() == m_last_timestamp) { + return Optional.of(m_last_heading); + } + + m_last_timestamp = result.getTimestampSeconds(); + + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + double yaw = target.getYaw(); + + Rotation2d yawR = new Rotation2d(Math.toRadians(-yaw)); + + return Optional.of(yawR); + } + + else { + Rotation2d temp = new Rotation2d(0); + return Optional.of(temp); + } + } + + public boolean hasTarget() { + PhotonPipelineResult result = camera.getLatestResult(); + return result.hasTargets(); + } + + public Rotation2d getSmoothYaw() { + // LinearFilter filter = LinearFilter.singlePoleIIR(0.1, 0.02); + // SmartDashboard.putNumber("/Vision/Raw Yaw", getTargetYaw().getDegrees()); + // double smooth = filter.calculate(getTargetYaw().getDegrees()); + // SmartDashboard.putNumber("/Vision/Smooth Yaw", smooth); + // return Rotation2d.fromDegrees(smooth); + + return getTargetYaw(); + } + + public double getTargetPitch() { + PhotonPipelineResult result = camera.getLatestResult(); + + if (result.hasTargets()) { + PhotonTrackedTarget target = result.getBestTarget(); + double pitch = target.getPitch(); + + return pitch; + } + + else { + double temp = 90; + return temp; + } + } +}