diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java index 19e27f3..cc120fc 100644 --- a/src/main/java/frc4388/robot/commands/RecordDriveInput.java +++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java @@ -92,7 +92,7 @@ public class RecordDriveInput extends CommandBase { for(long millis : timedInput.keySet()) writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]); - writer.println((millis + 1) + ",0,0,0"); + //// writer.println(( + 1) + ",0,0,0"); writer.close(); } catch(Exception e) { e.printStackTrace(); diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java index 7bf4fd4..910d229 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java @@ -88,8 +88,9 @@ public class TrackTarget extends CommandBase { m_visionOdometry.setDriverMode(false); m_visionOdometry.setLEDs(true); - points = m_visionOdometry.getTargetPoints(); - // points = filterPoints(points); + //// points = m_visionOdometry.getTargetPoints(); + points = getFakePoints(); + //// points = filterPoints(points); Point average = VisionOdometry.averagePoint(points); double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS; @@ -154,6 +155,17 @@ public class TrackTarget extends CommandBase { // return (ArrayList) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList()); // } + public final ArrayList getFakePoints() { + ArrayList fakePoints = new ArrayList<>(); + + for(int i = 0; i < 10; i++) { + Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2)); + fakePoints.add(p); + } + + return fakePoints; + } + public final double getDistance(double averageY) { double y_rot = averageY / VisionConstants.LIME_VIXELS; y_rot *= Math.toRadians(VisionConstants.V_FOV);