visionOdo

simgui.json

@@ -51,6 +51,7 @@
       "/LiveWindow/Ungrouped/Talon FX [8]": "Motor Controller",
       "/LiveWindow/Ungrouped/Talon FX [9]": "Motor Controller",
       "/LiveWindow/Vision": "Subsystem",
+      "/LiveWindow/VisionOdometry": "Subsystem",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/JVM Memory": "Command",
       "/SmartDashboard/Scheduler": "Scheduler",

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

@@ -60,6 +60,7 @@ import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.AimToCenter;
 import frc4388.robot.commands.Shoot;
+import frc4388.robot.commands.TrackTarget;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.Intake;
@@ -132,7 +133,8 @@ public class RobotContainer {
 
     // Turret default command
 
-    m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
+    //m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
+    m_robotTurret.setDefaultCommand(new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX())));
 
     //Swerve Drive
     m_robotSwerveDrive.setDefaultCommand(
@@ -226,6 +228,13 @@ public class RobotContainer {
     new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
         .whenPressed(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))
         .whenReleased(() -> m_robotStorage.runStorage(0.0));
+
+    //Shooter
+    new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
+        .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
+
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
   }
 
   /**

src/main/java/frc4388/robot/commands/AimToCenter.java

@@ -45,8 +45,14 @@ public class AimToCenter extends CommandBase {
     m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
     m_turret.runshooterRotatePID(m_targetAngle);
 
-    // Check if limelight is within range
-    m_visionOdometry.setLEDs(Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE);
+    // Check if limelight is within range (comment out to disable vision odo)
+    if (Math.abs(m_turret.getBoomBoomAngleDegrees() - m_targetAngle) < VisionConstants.RANGE){
+      m_visionOdometry.updateOdometryWithVision();
+      m_visionOdometry.setLEDs(true);
+    }
+    else{
+      m_visionOdometry.setLEDs(false);
+    }
   }
 
   public static double angleToCenter(double x, double y, double gyro) {

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

@@ -61,7 +61,6 @@ public class SwerveDrive extends SubsystemBase {
    */
   public SwerveDrivePoseEstimator m_poseEstimator;
   public SwerveDriveOdometry m_odometry;
-  public VisionOdometry m_visionOdometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
@@ -249,16 +248,8 @@ public class SwerveDrive extends SubsystemBase {
                             modules[1].getState(), 
                             modules[2].getState(), 
                             modules[3].getState());
+  }
   
-      // Also apply vision measurements if the camera can get vision
-      try {
-        m_poseEstimator.addVisionMeasurement(
-                m_visionOdometry.getVisionOdometry(),
-                Timer.getFPGATimestamp() - m_visionOdometry.getLatency());
-      } catch (VisionObscuredException err) {
-        err.printStackTrace();
-      }
-    }
   
   /**
    * Resets pigeon.

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

@@ -20,6 +20,7 @@ import org.photonvision.targeting.TargetCorner;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
@@ -124,7 +125,20 @@ public class VisionOdometry extends SubsystemBase {
   public double getLatency() {
     return latency;
   }
+  public boolean getLEDs() {
+    if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
+    return true;
+  }
 
+  public void updateOdometryWithVision(){
+    try {
+      m_drive.m_poseEstimator.addVisionMeasurement(
+              getVisionOdometry(),
+              Timer.getFPGATimestamp() - getLatency());
+    } catch (VisionObscuredException err) {
+      err.printStackTrace();
+    } 
+  }
   /** Reverse 3d projects target points from screen coordinates to 3d space
    * <p>
    * Uses the known height of the target to project points