NOTE vision

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<Dimensionless> kLowPower = Percent.of(50);
+        public static final Measure<Dimensionless> kLowPower = Percent.of(100);
     }
 
     public static class ClimberConstants {

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. */

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<GroundIntakeSubsystem> 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);
     }

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;
     }
 

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,23 +24,28 @@
 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<GroundIntakeSubsystem> m_intake;
 
     /**
      * Creates a new SwerveSubsystem.
      * 
      * @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<GroundIntakeSubsystem> 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<Rotation2d> supplier) {
+        cAutoPickUp(supplier, true);
+    }
+
+    public Command cAutoPickUp(Supplier<Rotation2d> 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();
+    }
 }