void bSplineMotionUCompute(PidMotion* pidMotion, PidMotionDefinition* motionDefinition) {
// Distance
BSplineCurve* curve = &(motionDefinition->curve);
PidComputationValues* computationValues = &(pidMotion->computationValues);
float pidTime = computationValues->pidTimeInSecond;
MotionInstruction* thetaInstruction = &(motionDefinition->inst[THETA]);
float normalPosition = computeNormalPosition(thetaInstruction, pidTime);
float normalDistance = fabsf(normalPosition);
// Computes the time of the bSpline in [0.00, 1.00]
float bSplineTime = computeBSplineTimeAtDistance(curve, normalDistance);
Position* robotPosition = getPosition();
Point* robotPoint = &(robotPosition->pos);
// SOME PRELIMINARY COMPUTATION
float normalAlpha = bSplineMotionGetNormalAlpha(curve, bSplineTime);
Point normalPoint;
// Computes the normal Point where the robot must be
computeBSplinePoint(curve, bSplineTime, &normalPoint);
// Compute a kind of "angle" between normal Point and robot Point
float alphaAndThetaDiff = bSplineMotionComputeAlphaAndThetaDiff(robotPoint, &normalPoint, normalAlpha);
// This value is always positive (distance), so we must know if the robot is in front of or in back of this distance
float distanceRealAndNormalPoint = distanceBetweenPoints(robotPoint, &normalPoint);
// ALPHA CORRECTION
normalAlpha = bSplineMotionUComputeAlphaError(pidMotion, curve, robotPosition, normalAlpha, alphaAndThetaDiff, distanceRealAndNormalPoint);
// THETA
bSplineMotionUComputeThetaError(pidMotion, motionDefinition, alphaAndThetaDiff, distanceRealAndNormalPoint);
}
/**
* @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;
}
/**
* Print a trajectory from a Motion Instruction with a maxPidTime.
*/
void printMotionInstructionTableTrajectory(OutputStream* outputStream, enum InstructionType instructionType, MotionInstruction* instruction, float pidInterval) {
printMotionInstructionTableTrajectoryHeader(outputStream, instructionType);
float pidTime;
unsigned int index = 0;
for (pidTime = 0.0f; pidTime < instruction->t3 + pidInterval; pidTime += pidInterval) {
float pulseNormalAcceleration = computeNormalAcceleration(instruction, pidTime);
float pulseNormalSpeed = computeNormalSpeed(instruction, pidTime);
float pulseNormalPosition = computeNormalPosition(instruction, pidTime);
float pwmNormalU = getNormalU(pulseNormalSpeed);
index++;
printMotionInstructionTableTrajectoryLine(outputStream, index, pidTime, pulseNormalAcceleration, pulseNormalSpeed, pulseNormalPosition, pwmNormalU);
}
appendTableHeaderSeparatorLine(outputStream);
}
/**
* @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);
}
