s'enligner avec un apriltag fini
This commit is contained in:
@@ -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{
|
||||
|
||||
@@ -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");
|
||||
|
||||
Reference in New Issue
Block a user