예제 #1
0
/**
 * @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;
}
예제 #2
0
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);
}