s'enligner avec un apriltag fini

This commit is contained in:
Antoine PerreaultE
2025-11-24 18:36:30 -05:00
parent f0c7508873
commit 1bf8f02db2
2 changed files with 2 additions and 15 deletions

View File

@@ -74,8 +74,8 @@ public class AprilTag3 extends Command {
else if(tagId ==7){
drivetrain.setControl(drive.
withRotationalRate(tx/20).
withVelocityX((2-limelight3.derive(botx))).
withVelocityY(2-limelight3.derive(botz)));
withVelocityX(2-botx).
withVelocityY(2-boty));
}
}
else{

View File

@@ -21,19 +21,6 @@ public class Limelight3 extends SubsystemBase {
for(int port = 5800; port <=5807; port++){
PortForwarder.add(port, "limelight.local", port);
}
}
double previousValue = 0;
double previousTime = Timer.getFPGATimestamp();
public double derive(double currentValue) {
double previousValue = 0;
double previousTime = Timer.getFPGATimestamp();
double now = Timer.getFPGATimestamp();
double dt = now - previousTime;
double derivative = (currentValue - previousValue) / dt;
previousValue = currentValue;
previousTime = now;
return derivative;
}
public double[] getTargetPose(){
NetworkTable limelightTable = NetworkTableInstance.getDefault().getTable("limelight-balon");