/** * @private * Theta means distance management, but we need to have in mind that to know * if we are before (slower) or after (too quick) in terms of distance, we must * project the point perpendicular to the curve * The problem with that approach is that the robot could consider that everything * is ok if he is "parallel" to the curve. * @param pidMotion * @param motionDefinition * @param robotPosition * @param bSplineTime * @param normalAlpha * @return */ float bSplineMotionUComputeThetaError(PidMotion* pidMotion, PidMotionDefinition* motionDefinition, float alphaAndThetaDiff, float distanceRealAndNormalPoint) { float cosAlphaAndThetaDiff = cosf(alphaAndThetaDiff); // We do the "projection" point to determine if we are the error only // the Distance error (theta Error)) float thetaError = distanceRealAndNormalPoint * cosAlphaAndThetaDiff; // Compute the THETA PID with this data MotionInstruction* thetaInstruction = &(motionDefinition->inst[THETA]); PidComputationValues* computationValues = &(pidMotion->computationValues); float pidTime = computationValues->pidTimeInSecond; float normalPosition = computeNormalPosition(thetaInstruction, pidTime); float thetaNormalSpeed = computeNormalSpeed(thetaInstruction, pidTime); PidParameter* thetaPidParameter = getPidParameterByPidType(pidMotion, PID_TYPE_GO_INDEX); // We take GO PID, because it's relative to a distance Management PidComputationInstructionValues* thetaValues = &(computationValues->values[THETA]); thetaValues->u = computePidCorrection(thetaValues, thetaPidParameter, thetaNormalSpeed, thetaError); // For history Management PidComputationInstructionValues* thetaComputationInstructionValues = &(computationValues->values[THETA]); thetaComputationInstructionValues->error = thetaError; thetaComputationInstructionValues->normalPosition = normalPosition; thetaComputationInstructionValues->normalSpeed = thetaNormalSpeed; storePidComputationInstructionValueHistory(thetaComputationInstructionValues, pidTime); return thetaError; }
/** * @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; }
/** * @private * Computes the next value of the pid. * @param instructionIndex the pid that we want to compute (0 = alpha, 1 = theta) * @param pidType the type of pid that we want to compute (between 0 and PID_TYPE_COUNT) * @param currentPosition the current position of the wheels (either alphaPosition, either thetaPosition) * @param time the time in pid sampling */ float computeNextPID(int instructionIndex, MotionInstruction* motionInstruction, Motion* motion, MotionError* motionError, float time) { unsigned char rollingTestMode = getRollingTestMode(); unsigned char pidType = motionInstruction->pidType; float currentPosition = motion->position; // instructionIndex = Alpha / Theta // pidType = Forward / Rotation / Final Approach ... unsigned pidIndex = getIndexOfPid(instructionIndex, pidType); Pid* pid = getPID(pidIndex, rollingTestMode); if (!pid->enabled) { return 0.0f; } float normalPosition = computeNormalPosition(motionInstruction, time); float normalSpeed = computeNormalSpeed(motionInstruction, time); float positionError = normalPosition - currentPosition; float result = computePidCorrection(motionError, pid, normalSpeed, positionError); return result; }
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); }