Блог

Как только я заставил робота колебаться контролируемым образом, используя только P, я попытался добавить D, чтобы предотвратить перерегулирование. Однако большую часть времени это не имело никакого эффекта, поскольку цикл управления выполняется так быстро, что в 19 циклах из 20 скорость изменения ошибки равна 0: робот не двигался или двигался недостаточно, чтобы его можно было измерить за это время. Я напечатал изменение ошибки и производный член каждого цикла, чтобы подтвердить это, и я мог видеть, что оба они будут равны 0 примерно для 20 циклов цикла, прежде чем принимать разумное значение, а затем вернуться к 0 еще для 20 циклов. Как я уже сказал, я думаю, это связано с тем, что циклы цикла настолько быстры, что робот буквально не перемещается достаточно для какого-либо заметного изменения ошибки. Это было большой проблемой, потому что это означало, что общий член практически не влиял на движение робота, поскольку он почти всегда был равен 0. Чтобы устранить эту проблему, я попытался использовать последнее ненулевое значение производной вместо любых значений 0, но это не сработало, и робот будет колебаться случайным образом, если последняя производная не отражает текущую скорость изменения ошибки.

Примечание: я также использую небольшую прямую связь для статического коэффициента трения, и я называю эту прямую связь «f»

Должен ли я добавить задержку?

Я понял, что, по-моему, источником обеих этих проблем является цикл, выполняемый очень и очень быстро, поэтому я подумал о добавлении оператора ожидания в конце цикла. Однако, похоже, что в целом плохое решение намеренно замедлять цикл. Это хорошая идея?

 turnHeading(double finalAngle, double kp, double ki, double kd, double f){
    std::clock_t timer;
    timer = std::clock();

    double pastTime = 0;
    double currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);

    const double initialHeading = getHeading();
    finalAngle = angleWrapDeg(finalAngle);

    const double initialAngleDiff = initialHeading - finalAngle;
    double error = angleDiff(getHeading(), finalAngle);
    double pastError = error;

    double firstTimeAtSetpoint = 0;
    double timeAtSetPoint = 0;
    bool atSetpoint = false;

    double integral = 0;
    double derivative = 0;
    double lastNonZeroD = 0;

    while (timeAtSetPoint < .05)
    {
        updatePos(encoderL.read(), encoderR.read());
        error = angleDiff(getHeading(), finalAngle);

        currentTime = ((std::clock() - timer) / (double)CLOCKS_PER_SEC);
        double dt = currentTime - pastTime;

        double proportional = error / fabs(initialAngleDiff);
        integral  = dt * ((error   pastError) / 2.0);
        double derivative = (error - pastError) / dt;
        
        //FAILED METHOD OF USING LAST NON-0 VALUE OF DERIVATIVE
        // if(epsilonEquals(derivative, 0))
        // {
        //     derivative = lastNonZeroD;
        // }
        // else
        // {
        //     lastNonZeroD = derivative;
        // }

        double power = kp * proportional   ki * integral   kd * derivative;

        if (power > 0)
        {
            setMotorPowers(-power - f, power   f);
        }
        else
        {
            setMotorPowers(-power   f, power - f);
        }

        if (fabs(error) < 2)
        {
            if (!atSetpoint)
            {
                atSetpoint = true;
                firstTimeAtSetpoint = currentTime;
            }
            else //at setpoint
            {
                timeAtSetPoint = currentTime - firstTimeAtSetpoint;
            }
        }
        else //no longer at setpoint
        {
            atSetpoint = false;
            timeAtSetPoint = 0;
        }
        pastTime = currentTime;
        pastError = error;
    }
    setMotorPowers(0, 0);
}

turnHeading(90, .37, 0, .00004, .12);