/** * @private * Computes and updates the position from the sp�cified * values from the coders. * @param left the value of the left coder * @param right the value of the right coder * @return 1 if the position has been updated, 0 otherwise */ int absoluteUpdateFromCoders(signed long left, signed long right, BOOL useThreshold, BOOL debug) { if (debug) { debugTrajectoryVariables("left=", left, ", right=", right); } float l = (float) left * WHEEL_LENGTH_LEFT; float r = (float) right * WHEEL_LENGTH_RIGHT; if (debug) { debugTrajectoryVariables("l=", l, ", r=", r); } float dl = l - lastLeft; float dr = r - lastRight; if (debug) { debugTrajectoryVariables("dl=", dl, ", dr=", dr); } if (useThreshold) { if (fabsf(dl) <= UPDATE_THRESHOLD && fabsf(dr) <= UPDATE_THRESHOLD) { return 0; } } // difference des distances parcourues par les roues en m float dw = r - l; // orientation finale = difference des distances / demi-distance des roues float orientation = fmod(dw / width, 2.0f * PI) + lastAngle + position.initialOrientation; // angle relatif au dernier mouvement // lastAngle is only used when we clear Coders ! float relativePositionOrientation = position.orientation; float angle = orientation - relativePositionOrientation; float meanOrientation = (orientation + relativePositionOrientation) / 2.0f; if (debug) { debugTrajectoryVariables("dw=", dw, ", orien=", orientation); debugTrajectoryVariables("angle=", angle, ",meanOrien=", meanOrientation); } // distance during last move float d = (dl + dr) / 2.0f; // project distance according the angle float k = 1.0f; if (angle != 0.0f) { float t = angle / 2.0f; k = sin(t) / t; } float dx = d * k * cosf(meanOrientation); float dy = d * k * sinf(meanOrientation); if (debug) { debugTrajectoryVariables("dx=", dx, ", dy=", dy); } // mise a jour de la position position.pos.x += dx; position.pos.y += dy; position.orientation = orientation; lastLeft = l; lastRight = r; return 1; }
/** * @private * Computes and updates the position from the specified * values from the coders. * @param left the value of the left coder * @param right the value of the right coder * @return 1 if the position has been updated, 0 otherwise */ bool absoluteUpdateFromCoders(signed long left, signed long right, bool useThreshold, bool debug) { if (debug) { debugTrajectoryVariables("left=", (float) left, ", right=", (float) right); } RobotKinematics* robotKinematics = getRobotKinematics(); float leftWheelLengthForOnePulse = getLeftWheelLengthForOnePulse(robotKinematics); float rightWheelLengthForOnePulse = getRightWheelLengthForOnePulse(robotKinematics); float l = (float) left * leftWheelLengthForOnePulse; float r = (float) right * rightWheelLengthForOnePulse; if (debug) { debugTrajectoryVariables("l=", l, ", r=", r); } float dl = l - lastLeft; float dr = r - lastRight; if (debug) { debugTrajectoryVariables("dl=", dl, ", dr=", dr); } if (useThreshold) { if (fabsf(dl) <= UPDATE_THRESHOLD && fabsf(dr) <= UPDATE_THRESHOLD) { return false; } } // difference des distances parcourues par les roues en m float dw = r - l; // orientation finale = difference des distances / demi-distance des roues float wheelsDistance = robotKinematics->wheelsDistance; float orientation = fmodf(dw / wheelsDistance, 2.0f * PI) + lastAngle + position.initialOrientation; // angle relatif au dernier mouvement // lastAngle is only used when we clear Coders ! float relativePositionOrientation = position.orientation; float angle = orientation - relativePositionOrientation; float meanOrientation = (orientation + relativePositionOrientation) / 2.0f; if (debug) { debugTrajectoryVariables("dw=", dw, ", orien=", orientation); debugTrajectoryVariables("angle=", angle, ",meanOrien=", meanOrientation); } // distance during last move float d = (dl + dr) / 2.0f; // project distance according the angle float k = 1.0f; if (angle != 0.0f) { float t = angle / 2.0f; k = sinf(t) / t; } float dx = d * k * cosf(meanOrientation); float dy = d * k * sinf(meanOrientation); if (debug) { debugTrajectoryVariables("dx=", dx, ", dy=", dy); } // update position position.pos.x += dx; position.pos.y += dy; position.orientation = orientation; lastLeft = l; lastRight = r; return true; }