Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -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 91bf5dc446
commit 330d2a7817
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/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;