Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-08 16:28:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

updated limealign and drivetolimedistance

Browse Source
This commit is contained in:
aarav18
2023-03-16 23:08:08 -06:00
parent 94a092b7b5
commit 5ae5db7847
5 changed files with 50 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2
View File
@@ -168,5 +168,7 @@ public final class Constants {
// public static final double MID_TARGET_HEIGHT = 34.0;
public static final double MID_TAPE_HEIGHT = 24.0;
public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
}
}
src/main/java/frc4388/robot/RobotContainer.java
+8 -11
View File
@@ -92,21 +92,18 @@ public class RobotContainer {
private ConditionalCommand alignToPole = new ConditionalCommand(
new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30)
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw()),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToTape())
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true)),
emptyCommand.asProxy(),
() -> m_robotLimeLight.numTargets() <= 2
() -> m_robotLimeLight.getNumTapes() <= 2
);
// TODO: make
private ConditionalCommand alignToShelf = new ConditionalCommand(
new SequentialCommandGroup(
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false)),
emptyCommand.asProxy(),
() -> true
);
private SequentialCommandGroup alignToShelf = new SequentialCommandGroup(
new RotateToAngle(m_robotSwerveDrive, 0.0),
new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw()),
new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 30, () -> m_robotLimeLight.getDistanceToApril()) // TODO: find distance
).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
public SequentialCommandGroup place = null;
src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java
+6 -2
View File
@@ -4,6 +4,8 @@
package frc4388.robot.commands.Placement;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.RobotContainer;
@@ -17,21 +19,23 @@ public class DriveToLimeDistance extends PelvicInflammatoryDisease {
Limelight lime;
double targetDistance;
DoubleSupplier ds;
/** Creates a new DriveToLimeDistance. */
public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance) {
public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
super(0.2, 0.0, 0.0, 0.0, 1);
this.drive = drive;
this.lime = lime;
this.targetDistance = targetDistance;
this.ds = ds;
addRequirements(drive, lime);
}
@Override
public double getError() {
return lime.getHorizontalDistanceToTarget(false) - targetDistance;
return ds.getAsDouble() - targetDistance;
}
@Override
src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
+8 -3
View File
@@ -4,6 +4,8 @@
package frc4388.robot.commands.Placement;
import java.util.function.DoubleSupplier;
import edu.wpi.first.math.geometry.Translation2d;
import frc4388.robot.Constants.VisionConstants;
import frc4388.robot.commands.PelvicInflammatoryDisease;
@@ -15,11 +17,14 @@ public class LimeAlign extends PelvicInflammatoryDisease {
SwerveDrive drive;
Limelight lime;
public LimeAlign(SwerveDrive drive, Limelight lime) {
DoubleSupplier ds;
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds) {
super(0.7, 0.4, 0.0, 0.0, 0.04);
this.drive = drive;
this.lime = lime;
this.ds = ds;
addRequirements(drive, lime);
}
@@ -27,14 +32,14 @@ public class LimeAlign extends PelvicInflammatoryDisease {
@Override
public double getError() {
if (lime.numTargets() > 2) {
if (lime.getNumTapes() > 2) {
return 0.0;
}
double err = 0.0;
try {
err = lime.getLowestTargetPoint().getYaw() / (VisionConstants.H_FOV / 2);
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
} catch (NullPointerException ex) {}
return err;
src/main/java/frc4388/robot/subsystems/Limelight.java
+26 -50
View File
@@ -28,8 +28,6 @@ public class Limelight extends SubsystemBase {
public Limelight() {
cam = new PhotonCamera(VisionConstants.NAME);
cam.setDriverMode(false);
setToLimePipeline();
}
public void setLEDs(boolean on) {
@@ -54,38 +52,9 @@ public class Limelight extends SubsystemBase {
cam.setPipelineIndex(0);
}
public ArrayList<Point> getTargetPoints() {
if (!cam.isConnected()) return null;
public PhotonTrackedTarget getAprilPoint() {
setToAprilPipeline();
PhotonPipelineResult result = cam.getLatestResult();
if (!result.hasTargets()) return null;
ArrayList<Point> points = new ArrayList<>(2);
for(PhotonTrackedTarget target : result.getTargets()) {
List<TargetCorner> corners = target.getDetectedCorners();
double sumX = 0.0;
double sumY = 0.0;
double mx = 0.0;
double my = 0.0;
for (TargetCorner c : corners) {
sumX += c.x;
sumY += c.y;
}
mx = sumX / 4.0;
my = sumY / 4.0;
points.add(new Point(mx, my));
}
return points;
}
public PhotonTrackedTarget getFirstTargetPoint() {
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
@@ -95,7 +64,20 @@ public class Limelight extends SubsystemBase {
return result.getBestTarget();
}
public PhotonTrackedTarget getLowestTargetPoint() {
public double getDistanceToApril() {
PhotonTrackedTarget aprilPoint = getAprilPoint();
if (aprilPoint == null) return -1;
double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + aprilPoint.getPitch();
double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
return distanceToApril;
}
public PhotonTrackedTarget getLowestTape() {
setToLimePipeline();
if (!cam.isConnected()) return null;
PhotonPipelineResult result = cam.getLatestResult();
@@ -114,29 +96,23 @@ public class Limelight extends SubsystemBase {
return lowest;
}
public int numTargets() {
public int getNumTapes() {
setToLimePipeline();
PhotonPipelineResult result = cam.getLatestResult();
return result.getTargets().size();
}
public double getHorizontalDistanceToTarget(boolean high) {
ArrayList<Point> targetPoints = getTargetPoints();
if (targetPoints == null) return -1;
// Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
// Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
public double getDistanceToTape() {
PhotonTrackedTarget tapePoint = getLowestTape();
if (tapePoint == null) return -1;
double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
double theta = 35.0 + tapePoint.getPitch();
double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
return horizontalDistanceToTarget;
double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
return distanceToTape;
}
int ctr = 0;
@@ -146,7 +122,7 @@ public class Limelight extends SubsystemBase {
// This method will be called once per scheduler run
if (ctr % 50 == 0) {
SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
SmartDashboard.putNumber("Horizontal Distance", getDistanceToTape());
}
ctr++;