void updateEndMotionData(int instructionIndex, MotionEndInfo* endMotion, MotionEndDetectionParameter* parameter, int time) {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
Motion* motion = &(computationValues->motion[instructionIndex]);
// Do not analyze it during startup time
if (time < parameter->noAnalysisAtStartupTime) {
motion->oldPosition = motion->position;
return;
}
if ((time % parameter->timeRangeAnalysis) == 0) {
endMotion->integralTime++;
// the current array value is full
if ((endMotion->integralTime % MAX_HISTORY_COUNT) == 0) {
// get the next Index
endMotion->index = (endMotion->index + 1) % MAX_HISTORY_COUNT;
// clean it before using it
endMotion->absUIntegralHistory[endMotion->index] = 0;
endMotion->absDeltaPositionIntegralHistory[endMotion->index] = 0;
}
}
float absU = fabsf(motion->u);
float absDeltaPosition = fabsf(motion->position - motion->oldPosition);
// In every case
endMotion->absUIntegralHistory[endMotion->index] += absU;
endMotion->absDeltaPositionIntegralHistory[endMotion->index] += absDeltaPosition;
// To compute for next iteration
motion->oldPosition = motion->position;
// Update aggregate Values
updateAggregateValues(endMotion);
}
void clearInitialSpeeds() {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
// TODO : For continous trajectory : to change
Motion* alphaMotion = &(computationValues->motion[INSTRUCTION_ALPHA_INDEX]);
alphaMotion->currentSpeed = 0.0f;
Motion* thetaMotion = &(computationValues->motion[INSTRUCTION_THETA_INDEX]);
thetaMotion->currentSpeed = 0.0f;
}
void clearInitialSpeeds() {
PidMotion* pidMotion = getPidMotion();
PidComputationValues* computationValues = &(pidMotion->computationValues);
// TODO : For continuous trajectory : to change
PidCurrentValues* alphaCurrentValues = &(computationValues->currentValues[ALPHA]);
alphaCurrentValues->currentSpeed = 0.0f;
PidCurrentValues* thetaCurrentValues = &(computationValues->currentValues[THETA]);
thetaCurrentValues->currentSpeed = 0.0f;
}
BOOL isRobotBlocked(int instructionIndex, MotionEndInfo* endMotion, MotionEndDetectionParameter* parameter) {
if (endMotion->integralTime < parameter->timeRangeAnalysis) {
return FALSE;
}
PidMotionDefinition* motionDefinition = &(getPidMotion()->currentMotionDefinition);
MotionInstruction* localInst = &(motionDefinition->inst[instructionIndex]);
float normalU = getNormalU(localInst->speed);
float maxUIntegral = fabsf(
limit(
parameter->maxUIntegralConstantThreshold + normalU * parameter->maxUIntegralFactorThreshold
, PID_NEXT_VALUE_LIMIT
)
* parameter->timeRangeAnalysis);
if (endMotion->absUIntegral > maxUIntegral) {
return TRUE;
}
return FALSE;
}
/**
* Compute the PID when the instruction is very simple (not b spline)
*/
void simpleMotionUCompute() {
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
MotionInstruction* thetaInst = &(motionDefinition->inst[INSTRUCTION_THETA_INDEX]);
MotionInstruction* alphaInst = &(motionDefinition->inst[INSTRUCTION_ALPHA_INDEX]);
PidComputationValues* computationValues = &(pidMotion->computationValues);
Motion* thetaMotion = &(computationValues->motion[INSTRUCTION_THETA_INDEX]);
Motion* alphaMotion = &(computationValues->motion[INSTRUCTION_ALPHA_INDEX]);
MotionError* thetaErr = &(computationValues->err[INSTRUCTION_THETA_INDEX]);
MotionError* alphaErr = &(computationValues->err[INSTRUCTION_ALPHA_INDEX]);
float pidTime = computationValues->pidTime;
thetaMotion->u = computeNextPID(INSTRUCTION_THETA_INDEX, thetaInst, thetaMotion, thetaErr, pidTime);
alphaMotion->u = computeNextPID(INSTRUCTION_ALPHA_INDEX, alphaInst, alphaMotion, alphaErr, pidTime);
}
void setNextPosition(enum InstructionType instructionType,
enum MotionParameterType motionParameterType,
enum PidType pidType,
float pNextPosition,
float pa,
float pSpeed) {
initNextPositionVars(instructionType);
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
MotionInstruction* localInst = &(motionDefinition->inst[instructionType]);
localInst->nextPosition = pNextPosition;
localInst->motionParameterType = motionParameterType;
localInst->pidType = pidType;
if (pNextPosition > 0.001f) {
localInst->a = pa * A_FACTOR;
localInst->speed = pSpeed * SPEED_FACTOR;
}
// Acceleration and speed becomes negative
else if (pNextPosition < -0.001f) {
localInst->a = -pa * A_FACTOR;
localInst->speed = -pSpeed * SPEED_FACTOR;
}
// pNextPosition == 0.0f Don't change the position
else {
if (motionParameterType == MOTION_PARAMETER_TYPE_MAINTAIN_POSITION) {
localInst->a = 1.0f;
localInst->speed = 100.0f;
} else {
// Slavery free
localInst->a = 0.0f;
localInst->speed = 0.0f;
}
}
computeMotionInstruction(localInst);
// When using classic motion, we compute a U value with alpha/theta (2 main parameters), and not with spline (3 values)
pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}
/**
* @private
* Init all variables.
* @param index corresponds to INSTRUCTION_THETA_INDEX or INSTRUCTION_ALPHA_INDEX
*/
void initNextPositionVars(int index) {
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
// Initialization of MotionInstruction : TODO : do not reset it !
MotionInstruction* localInst = &(motionDefinition->inst[index]);
localInst->nextPosition = 0;
localInst->a = 0;
localInst->speed = 0;
localInst->speedMax = 0;
localInst->endSpeed = 0;
PidComputationValues* computationValues = &(pidMotion->computationValues);
// Initialization of MotionError
MotionError* localErr = &(computationValues->err[index]);
localErr->previousError = 0;
localErr->error = 0;
localErr->derivativeError = 0;
localErr->integralError = 0;
// Initialization of Motion
Motion* localMotion = &(computationValues->motion[index]);
localInst->initialSpeed = localMotion->currentSpeed;
localMotion->position = 0;
localMotion->oldPosition = 0;
localMotion->u = 0;
localInst->profileType = 0;
localInst->pidType = 0;
localInst->motionType = 0;
// Initialization of motionEnd & motionBlocked
MotionEndInfo* localEnd = &(computationValues->motionEnd[index]);
resetMotionEndData(localEnd);
}
void setNextPosition(int instructionIndex,
unsigned char motionType,
unsigned char pidType,
float pNextPosition,
float pa,
float pSpeed) {
initNextPositionVars(instructionIndex);
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
MotionInstruction* localInst = &(motionDefinition->inst[instructionIndex]);
localInst->nextPosition = pNextPosition;
localInst->motionType = motionType;
localInst->pidType = pidType;
if (pNextPosition > 0.0f) {
localInst->a = pa * A_FACTOR;
localInst->speed = pSpeed * SPEED_FACTOR;
} // Acceleration and speed becomes negative
else if (pNextPosition < 0L) {
localInst->a = -pa * A_FACTOR;
localInst->speed = -pSpeed * SPEED_FACTOR;
} // Don't change the position
else {
if (motionType == MOTION_TYPE_MAINTAIN_POSITION) {
localInst->a = 1.0f;
localInst->speed = 100.0f;
} else {
// Slavery free
localInst->a = 0.0f;
localInst->speed = 0.0f;
}
}
computeMotionInstruction(localInst);
pidMotion->currentMotionDefinition.computeU = &simpleMotionUCompute;
}
/**
* @private
* Init all variables.
* @param index corresponds to THETA or ALPHA
*/
void initNextPositionVars(int index) {
PidMotion* pidMotion = getPidMotion();
PidMotionDefinition* motionDefinition = &(pidMotion->currentMotionDefinition);
// Initialization of MotionInstruction : TODO : do not reset it !
MotionInstruction* localInst = &(motionDefinition->inst[index]);
localInst->nextPosition = 0;
localInst->a = 0;
localInst->speed = 0;
localInst->speedMax = 0;
localInst->endSpeed = 0;
PidComputationValues* computationValues = &(pidMotion->computationValues);
// Initialization of MotionError
PidMotionError* localErr = &(computationValues->errors[index]);
localErr->previousError = 0;
localErr->error = 0;
localErr->derivativeError = 0;
localErr->integralError = 0;
// Initialization of Motion
PidCurrentValues* localCurrentValues = &(computationValues->currentValues[index]);
localInst->initialSpeed = localCurrentValues->currentSpeed;
localCurrentValues->position = 0;
localCurrentValues->oldPosition = 0;
localCurrentValues->u = 0;
localInst->profileType = PROFILE_TYPE_TRAPEZE;
localInst->pidType = PID_TYPE_GO_INDEX;
localInst->motionParameterType = MOTION_PARAMETER_TYPE_MAINTAIN_POSITION;
// Initialization of motionEnd & motionBlocked
MotionEndInfo* localEnd = &(computationValues->motionEnd[index]);
resetMotionEndData(localEnd);
}
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);
}
