void rotationDegree(PidMotion* pidMotion, float angleDegree, float a, float speed, OutputStream* notificationOutputStream) {
RobotKinematics* robotKinematics = getRobotKinematics();
float leftMM = rotationInDegreeToRealDistanceForLeftWheel(robotKinematics, angleDegree);
float rightMM = rotationInDegreeToRealDistanceForRightWheel(robotKinematics, angleDegree);
gotoSimplePosition(pidMotion, leftMM, rightMM, a, speed, notificationOutputStream);
}
void rightOneWheelDegree(float angleDegree, float a, float speed) {
// We multiply by 2, because, only one wheel rotates
float angleRadius = angleDegree * PI_DIVIDE_1800 * 2.0f;
RobotKinematics* robotKinematics = getRobotKinematics();
float rightWheelLengthForOnePulse = getRightWheelLengthForOnePulse(robotKinematics);
float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
float realDistanceLeft = (wheelsDistanceFromCenter * angleRadius) / rightWheelLengthForOnePulse;
gotoPosition(realDistanceLeft, 0.0f, a, speed);
}
void leftOneWheelDegree(PidMotion* pidMotion, float angleDegree, float a, float speed, OutputStream* notificationOutputStream) {
// We multiply by 2, because, only one wheel rotates
float angleRadius = degToRad(angleDegree) * 2.0f;
RobotKinematics* robotKinematics = getRobotKinematics();
float leftWheelLengthForOnePulse = getCoderLeftWheelLengthForOnePulse(robotKinematics);
float wheelsDistanceFromCenter = getCoderWheelsDistanceFromCenter(robotKinematics);
float realDistanceRight = (wheelsDistanceFromCenter * angleRadius) / leftWheelLengthForOnePulse;
gotoSimplePosition(pidMotion, 0.0f, realDistanceRight, a, speed, notificationOutputStream);
}
float rotationDegree(float angleDeciDegree, float a, float speed) {
float angleRadius = angleDeciDegree * PI_DIVIDE_1800;
RobotKinematics* robotKinematics = getRobotKinematics();
float leftWheelLengthForOnePulse = getLeftWheelLengthForOnePulse(robotKinematics);
float rightWheelLengthForOnePulse = getRightWheelLengthForOnePulse(robotKinematics);
float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
float realDistanceLeft = -(wheelsDistanceFromCenter * angleRadius) / leftWheelLengthForOnePulse;
float realDistanceRight = (wheelsDistanceFromCenter * angleRadius) / rightWheelLengthForOnePulse;
gotoPosition(realDistanceLeft, realDistanceRight, a, speed);
return angleDeciDegree;
}
float forwardMM(float distanceInMM, float a, float speed) {
RobotKinematics* robotKinematics = getRobotKinematics();
float leftWheelLengthForOnePulse = getLeftWheelLengthForOnePulse(robotKinematics);
float rightWheelLengthForOnePulse = getRightWheelLengthForOnePulse(robotKinematics);
float realDistanceLeft = distanceInMM / leftWheelLengthForOnePulse;
float realDistanceRight = distanceInMM / rightWheelLengthForOnePulse;
// Go at a position in millimeter
gotoPosition(realDistanceLeft, realDistanceRight, a, speed);
return distanceInMM;
}
/**
* @private
* Compute the Alpha part of the PID
*/
float bSplineMotionUComputeAlphaError(PidMotion* pidMotion, BSplineCurve* curve, Position* robotPosition, float normalAlpha, float alphaAndThetaDiff, float distanceRealAndNormalPoint) {
// Real Alpha
float realAlpha = robotPosition->orientation;
// Error
float alphaErrorInRadian = (normalAlpha - realAlpha);
// restriction to [-PI, PI]
alphaErrorInRadian = mod2PI(alphaErrorInRadian);
// Convert angleError into pulse equivalent
RobotKinematics* robotKinematics = getRobotKinematics();
// Convert the alpha error into a "distance" equivalence
// TODO : To review
float alphaDistanceEquivalentError = rotationInRadiansToRealDistanceForLeftWheel(robotKinematics, alphaErrorInRadian);
// But it's not finished, because even if alphaError is equal to 0, and "theta" is OK, the risk is too
// Follow a "parallel" curve to the right One
// So we try to compute a complementary distance equivalent to the distance between this "parallel curve" (the normal curve)
// and the robot curve
float additionalError = -(sinf(alphaAndThetaDiff) * distanceRealAndNormalPoint);
if (curve->backward) {
alphaDistanceEquivalentError -= additionalError;
}
else {
alphaDistanceEquivalentError += additionalError;
}
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidComputationInstructionValues* alphaValues = &(computationValues->values[ALPHA]);
// TODO : To check : not really true if the curve is strong !!
float alphaNormalSpeed = 0.0f;
// The PID Parameters used must be strong to keep the angle => We use PID_TYPE_MAINTAIN_POSITION_INDEX
PidParameter* alphaPidParameter = getPidParameterByPidType(pidMotion, PID_TYPE_MAINTAIN_POSITION_INDEX);
alphaValues->u = computePidCorrection(alphaValues, alphaPidParameter, alphaNormalSpeed, alphaDistanceEquivalentError);
// For History
PidComputationInstructionValues* alphaComputationInstructionValues = &(computationValues->values[ALPHA]);
alphaComputationInstructionValues->error = alphaDistanceEquivalentError;
alphaComputationInstructionValues->normalSpeed = alphaNormalSpeed;
alphaComputationInstructionValues->normalPosition = normalAlpha;
float pidTime = computationValues->pidTimeInSecond;
storePidComputationInstructionValueHistory(alphaComputationInstructionValues, pidTime);
return normalAlpha;
}
void bSplineMotionUCompute(void) {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
BSplineCurve* curve = &(motionDefinition->curve);
float pidTime = computationValues->pidTime;
MotionInstruction* thetaInst = &(motionDefinition->inst[THETA]);
float normalPosition = computeNormalPosition(thetaInst, pidTime);
// Computes the time of the bSpline in [0.00, 1.00]
float bSplineTime = computeBSplineTimeAtDistance(curve, normalPosition);
Point normalPoint;
// Computes the normal Point where the robot must be
computeBSplinePoint(curve, bSplineTime, &normalPoint);
// Convert normalPoint into mm space
RobotKinematics* robotKinematics = getRobotKinematics();
float wheelAverageLength = robotKinematics->wheelAverageLengthForOnePulse;
normalPoint.x = normalPoint.x * wheelAverageLength;
normalPoint.y = normalPoint.y * wheelAverageLength;
Position* robotPosition = getPosition();
Point robotPoint = robotPosition->pos;
// GET PID
unsigned pidIndex = getIndexOfPid(THETA, thetaInst->pidType);
unsigned char rollingTestMode = getRollingTestMode();
PidParameter* pidParameter = getPidParameter(pidIndex, rollingTestMode);
// ALPHA
PidMotionError* alphaMotionError = &(computationValues->errors[ALPHA]);
float normalAlpha = computeBSplineOrientationWithDerivative(curve, bSplineTime);
float realAlpha = robotPosition->orientation;
// backward management
if (curve->backward) {
realAlpha += PI;
}
float alphaError = (normalAlpha - realAlpha);
// restriction to [-PI, PI]
alphaError = mod2PI(alphaError);
// Convert angleError into pulse equivalent
float wheelsDistanceFromCenter = getWheelsDistanceFromCenter(robotKinematics);
float alphaPulseError = (-wheelsDistanceFromCenter * alphaError) / wheelAverageLength;
// THETA
PidMotionError* thetaMotionError = &(computationValues->errors[THETA]);
// thetaError must be in Pulse and not in MM
float thetaError = distanceBetweenPoints(&robotPoint, &normalPoint) / wheelAverageLength;
float thetaAngle = angleOfVector(&robotPoint, &normalPoint);
if (curve->backward) {
thetaAngle += PI;
}
float alphaAndThetaDiff = thetaAngle - normalAlpha;
// restriction to [-PI, PI]
alphaAndThetaDiff = mod2PI(alphaAndThetaDiff);
float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff);
float thetaErrorWithCos = thetaError * cosAlphaAndThetaDiff;
float normalSpeed = computeNormalSpeed(thetaInst, pidTime);
float thetaU = computePidCorrection(thetaMotionError, pidParameter, normalSpeed, thetaErrorWithCos);
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
thetaCurrentValues->u = thetaU;
// ALPHA CORRECTION
alphaPulseError *= 5.0f;
float alphaCorrection = -0.00050f * normalSpeed * thetaError * (alphaAndThetaDiff);
// float alphaCorrection = 0.0f;
alphaPulseError += alphaCorrection;
float alphaU = computePidCorrection(alphaMotionError, pidParameter, 0, alphaPulseError);
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
alphaCurrentValues->u = alphaU;
// LOG
OutputStream* out = getDebugOutputStreamLogger();
// appendStringAndDecf(out, "pt=", pidTime);
appendStringAndDecf(out, ",t=", bSplineTime);
// Normal Position
appendStringAndDecf(out, ",nx=", normalPoint.x);
appendStringAndDecf(out, ",ny=", normalPoint.y);
appendStringAndDecf(out, ",na=", normalAlpha);
// Real position
appendStringAndDecf(out, ",rx=", robotPoint.x);
appendStringAndDecf(out, ",ry=", robotPoint.y);
appendStringAndDecf(out, ",ra=", realAlpha);
// ALPHA
appendStringAndDecf(out, ",ta=", thetaAngle);
appendStringAndDecf(out, ",ae=", alphaError);
//appendStringAndDecf(out, ",atdiff=", alphaAndThetaDiff);
//appendStringAndDecf(out, ",catdiff=", cosAlphaAndThetaDiff);
appendStringAndDecf(out, ",au=", alphaU);
appendStringAndDecf(out, ",ac=", alphaCorrection);
// THETA
// appendString(out, ",np=");
// appendDecf(out, normalPosition);
appendStringAndDecf(out, ",te=", thetaError);
appendStringAndDecf(out, ",tec=", thetaErrorWithCos);
appendStringAndDecf(out, ",tu=", thetaU);
appendCRLF(out);
}
/**
* @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;
}
