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

Added point filtering + added AaraVector2D

Browse Source
This commit is contained in:
66945
2022-03-16 18:58:58 -06:00
parent 2f23aed7f1
commit 627812e4fc
3 changed files with 79 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+15 -3
View File
@@ -19,6 +19,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Turret;
import frc4388.robot.subsystems.Vision;
import frc4388.robot.subsystems.VisionOdometry;
import frc4388.utility.Vector2D;
import frc4388.utility.desmos.DesmosServer;
public class TrackTarget extends CommandBase {
@@ -69,13 +70,24 @@ public class TrackTarget extends CommandBase {
try {
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
Point average = VisionOdometry.averagePoint(points);
for(Point point : points) {
Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average));
if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
points.remove(point);
}
average = VisionOdometry.averagePoint(points);
DesmosServer.putPoint("average", average);
for(int i = 0; i < points.size(); i++) {
DesmosServer.putPoint("Point" + i, points.get(i));
}
Point average = VisionOdometry.averagePoint(points);
DesmosServer.putPoint("average", average);
output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 4;
// output *= 0.5;