@Override
public void updateService(boolean forced) {
//
updatePosition(
new RobotPosition(
pos, angle.getTheta(), speed.getSpeed(), angularRate.getAngularRate(), 0.0f));
}
@Override
public void gotNewData(DeadReckoning source) {
Position curDrPos = source.getCurrentPosition();
PolarVector vector = new PolarVector(lastDrPos, curDrPos);
lastDrPos = curDrPos;
vector.setTheta(angle.getTheta());
// Set new speed
speed.setSpeed(source.getCurrentSpeed());
speed.setTimeStamp(curDrPos.getTimeStamp());
// Get new DeadReckoning theta (used to calculate GyroBias)
drAngle.setTheta(source.getCurrentTheta());
drAngle.setTimeStamp(curDrPos.getTimeStamp());
// Theta Bias
thetaBias = Math2.angularIncrement(drAngle.getTheta(), angle.getTheta());
deadReckoning.setThetaBias(thetaBias);
// Set new position base on encoder displacement
pos.add(vector);
pos.setTimeStamp(curDrPos.getTimeStamp());
//
// System.out.println(vector.toString() + "\t\t" + lastDrPos.toString() + "\t\t" +
// curDrPos.toString() + "\n");
}