ПИД

[eddy_em]

Все-таки дошло до меня, почему дипсик так упорно тыкал в меня "классическим" ПИД, где вычисляется именно скорость движения в следующем цикле, а не поправка. Ведь фактически "воздействие", вычисленное по формуле ПИД, дает величину смещения, которое нужно пройти за следующий цикл. Соответственно, скорость на этом этапе должна быть равна отношению "воздействия" к длительности цикла, dt.
А вот на мелких отклонениях, когда идем уже "ноздря в ноздрю", такой расчет приведет к осцилляциям. Поэтому нужно брать какую-то небольшую часть отношения "воздействия" к dt и суммировать с текущей скоростью. Вот такая ошибка получается после выхода на режим сопровождения (ошибка < 0.1 условной единицы). Шум в пределах ±0.02 имитирует дребезг младших разрядов энкодера. Скачок на t=30 - тоже имитация реального скачка (например, от порыва). В промежутке t=20÷30 координата постоянна, далее опять возвращается к движению по синусоиде.

Ну не красота ли?

Аж не верится, что такая красота получается. Покуда я для всех случаев считал v+dv(e) (т.е. постоянно вносил аддитивную компоненту, из-за чего "телескоп" быстро разгонялся, а потом "пролетал" мимо цели и осциллировал), у меня было достаточно уродливо, и ошибка редко была в допуске.

Наведение и отслеживание, 100 секунд.

А вот так оно ведет себя в самом начале:

Начало наведения: выход на режим отслеживания.

P-компонента "проскакивает" нужное положение, но D ее "утягивает" куда нужно. Вот с I ничего хорошего в данном случае не вышло: только осцилляции добавляет. Но в данном случае оно и понятно: т.к. я итог делю на dt, то фактически I-компонента становится P, P превращается в D, а D становится второй производной. Но как "натянуть" честный ПИД на вычисление скорости по координате — не представляю. Сами вычисления:

static double getNewSpeed(const moveparam_t *p, double targcoord, double dt){
    double error = targcoord - p->coord, fe = fabs(error);
    switch(state){
        case Slewing:
            if(fe < MAX_POINTING_ERR){
                pid_clear(&pid);
                state = Pointing;
                green("--> Pointing\n");
            }else{
                red("Slewing...\n");
                return (error > 0.) ? limits.max.speed : -limits.max.speed;
            }
            break;
        case Pointing:
            if(fe < MAX_GUIDING_ERR){
                pid_clear(&pid);
                state = Guiding;
                green("--> Guiding\n");
            }else if(fe > MAX_POINTING_ERR){
                red("--> Slewing\n");
                state = Slewing;
                return (error > 0.) ? limits.max.speed : -limits.max.speed;
            }
            break;
        case Guiding:
            if(fe > MAX_GUIDING_ERR){
                red("--> Pointing\n");
                state = Pointing;
            }else if(fe < G.minerr){
                    green("At target\n");
                    //pid_clear(&pid);
                    //return p->speed;
            }
            break;
    }

    red("Calculate PID\n");
    double oldi = pid.pidIarray[pid.curIidx], newi = error * dt;
    pid.pidIarray[pid.curIidx++] = oldi;
    if(pid.curIidx >= pid.pidIarrSize) pid.curIidx = 0;
    pid.integral += newi - oldi;
    double derivative = (error - pid.prev_error) / dt;
    pid.prev_error = error;
    DBG("P=%g, I=%g, D=%g", pid.kp * error, pid.integral, derivative);
    double add = (pid.kp * error + pid.ki * pid.integral + pid.kd * derivative);
    if(state == Pointing) add /= 3.;
    else if(state == Guiding) add /= 7.;
    DBG("ADD = %g; new speed = %g", add, p->speed + add);
    if(state == Guiding) return p->speed + add  / dt / 10.;
    return add / dt;
}

Вот, еще думаю: стоит ли сбрасывать предыдущие значения ПИД при переходе между режимами "сверху вниз" (т.е. с улучшением точности)?