Как я могу получить контроллер Cascade-PID, работающий с моим Arduino? - PullRequest
Новая
       26

Как я могу получить контроллер Cascade-PID, работающий с моим Arduino?

0 голосов
/ 23 марта 2019

Я построил квадрокоптер и попытался стабилизировать его с помощью MPU6050 (акселерометр и гироскоп) и Arduino Uno в качестве контроллера полета.
Я пытаюсь реализовать PID-контроллер Cascade, но безуспешно.Я уже реализовал PID-Loop Rate, который работал как ожидалось.Таким образом, вы можете управлять квадрокоптером, но не в угловом режиме, как в Flightcontoller со вспышкой Betaflight.

Я уже пытался ослабить вибрации, но это помогло только в режиме Rate PID.

Это ПИД-петля и расчеты.Если вы установите время цикла, оно вернет предыдущий результат вычисления PID. 

float PID::pidLoop(float setpoint, float position, float dt){
    if(isLoopTime){
        if(prevLoopTime + loopTime < millis()){
            prevLoopTime = millis();
            return calcPid(setpoint, position, dt);
        } else {
            return output;
        }
    } else {
        return calcPid(setpoint, position, dt);
    }
}


float PID::calcPid(float setpoint, float position, float dt){
    error = position - setpoint;
    errorSum += error * dt;
    dError = (error - prevError) / dt;
    prevError = error;
    output = kp * error + ki * errorSum + kd * dError;
    if(isMinMax){
        output = minMax(min, max, output);
    }
    return output;
}

Поскольку внешний цикл должен быть в 4 раза медленнее, чем внутренний цикл, я установил время обновления 0,016 секунд, потому чтоВнутренний цикл работает так же быстро, как escs, который обновляет 0,004 секунды 

//outer PID LOOP
pitchAnglePid = anglePid[pitch].pidLoop(setpoint[pitch], mpu6050.getPitchAngle(), 0.016);
rollAnglePid = anglePid[roll].pidLoop(setpoint[roll],mpu6050.getRollAngle(), 0.016);

//inner PID 
pitch_pid = ratePid[pitch].pidLoop(pitchAnglePid, mpu6050.getPitchRate(), interval);
roll_pid = ratePid[roll].pidLoop(rollAnglePid, mpu6050.getRollRate(),interval);
yaw_pid = ratePid[yaw].pidLoop(setpoint[yaw], mpu6050.getYawRate(), interval);

Результаты внутреннего цикла складываются / вычитаются из ESC. 

esc_1 = instructions[throttle]- pitch_pid + roll_pid - yaw_pid;
esc_2 = instructions[throttle]+ pitch_pid + roll_pid + yaw_pid;
esc_3 = instructions[throttle]+ pitch_pid - roll_pid - yaw_pid;
esc_4 = instructions[throttle]- pitch_pid - roll_pid + yaw_pid;

Мои коэффициенты ПИД: 

//PID Controller   //PITCH          ROLL        YAW
PID ratePid[3] =  {PID(2,0,0),      PID(2,0,0), PID(4,0,0)};
PID anglePid[2] = {PID(1,0,0,0.016),PID(1,0,0,0.016)};

Теоретически теперь он должен стабилизироваться в градусах, но Quadcopter просто переворачивается на старте без причины.Это не возможно летать на нем.Любая помощь приветствуется.Заранее спасибо

...