Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Properly handle obscured vision

Browse Source
This commit is contained in:
nathanrsxtn
2022-04-04 17:15:36 -06:00
parent 2c75485018
commit a3602b2004
4 changed files with 54 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+1 -5
View File
@@ -9,7 +9,6 @@ import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.VisionObscuredException;
public class RotateUntilTarget extends CommandBase {
@@ -50,10 +49,7 @@ public class RotateUntilTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; }
return true;
return this.visionOdometry.getTargetPoints() == null;
// return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+44 -43
View File
@@ -6,6 +6,7 @@ package frc4388.robot.commands.ShooterCommands;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.logging.Logger;
import java.util.stream.Collector;
import java.util.stream.Collectors;
@@ -28,6 +29,7 @@ import frc4388.utility.Vector2D;
import frc4388.utility.desmos.DesmosServer;
public class TrackTarget extends CommandBase {
private static final Logger LOGGER = Logger.getLogger(TrackTarget.class.getSimpleName());
/** Creates a new TrackTarget. */
SwerveDrive m_swerve;
Turret m_turret;
@@ -88,51 +90,50 @@ public class TrackTarget extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
try {
points = m_visionOdometry.getTargetPoints();
// points = getFakePoints();
//// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2.1;
m_turret.runTurretWithInput(output);
// double position = m_turret.m_boomBoomRotateEncoder.getPosition();
// if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
// Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
// else
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
// RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
// true);
double regressedDistance = getDistance(average.y);
// ! no longer a +30 lol -aarav
double distAdj = SmartDashboard.getNumber("Distance Adjust", -35);
velocity = m_boomBoom.getVelocity(regressedDistance + distAdj);
hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
double currentHood = this.m_hood.getEncoderPosition();
targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2);
SmartDashboard.putNumber("Distance", regressedDistance - 35);
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
SmartDashboard.putNumber("Vel Target Track", velocity);
SmartDashboard.putBoolean("Target Locked", targetLocked);
} catch (Exception e){
e.printStackTrace();
points = m_visionOdometry.getTargetPoints();
if (points.isEmpty()) {
LOGGER.severe("Vision Obscured");
return;
}
// points = getFakePoints();
//// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = ((average.x + 40) - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2.1;
m_turret.runTurretWithInput(output);
// double position = m_turret.m_boomBoomRotateEncoder.getPosition();
// if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
// Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
// else
// m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
// RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
// true);
double regressedDistance = getDistance(average.y);
// ! no longer a +30 lol -aarav
double distAdj = SmartDashboard.getNumber("Distance Adjust", -35);
velocity = m_boomBoom.getVelocity(regressedDistance + distAdj);
hoodPosition = m_boomBoom.getHood(regressedDistance + distAdj);
m_boomBoom.runDrumShooterVelocityPID(velocity);
m_hood.runAngleAdjustPID(hoodPosition);
double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
double currentHood = this.m_hood.getEncoderPosition();
targetLocked = (Math.abs(currentDrumVel - velocity) < velocityTolerance) && (Math.abs(currentHood - hoodPosition) < hoodTolerance) && (output < 0.2);
SmartDashboard.putNumber("Distance", regressedDistance - 35);
SmartDashboard.putNumber("Hood Target Angle Track", hoodPosition);
SmartDashboard.putNumber("Vel Target Track", velocity);
SmartDashboard.putBoolean("Target Locked", targetLocked);
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+9 -9
View File
@@ -24,7 +24,6 @@ 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;
import frc4388.utility.VisionObscuredException;
/** Represents the limelight and odometry related functionality
* @author Daniel McGrath
@@ -61,16 +60,15 @@ public class VisionOdometry extends SubsystemBase {
* @return Vision points on the rim of the target in screen space
* @throws VisionObscuredException
*/
public ArrayList<Point> getTargetPoints() throws VisionObscuredException {
public ArrayList<Point> getTargetPoints() {
PhotonPipelineResult result = m_camera.getLatestResult();
latency = result.getLatencyMillis();
System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
if(!result.hasTargets())
throw new VisionObscuredException();
ArrayList<Point> points = new ArrayList<>();
if(!result.hasTargets())
return points;
for(PhotonTrackedTarget target : result.getTargets()) {
List<TargetCorner> corners = target.getCorners();
@@ -102,11 +100,11 @@ public class VisionOdometry extends SubsystemBase {
m_camera.setDriverMode(driverMode);
}
public Point getTargetOffset() throws VisionObscuredException {
public Point getTargetOffset() {
ArrayList<Point> screenPoints = getTargetPoints();
if(screenPoints.size() < 3)
throw new VisionObscuredException("Less than 3 vision points available");
return null;
ArrayList<Point3> points3d = get3dPoints(screenPoints);
ArrayList<Point> points = topView(points3d);
@@ -125,8 +123,10 @@ public class VisionOdometry extends SubsystemBase {
* @return The estimated odometry pose, including gyro rotation
* @throws VisionObscuredException
*/
public Pose2d getVisionOdometry() throws VisionObscuredException {
public Pose2d getVisionOdometry() {
Point targetOffset = getTargetOffset();
if (targetOffset == null)
return null;
targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees());
targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees());