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); }