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

sorta working / some fixes (namely funky turning after shooter)

Browse Source
This commit is contained in:
Ryan
2022-03-21 20:00:34 -06:00
parent 065e48fed1
commit 4c5106d14f
4 changed files with 40 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+26 -26
View File
@@ -46,8 +46,7 @@ public class TrackTarget extends CommandBase {
boolean isExecuted = false;
public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_swerve = swerve;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
@@ -71,22 +70,22 @@ public class TrackTarget extends CommandBase {
m_visionOdometry.setLEDs(true);
points = m_visionOdometry.getTargetPoints();
points = filterPoints(points);
// points = filterPoints(points);
Point average = VisionOdometry.averagePoint(points);
double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
output *= 2;
m_turret.runTurretWithInput(output);
double position = m_turret.m_boomBoomRotateEncoder.getPosition();
// 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);
// 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);
@@ -113,25 +112,25 @@ public class TrackTarget extends CommandBase {
}
}
public ArrayList<Point> filterPoints(ArrayList<Point> points) {
Point average = VisionOdometry.averagePoint(points);
// public ArrayList<Point> filterPoints(ArrayList<Point> points) {
// Point average = VisionOdometry.averagePoint(points);
HashMap<Point, Double> pointDistances = new HashMap<>();
double distanceSum = 0;
// HashMap<Point, Double> pointDistances = new HashMap<>();
// double distanceSum = 0;
for(Point point : points) {
Vector2D difference = new Vector2D(point);
difference.subtract(new Vector2D(average));
// for(Point point : points) {
// Vector2D difference = new Vector2D(point);
// difference.subtract(new Vector2D(average));
double mag = difference.magnitude();
distanceSum += mag;
// double mag = difference.magnitude();
// distanceSum += mag;
pointDistances.put(point, mag);
}
// pointDistances.put(point, mag);
// }
final double averageDist = distanceSum / points.size();
return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
}
// final double averageDist = distanceSum / points.size();
// return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
// }
public final double getDistance(double averageY) {
double y_rot = averageY / VisionConstants.LIME_VIXELS;
@@ -143,7 +142,8 @@ public class TrackTarget extends CommandBase {
double regressedDistance = distanceRegression(distance);
regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
SmartDashboard.putNumber("Distance from Lime 123", distance);
SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance);
return regressedDistance;
}