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

Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

Browse Source
This commit is contained in:
Ryan Manley
2022-03-16 21:18:35 -06:00
parent 411e06211c 627812e4fc
commit 988735440e
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/Constants.java
+2
View File
@@ -248,6 +248,8 @@ public final class Constants {
public static final double LIME_VIXELS = 720;
public static final double TURRET_kP = 0.1;
public static final double POINTS_THRESHOLD = 400;
public static final double RANGE = 10;
public static final double EDGE_TO_CENTER = 20;
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;