mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Refactored everything for 2022 port
This commit is contained in:
aarav18
@@ -72,7 +72,8 @@ import frc4388.robot.subsystems.LED;
|
||||
// import frc4388.robot.subsystems.ShooterAim_1;
|
||||
// import frc4388.robot.subsystems.ShooterHood_1;
|
||||
// import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
// import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.VisionOdomotry;
|
||||
// import frc4388.robot.subsystems.Storage.StorageMode;
|
||||
import frc4388.utility.controller.ButtonFox;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
@@ -101,7 +102,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();
|
||||
public final VisionOdomotry m_robotVision = new VisionOdomotry();
|
||||
|
||||
/* Controllers */
|
||||
public boolean isGS = false;
|
||||
@@ -231,7 +232,7 @@ public class RobotContainer {
|
||||
// .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive));
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new VisionUpdateOdometry(m_robotVision));//, m_robotShooterAim, m_robotDrive));
|
||||
.whenPressed(() -> m_robotVision.getVisionOdometry());
|
||||
|
||||
// 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));
|
||||
|
||||
@@ -26,10 +26,11 @@ import frc4388.robot.Constants.VOPConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
// import frc4388.robot.subsystems.Drive;
|
||||
// import frc4388.robot.subsystems.ShooterAim_1;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.VisionObscuredException;
|
||||
|
||||
public class VisionUpdateOdometry extends CommandBase {
|
||||
private Vision m_limeLight;
|
||||
private VisionOdometry m_limeLight;
|
||||
// private ShooterAim_1 m_shooterAim;
|
||||
// private Drive m_driveTrain;
|
||||
|
||||
@@ -45,7 +46,7 @@ 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(Vision limeLight) {
|
||||
public VisionUpdateOdometry(VisionOdometry limeLight) {
|
||||
m_limeLight = limeLight;
|
||||
// m_shooterAim = shooterAim;
|
||||
// m_driveTrain = driveTrain;
|
||||
@@ -66,7 +67,7 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
|
||||
// Called every time the scheduler runs while the command is scheduled.
|
||||
@Override
|
||||
public void execute() {
|
||||
public void execute() throws VisionObscuredException {
|
||||
m_limeLight.setLEDs(true);
|
||||
// m_limeLight.changePipeline(5);
|
||||
|
||||
@@ -75,10 +76,9 @@ public class VisionUpdateOdometry extends CommandBase {
|
||||
// Debug power off
|
||||
m_limeLight.setLEDs(false);
|
||||
|
||||
if(!(screenPoints != null && screenPoints.size() >= 3)) {
|
||||
System.err.println("Vision Update Odometry Error: Not enough points");
|
||||
if(screenPoints.size() < 3) {
|
||||
m_limeLight.setLEDs(false);
|
||||
return;
|
||||
throw new VisionObscuredException("Not enough vision points available");
|
||||
}
|
||||
|
||||
ArrayList<Point3> points3d = get3dPoints(screenPoints);
|
||||
|
||||
@@ -1,60 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.VOPConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Vision extends SubsystemBase {
|
||||
private PhotonCamera m_camera;
|
||||
// roborio ip & port: 10.43.88.2:1735
|
||||
|
||||
public Vision() {
|
||||
m_camera = new PhotonCamera(VOPConstants.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, VOPConstants.LIME_VIXELS - corner.y));
|
||||
}
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
public void setLEDs(boolean on) {
|
||||
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,230 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* 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.opencv.core.Point3;
|
||||
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.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.VOPConstants;
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
import frc4388.utility.VisionObscuredException;
|
||||
|
||||
/** Represents the limelight and odometry related functionality
|
||||
* @author Daniel Thomas McGrath
|
||||
*/
|
||||
public class VisionOdometry extends SubsystemBase {
|
||||
// roborio ip & port: 10.43.88.2:1735
|
||||
private PhotonCamera m_camera;
|
||||
|
||||
private VOPDrive m_drive; // replace with swerve drive subsystem
|
||||
private VOPShooter m_shooter; // replace with turret subsystem
|
||||
|
||||
/** Creates VisionOdometry
|
||||
*
|
||||
* @param drive
|
||||
* @param shooter
|
||||
*/
|
||||
public VisionOdometry(VOPDrive drive, VOPShooter shooter) {
|
||||
m_camera = new PhotonCamera(VOPConstants.NAME);
|
||||
m_drive = drive;
|
||||
m_shooter = shooter;
|
||||
}
|
||||
|
||||
/** Gets the vision points from the limelight
|
||||
* <p>
|
||||
* Breaks down targets into 4 corners and uses the top 2 points
|
||||
*
|
||||
* @return Vision points on the rim of the target in screen space
|
||||
*/
|
||||
public ArrayList<Point> getTargetPoints() {
|
||||
PhotonPipelineResult result = m_camera.getLatestResult();
|
||||
|
||||
if(!result.hasTargets())
|
||||
return new ArrayList<Point>();
|
||||
|
||||
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, VOPConstants.LIME_VIXELS - corner.y));
|
||||
}
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
/** Sets LEDs on or off (duh)
|
||||
*
|
||||
* @param on LED state
|
||||
*/
|
||||
public void setLEDs(boolean on) {
|
||||
m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
|
||||
}
|
||||
|
||||
/** Gets estimated odometry based on limelight data
|
||||
*
|
||||
* @return The estimated odometry pose, including gyro rotation
|
||||
* @throws VisionObscuredException
|
||||
*/
|
||||
public Pose2d getVisionOdometry() throws VisionObscuredException {
|
||||
ArrayList<Point> screenPoints = getTargetPoints();
|
||||
|
||||
if(screenPoints.size() < 3)
|
||||
throw new VisionObscuredException("Not enough vision points available");
|
||||
|
||||
ArrayList<Point3> points3d = get3dPoints(screenPoints);
|
||||
ArrayList<Point> points = topView(points3d);
|
||||
|
||||
Point guess = averagePoint(points);
|
||||
|
||||
for(int i = 0; i < 30; i++) {
|
||||
guess = iterateGuess(guess, points);
|
||||
}
|
||||
|
||||
// TODO rotate guess for shooter & gyro
|
||||
|
||||
SmartDashboard.putNumber("Vision ODO x: ", guess.x);
|
||||
SmartDashboard.putNumber("Vision ODO y: ", guess.y);
|
||||
|
||||
Pose2d odometryPose = new Pose2d(0, 0, new Rotation2d(0));
|
||||
|
||||
return odometryPose;
|
||||
}
|
||||
|
||||
/** Reverse 3d projects target points from screen coordinates to 3d space
|
||||
* <p>
|
||||
* Uses the known height of the target to project points
|
||||
*
|
||||
* @param points2d Vision points on the rim of the target in screen space
|
||||
* @return Reverse 3d projected points
|
||||
*/
|
||||
public static final ArrayList<Point3> get3dPoints(ArrayList<Point> points2d) {
|
||||
ArrayList<Point3> points3d = new ArrayList<>();
|
||||
|
||||
for(Point point2d : points2d) {
|
||||
double y_rot = point2d.y / VOPConstants.LIME_VIXELS;
|
||||
y_rot *= Math.toRadians(VOPConstants.V_FOV);
|
||||
y_rot -= Math.toRadians(VOPConstants.V_FOV) / 2;
|
||||
y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
|
||||
|
||||
double x_rot = point2d.x / VOPConstants.LIME_HIXELS;
|
||||
x_rot *= Math.toRadians(VOPConstants.H_FOV);
|
||||
x_rot -= Math.toRadians(VOPConstants.H_FOV) / 2;
|
||||
|
||||
double z = VOPConstants.TARGET_HEIGHT / Math.tan(y_rot);
|
||||
double x = z * Math.tan(x_rot);
|
||||
double y = VOPConstants.TARGET_HEIGHT;
|
||||
|
||||
points3d.add(new Point3(x, y, z));
|
||||
}
|
||||
|
||||
return points3d;
|
||||
}
|
||||
|
||||
/** Flattens 3d points from above
|
||||
*
|
||||
* @param points3d 3d points along the target rim
|
||||
* @return An array of flattened 3d points
|
||||
*/
|
||||
public static final ArrayList<Point> topView(ArrayList<Point3> points3d) {
|
||||
ArrayList<Point> points = new ArrayList<>();
|
||||
|
||||
for(Point3 point3d : points3d) {
|
||||
points.add(new Point(point3d.x, point3d.z));
|
||||
}
|
||||
|
||||
return points;
|
||||
}
|
||||
|
||||
/** Finds the average point from an array of points
|
||||
*
|
||||
* @param points The points the average will be taken from
|
||||
* @return The average point
|
||||
*/
|
||||
public static final Point averagePoint(ArrayList<Point> points) {
|
||||
Point average = new Point(0, 0);
|
||||
for(Point point : points) {
|
||||
average.x += point.x;
|
||||
average.y += point.y;
|
||||
}
|
||||
|
||||
average.x /= points.size();
|
||||
average.y /= points.size();
|
||||
|
||||
return average;
|
||||
}
|
||||
|
||||
/** Iterates the current guess for the vision center (relative to the limelight)
|
||||
* based on points on the rim of the target
|
||||
* <p>
|
||||
* The guess is iterated by finding the current average vector error between the guess
|
||||
* and the circlePoints, assuming that the guess should be a constant radius from each point
|
||||
*
|
||||
* @param guess The current estimate for the vision center
|
||||
* @param circlePoints Vision points along the rim of the target
|
||||
* @return The guess after iteration
|
||||
*/
|
||||
public static final Point iterateGuess(Point guess, ArrayList<Point> circlePoints) {
|
||||
Point totalDiff = new Point(0, 0);
|
||||
|
||||
for(Point circlePoint : circlePoints) {
|
||||
double angle = Math.atan((guess.y - circlePoint.y) / (guess.x - circlePoint.x));
|
||||
angle = correctQuadrent(angle, guess, circlePoint);
|
||||
|
||||
Point estimate = new Point();
|
||||
estimate.x = VOPConstants.TARGET_RADIUS * Math.cos(angle) + guess.x;
|
||||
estimate.y = VOPConstants.TARGET_RADIUS * Math.sin(angle) + guess.y;
|
||||
|
||||
Point diff = new Point(estimate.x - circlePoint.x, estimate.y - circlePoint.y);
|
||||
totalDiff.x += diff.x;
|
||||
totalDiff.y += diff.y;
|
||||
}
|
||||
|
||||
totalDiff.x /= circlePoints.size();
|
||||
totalDiff.y /= circlePoints.size();
|
||||
|
||||
return new Point(guess.x - totalDiff.x, guess.y - totalDiff.y);
|
||||
}
|
||||
|
||||
/** Corrects the angle from the current center estimate to a point on the target rim
|
||||
* for multiple quadrents
|
||||
*
|
||||
* @param angle The angle to be corrected
|
||||
* @param guess The current guess for the vision center
|
||||
* @param circlePoint A point along the target rim
|
||||
* @return The angle corrected for the quadrent
|
||||
*/
|
||||
public static final double correctQuadrent(double angle, Point guess, Point circlePoint) {
|
||||
if(circlePoint.x - guess.x < 0) {
|
||||
return angle - Math.PI;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package frc4388.utility;
|
||||
|
||||
/** Exception that occurs if the limelight can't see enough points
|
||||
* @author Daniel Thomas McGrath
|
||||
*/
|
||||
public class VisionObscuredException extends RuntimeException {
|
||||
public VisionObscuredException() {
|
||||
super();
|
||||
}
|
||||
|
||||
public VisionObscuredException(String message) {
|
||||
super(message);
|
||||
}
|
||||
|
||||
public VisionObscuredException(String message, Throwable cause) {
|
||||
super(message, cause);
|
||||
}
|
||||
|
||||
public VisionObscuredException(Throwable cause) {
|
||||
super(cause);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user