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

Speedy regression + circle fitting for target track

Browse Source
This commit is contained in:
66945
2022-03-13 21:43:14 -06:00
parent 4190491459
commit 86cc9b6ff5
2 changed files with 39 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+20 -15
View File
@@ -91,12 +91,7 @@ public class VisionOdometry extends SubsystemBase {
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 {
public Point getTargetOffset() throws VisionObscuredException {
ArrayList<Point> screenPoints = getTargetPoints();
if(screenPoints.size() < 3)
@@ -111,14 +106,25 @@ public class VisionOdometry extends SubsystemBase {
guess = iterateGuess(guess, points);
}
guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
// guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
return guess;
}
SmartDashboard.putNumber("Vision ODO x: ", guess.x);
SmartDashboard.putNumber("Vision ODO y: ", guess.y);
/** Gets estimated odometry based on limelight data
*
* @return The estimated odometry pose, including gyro rotation
* @throws VisionObscuredException
*/
public Pose2d getVisionOdometry() throws VisionObscuredException {
Point targetOffset = getTargetOffset();
targetOffset = correctGuessForCenter(targetOffset, m_shooter.getBoomBoomAngleDegrees());
targetOffset = correctGuessForGyro(targetOffset, m_drive.getRegGyro().getDegrees());
SmartDashboard.putNumber("Vision ODO x: ", targetOffset.x);
SmartDashboard.putNumber("Vision ODO y: ", targetOffset.y);
Rotation2d rotation = new Rotation2d(Math.toDegrees(m_drive.getRegGyro().getDegrees()));
Pose2d odometryPose = new Pose2d(guess.x, guess.y, rotation);
Pose2d odometryPose = new Pose2d(targetOffset.x, targetOffset.y, rotation);
return odometryPose;
}
@@ -128,8 +134,7 @@ public class VisionOdometry extends SubsystemBase {
}
public boolean getLEDs() {
if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
return true;
return m_camera.getLEDMode() != VisionLEDMode.kOff;
}
public void updateOdometryWithVision(){
@@ -141,6 +146,7 @@ public class VisionOdometry extends SubsystemBase {
err.printStackTrace();
}
}
/** Reverse 3d projects target points from screen coordinates to 3d space
* <p>
* Uses the known height of the target to project points
@@ -286,9 +292,8 @@ public class VisionOdometry extends SubsystemBase {
* @return The angle corrected for the quadrent
*/
public static final double correctQuadrent(double angle, Point guess, Point circlePoint) {
if(circlePoint.x - guess.x < 0) {
if(circlePoint.x - guess.x < 0)
return angle - Math.PI;
}
return angle;
}