/**
* @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;
}
