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