Added buttons

src/main/java/frc4388/robot/Constants.java

@@ -249,6 +249,8 @@ public final class Constants {
   }
 
   public static final class VisionConstants {
+    public static final String NAME = "limelight";
+
     public static final double FOV = 29.8; // Field of view of limelight
     public static final double TARGET_HEIGHT = 67.5;
     public static final double LIME_ANGLE = 24.7;

src/main/java/frc4388/robot/RobotContainer.java

@@ -49,6 +49,8 @@ import frc4388.robot.commands.climber.RunClimberWithTriggers;
 import frc4388.robot.commands.climber.RunLevelerWithJoystick;
 import frc4388.robot.commands.drive.DriveWithJoystick;
 import frc4388.robot.commands.drive.PlaySongDrive;
+import frc4388.robot.commands.drive.SetShooterToOdo;
+import frc4388.robot.commands.drive.VisionUpdateOdometry;
 import frc4388.robot.commands.intake.RunIntakeWithTriggers;
 import frc4388.robot.commands.shooter.CalibrateShooter;
 import frc4388.robot.commands.shooter.RunHoodWithJoystick;
@@ -70,6 +72,7 @@ import frc4388.robot.subsystems.Shooter;
 import frc4388.robot.subsystems.ShooterAim;
 import frc4388.robot.subsystems.ShooterHood;
 import frc4388.robot.subsystems.Storage;
+import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Storage.StorageMode;
 import frc4388.utility.controller.ButtonFox;
 import frc4388.utility.controller.IHandController;
@@ -98,6 +101,7 @@ public class RobotContainer {
   private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40);
   private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40);
   public final LimeLight m_robotLime = new LimeLight();
+  public final Vision m_robotVision = new Vision();
 
   /* Controllers */
   public boolean isGS = false;
@@ -221,10 +225,20 @@ public class RobotContainer {
     // extends or retracts the extender
     new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
     // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood));
-    new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
-    // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
-    // safety for climber and leveler
-    new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber));
+
+    // VOP system testing
+    new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
+    .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
+
+    new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON)
+    .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive));
+
+    // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0)));
+    // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood));
+    // // safety for climber and leveler
+    // new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON).whenPressed(new InstantCommand(m_robotClimber::setSafetyPressed, m_robotClimber)).whenReleased(new InstantCommand(m_robotClimber::setSafetyNotPressed, m_robotClimber));
+
+
     // starts tracking target
     new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff));
     // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID()));

src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java

@@ -0,0 +1,36 @@
+package frc4388.robot.commands.drive;
+
+import org.opencv.core.Mat;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class SetShooterToOdo extends CommandBase {
+  private ShooterAim m_shooterAim;
+  private Drive m_driveTrain;
+
+  private double targetAngle;
+
+  public SetShooterToOdo(ShooterAim shooterAim, Drive driveTrain) {
+    m_shooterAim = shooterAim;
+    m_driveTrain = driveTrain;
+  }
+
+  @Override
+  public void initialize() {
+    double xPos = m_driveTrain.getPose().getX();
+    double yPos = m_driveTrain.getPose().getY();
+    targetAngle = Math.atan(yPos / xPos) - m_driveTrain.getPose().getRotation().getDegrees();
+  }
+
+  @Override
+  public void execute() {
+    m_shooterAim.runshooterRotatePID(targetAngle);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return Math.abs(m_shooterAim.getShooterRotatePosition() - targetAngle) < 5;
+  }
+}

src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java

@@ -1,10 +1,13 @@
 package frc4388.robot.commands.drive;
 
+import java.util.ArrayList;
+
 import com.ctre.phoenix.sensors.PigeonIMU;
 import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 
 import org.ejml.simple.SimpleMatrix;
 import org.opencv.core.Mat;
+import org.opencv.core.Point;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -21,9 +24,10 @@ import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.LimeLight;
 import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.Vision;
 
 public class VisionUpdateOdometry extends CommandBase {
-  private LimeLight m_limeLight;
+  private Vision m_limeLight;
   private ShooterAim m_shooterAim;
   private Drive m_driveTrain;
 
@@ -39,30 +43,39 @@ public class VisionUpdateOdometry extends CommandBase {
    * @param shooterAim replace with Turret subsystem for integration with 2022
    * @param driveTrain replace with Swerve subsystem for integration with 2022
    */
-  public VisionUpdateOdometry(LimeLight limeLight, ShooterAim shooterAim, Drive driveTrain) {
+  public VisionUpdateOdometry(Vision limeLight, ShooterAim shooterAim, Drive driveTrain) {
     m_limeLight = limeLight;
     m_shooterAim = shooterAim;
     m_driveTrain = driveTrain;
     addRequirements(m_limeLight, m_shooterAim, m_driveTrain);
 
-    // Turn camera on but leave LEDs off
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
-    NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
+    // // Turn camera on but leave LEDs off
+    // NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1);
+    // NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     // Vision Processing Mode
-    m_limeLight.limeOn();
-    m_limeLight.changePipeline(0);
+    m_limeLight.setLEDs(true);
+    // m_limeLight.changePipeline(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double[] xPoints = m_limeLight.getXArray();
-    double[] yPoints = m_limeLight.getYArray();
+    ArrayList<Point> points = m_limeLight.getTargetPoints();
+    if(points.size() < 5)
+      System.out.println("not enough points");
+
+    double[] xPoints = new double[5];
+    double[] yPoints = new double[5];
+
+    for(int i = 0; i < 5; i++) {
+      xPoints[i] = points.get(i).x;
+      yPoints[i] = points.get(i).y;
+    }
 
     double[] ellipseRadii = getEllipseRadii(xPoints, yPoints);
 
@@ -82,13 +95,15 @@ public class VisionUpdateOdometry extends CommandBase {
     xPos = Math.cos(relativeAngle) * distance;
     yPos = Math.sin(relativeAngle) * distance;
     translate = new Translation2d(xPos, yPos);
+
+    Pose2d pose = new Pose2d(translate, rotation);
+    m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter
   }
 
   // http://www.lee-mac.com/5pointellipse.html
   // https://math.stackexchange.com/questions/163920/how-to-find-an-ellipse-given-five-points
   // https://towardsdatascience.com/understanding-singular-value-decomposition-and-its-application-in-data-science-388a54be95d
-  // https://www.desmos.com/calculator/ounqguxjac
-  // https://en.wikipedia.org/wiki/Five_points_determine_a_conic
+  // https://www.desmos.com/calculatoroe_points_determine_a_conic
 
   /* solves the determinant of the following matrix
    * | x0^2 x0y0 y0^2 x0 y0 1 |
@@ -200,9 +215,6 @@ public class VisionUpdateOdometry extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    Pose2d pose = new Pose2d(translate, rotation);
-    m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter
-
-    return false;
+    return true;
   }
 }

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.subsystems;
+import java.util.ArrayList;
+import java.util.List;
+
+import org.opencv.core.Point;
+import org.photonvision.PhotonCamera;
+import org.photonvision.common.hardware.VisionLEDMode;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
+
+public class Vision extends SubsystemBase {
+  private PhotonCamera m_camera;
+
+  public Vision() {
+    m_camera = new PhotonCamera(VisionConstants.NAME);
+  }
+
+  public ArrayList<Point> getTargetPoints() {
+    PhotonPipelineResult result = m_camera.getLatestResult();
+
+    if(!result.hasTargets())
+      return null;
+    
+    ArrayList<Point> points = new ArrayList<>();
+
+    for(PhotonTrackedTarget target : result.getTargets()) {
+      List<TargetCorner> corners = target.getCorners();
+
+      double centerY = 0;
+      for(TargetCorner corner : corners) {
+        centerY += corner.y;
+      }
+      centerY /= corners.size();
+
+      for(TargetCorner corner : corners) {
+        if(corner.y <= centerY)
+          points.add(new Point(corner.x, corner.y));
+      }
+    }
+
+    return points;
+  }
+
+  public void setLEDs(boolean on) {
+    m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+}

vendordeps/photonlib.json

@@ -0,0 +1,41 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2022.1.4",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2022.1.4",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxx86-64"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2022.1.4"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2022.1.4"
+        }
+    ]
+}