コード例 #1
0
 @Override
 public void updateService(boolean forced) {
   //
   updatePosition(
       new RobotPosition(
           pos, angle.getTheta(), speed.getSpeed(), angularRate.getAngularRate(), 0.0f));
 }
コード例 #2
0
 @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");
 }