public void updateAcel() {
double timestamp = Timer.getFPGATimestamp();
double deltaTime = timestamp - lastTimestamp;
acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
velocity =
Vector2.add(
velocity, Vector2.rotate(Vector2.scale(acceleration, deltaTime), -getHeading()));
position = Vector2.add(position, Vector2.scale(velocity, deltaTime));
lastTimestamp = timestamp;
}
public void run() {
SmartDashboard.putString("Status", "Started");
double lastTime = Timer.getFPGATimestamp();
double time = lastTime;
double deltaTime;
while (!Thread.interrupted()) {
time = Timer.getFPGATimestamp();
deltaTime = time - lastTime;
acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
velocity = Vector2.add(velocity, Vector2.scale(acceleration, deltaTime));
position =
Vector2.add(position, Vector2.rotate(Vector2.scale(velocity, deltaTime), -getHeading()));
lastTime = time;
try {
Thread.sleep(20);
} catch (InterruptedException e) {
break;
}
}
}