Motor Positionsregelung mit Encoder

[Holomino]

Hast Du schon mal was von PID-Reglern gehört? Die sind relativ einfach in einem Controller umzusetzen.
Das hilft allerdings bei Deinem Schrägenproblem nur bedingt , weil:

"Ohne Abweichung keine Regelung!"

Vielleicht wäre für das Schrägenproblem denkbar, über einen 3-Ach-Accelerometer die Schräglage zu messen und damit einen zweiten PID-Regler zuzuschalten (Die Dinger kann man kaskadieren, also das Ergebnis mehrerer Regler je nach Anwendungsfall miteinander multiplizieren oder addieren.)

Mal ein Beispiel für eine PID-Implementierung

PID.h

#ifndef INCL_PID_H
	#define INCL_PID_H

    #include <avr/io.h> 
    #include <stdint.h>

    #define PID_SIZEOFFSET 19 // Offset in uint8_t array for data start

    typedef struct
    {
        uint8_t Depth;  //0
        double KP;      //1
        double KI;      //5
        double KD;      //9
        double Sum;     //13
        uint8_t Index;  //17
        double Buffer[];//18
    }PID_t;


    extern void InitPIDStruct(uint8_t arr[], uint16_t size);
    extern void PIDCalculate(PID_t* pid, double diff, double* result);


#endif

So ein PID-Regler benötigt drei Konstanten, die an das System anzupassen sind: KP(roportional), KI(ntegral), und KD(ifferential).
Proportional: Je größer die aktuelle Abweichung, desto größer die Gegensteuerung
Integral: Je "stabiler" (länger anhaltend) die Abweichung, desto größer die Gegensteuerung
Differential: Je nach Trend (Änderung der Abweichung) ändert sich die Gegensteuerung.



PID.c

#include <stdint.h>
#include <math.h>
#include <string.h>
#include "PID.h"



//Init array: iRead/iWrite = 0, data elements = 0, Buffer length = array length -4
void InitPIDStruct(uint8_t arr[], uint16_t size)
{
    memset( arr, 0, size);
    arr[0] = (size-PID_SIZEOFFSET) / sizeof(double);
}

double p, i, d;

void PIDCalculate(PID_t* pid, double diff, double* result)
{
    //Proportional
    p = diff * pid->KP;

    //Differential
    double previous = pid->Buffer[pid->Index];
    d = (diff - previous) * pid->KD;

    //Integral
    pid->Index = (pid->Index + 1) % pid->Depth; //Step up index
    double oldVal = pid->Buffer[pid->Index];
    pid->Buffer[pid->Index] = diff;
    pid->Sum -= oldVal;
    pid->Sum += diff;
    i = (pid->Sum/ pid->Depth) * pid->KI;

    *result = -1.0 * (p + i + d);
}

Um den integralen Anteil zu berechnen, verwende ich hier ein Array, dass die letzten 16 Werte hält. Allerdings berechne ich nicht die komplette Summe in jedem Durchlauf neu, sondern ziehe immer nur das älteste Element ab und füge das neu hinzugekommene Element hinzu.



Initialisierungsbeispiel:

static uint8_t arrDistance[16 * sizeof(double) + PID_SIZEOFFSET];
static PID_t* PIDDistance;

void Drive_Init()
{
    InitPIDStruct(arrDistance, sizeof(arrDistance));
    PIDDistance = (PID_t*) arrDistance;
    
    PIDDistance->KP = 0.6;
    PIDDistance->KI = 0.2;
    PIDDistance->KD = 0.4;
}

Vielleicht wird's hier klar, warum ich zuerst ein Array anlege und anschließend auf den Strukturtyp caste. Ich kann dadurch die Größe des Integrationspuffers in der Arraydefinition anpassen (in InitPIDStruct wird entsprechend dann das "Depth" errechnet).


Ein Durchlauf in etwa so:

    //calculate Distance regulation value
    static double dregDistance;
    PIDCalculate(PIDDistance, dDistance, &dregDistance);
   
    //!!! Wenn Neutral = 15000, dann hier aufaddieren
    int16_t newPWM = 15000 + (int16_t) dRegDistance;

Eingang ist die Regeldifferenz dDistance, also Sollwert-Istwert
Ausgang ist der Regelwert, hier als Beispiel mal die PWM.

Die drei Konstanten kP, kI und kD einzustellen, ist allerdings eine Kunst für sich. Es gibt sowohl mathematische Ansätze als auch den reinen Probieralgorithmus, in dem zuerst kP möglichst optimal eingestellt, danach kI dazugenommen und kD als optimierendes i-Tüpfelchen zum Schluss ausgetestet wird.