float computePidCorrection(MotionError* motionError, Pid* pid, float normalSpeed, float error) { // Computes the error, and limit too high value motionError->error = error; // Computes the integral error, and limit too high value motionError->integralError += motionError->error; motionError->integralError = limit(motionError->integralError, pid->maxIntegral); // Saves the error // When error increases if speed > 0 : error - previousError > 0 (Ex : error (t) = 200, error (t-1) = 150 => 50 (=> increases u) // When error decreases if speed > 0 : error - previousError < 0 (Ex : error (t) = 180, error (t-1) = 220 => -40 (=> limits u) motionError->derivativeError = motionError->error - motionError->previousError; motionError->previousError = motionError->error; // Computes PID float u = (motionError->error * pid->p + motionError->integralError * pid->i + motionError->derivativeError * pid->d); // We divide to be on cool range when defining PID constant float result = (u / PID_GLOBAL_DIVISER); // Corresponds to the normal speed which must be added result += getNormalU(normalSpeed); // Limits the value of "u" result = limit(result, PID_NEXT_VALUE_LIMIT); return result; }
/** * 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); }
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; }