Merge pull request #39 from Team4388/limelight

Limelight --> Master

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

@@ -150,4 +150,23 @@ public final class Constants {
     public static final double RIGHT_AXIS_DEADBAND = 0.6;
     public static final boolean SKEW_STICKS = true; // ! this might have to actually be false, merge conflicts are confusing
   }
+
+  public static final class VisionConstants {
+    public static final String NAME = "photonCamera";
+
+    public static final int LIME_HIXELS = 640;
+    public static final int LIME_VIXELS = 480;
+
+    public static final double H_FOV = 59.6;
+    public static final double V_FOV = 45.7;
+
+    public static final double LIME_HEIGHT = 6.0;
+    public static final double LIME_ANGLE = 55.0;
+
+    // public static final double HIGH_TARGET_HEIGHT = 46.0;
+    public static final double HIGH_TAPE_HEIGHT = 44.0;
+    
+    // public static final double MID_TARGET_HEIGHT = 34.0;
+    public static final double MID_TAPE_HEIGHT = 24.0;
+  }
 }

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

@@ -7,10 +7,6 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-
-import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -21,9 +17,10 @@ import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.Claw;
+import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
-import frc4388.robot.commands.PivotCommand;
+import frc4388.robot.commands.LimeAlign;
 import frc4388.robot.commands.PlaybackChooser;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
@@ -50,6 +47,8 @@ public class RobotContainer {
 
     public final Claw m_robotClaw = new Claw(m_robotMap.servo);
 
+    public final Limelight m_limeLight = new Limelight();
+
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -153,13 +152,15 @@ public class RobotContainer {
 
         
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-            .onTrue(new PivotCommand(m_robotArm, 135));
-        
         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new PivotCommand(m_robotArm, 210));
+            .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
+
         
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+        
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+            .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
+        
+            new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)

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

@@ -38,16 +38,4 @@ public class AutoBalance extends PelvicInflammatoryDisease {
 		if (Math.abs(getError()) < 3) out2 = 0;
 		drive.driveWithInput(new Translation2d(0, out2), new Translation2d(), false);
 	}
-
-	@Override
-	public void initialize() {
-		super.initialize();
-		// this.gyro.reset();
-	}
-
-	// Returns true when the command should end.
-	@Override
-	public boolean isFinished() {
-		return false;
-	}
 }

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

@@ -4,31 +4,47 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 
-public class LimeAlign extends CommandBase {
-  public LimeAlign(SwerveDrive drive) {
-    addRequirements(drive);
-  }
+public class LimeAlign extends PelvicInflammatoryDisease {
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
+  SwerveDrive drive;
+  Limelight lime;
+
+  public LimeAlign(SwerveDrive drive, Limelight lime) {
+    super(0.7, 0.1, 0.0, 0.0, 0);
+
+    this.drive = drive;
+    this.lime = lime;
     
+    addRequirements(drive, lime);
   }
 
-  // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public double getError() {
+    double err = 0.0;
 
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
+    try {
+      err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
+    } catch (NullPointerException ex) {}
+    
+    return err;
+  }
 
-  // Returns true when the command should end.
   @Override
-  public boolean isFinished() {
-    return false;
+  public void runWithOutput(double output) {
+
+    if (output > 0) {
+      output += 0.6;
+    } else if (output < 0) {
+      output -= 0.6;
+    }
+
+    drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
   }
 }

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

@@ -4,7 +4,6 @@
 
 package frc4388.robot.commands;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.utility.Gains;
 
@@ -26,12 +25,13 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 
 	/** produces the error from the setpoint */
 	public abstract double getError();
+	
 	/** figure it out bitch */
 	public abstract void runWithOutput(double output);
 
 	// Called when the command is initially scheduled.
 	@Override 
-	public void initialize() {
+	public final void initialize() {
 		output = 0;
 	}
 
@@ -39,7 +39,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 	
 	// Called every time the scheduler runs while the command is scheduled.
 	@Override 
-	public void execute() {
+	public final void execute() {
 		double error = getError();
 		cumError += error * .02; // 20 ms
 		double delta = error - prevError;
@@ -54,7 +54,7 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
 
 	// Returns true when the command should end.
 	@Override
-	public boolean isFinished() {
+	public final boolean isFinished() {
 		return Math.abs(getError()) < tolerance;
 	}
 }

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

@@ -0,0 +1,134 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.utility.AbhiIsADumbass;
+
+public class Limelight extends SubsystemBase {
+  private PhotonCamera cam;
+
+  private boolean lightOn;
+
+  /** Creates a new Limelight. */
+  public Limelight() {
+    cam = new PhotonCamera(VisionConstants.NAME);
+    cam.setDriverMode(false);
+    
+    setToLimePipeline();
+  }
+
+  public void setLEDs(boolean on) {
+    lightOn = on;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  public void toggleLEDs() {
+    lightOn = !lightOn;
+    cam.setLED(lightOn ? VisionLEDMode.kOn : VisionLEDMode.kOff);
+  }
+
+  public void setDriverMode(boolean driverMode) {
+    cam.setDriverMode(driverMode);
+  }
+
+  public void setToLimePipeline() {
+    cam.setPipelineIndex(1);
+  }
+
+  public void setToAprilPipeline() {
+    cam.setPipelineIndex(0);
+  }
+
+  public ArrayList<Point> getTargetPoints() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    ArrayList<Point> points = new ArrayList<>(2);
+
+    for(PhotonTrackedTarget target : result.getTargets()) {
+      List<TargetCorner> corners = target.getDetectedCorners();
+  
+      double sumX = 0.0;
+      double sumY = 0.0;
+      double mx = 0.0;
+      double my = 0.0;
+  
+      for (TargetCorner c : corners) {
+        sumX += c.x;
+        sumY += c.y;
+      }
+  
+      mx = sumX / 4.0;
+      my = sumY / 4.0;
+
+      points.add(new Point(mx, my));
+    }
+
+    return points;
+  }
+
+  public PhotonTrackedTarget getFirstTargetPoint() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget();
+  }
+
+  private double getPointAngle(Point point) {
+    return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
+  }
+
+  public double getHorizontalDistanceToTarget(boolean high) {
+    ArrayList<Point> targetPoints = getTargetPoints();
+    if (targetPoints == null) return -1;
+
+    // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
+    // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
+
+    PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
+    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
+
+    double theta = 35.0 + tapePoint.getPitch();
+
+    double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
+
+    double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
+
+    return horizontalDistanceToTarget;
+  }
+
+  int ctr = 0;
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    
+    if (ctr % 50 == 0) {
+      SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
+    }
+
+    ctr++;
+
+  }
+}