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

lime align and april align working mostly

Browse Source
This commit is contained in:
aarav18
2023-03-17 15:14:53 -06:00
parent 3d8ea49aed
commit 8ebbb1ecdc
6 changed files with 109 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/Placement/LimeAlign.java
+9 -13
View File
@@ -19,8 +19,8 @@ public class LimeAlign extends PelvicInflammatoryDisease {
DoubleSupplier ds;
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds) {
super(0.7, 0.4, 0.0, 0.0, 0.04);
public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
super(0.4, 0.4, 0.0, 0.0, tolerance);
this.drive = drive;
this.lime = lime;
@@ -31,15 +31,11 @@ public class LimeAlign extends PelvicInflammatoryDisease {
@Override
public double getError() {
if (lime.getNumTapes() > 2) {
return 0.0;
}
double err = 0.0;
try {
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
System.out.println(ds.getAsDouble());
err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
} catch (NullPointerException ex) {}
return err;
@@ -48,11 +44,11 @@ public class LimeAlign extends PelvicInflammatoryDisease {
@Override
public void runWithOutput(double output) {
if (output > 0) {
output += 0.6;
} else if (output < 0) {
output -= 0.6;
}
// if (output > 0) {
// output += 0.6;
// } else if (output < 0) {
// output -= 0.6;
// }
drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
}