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
So ein PID-Regler benötigt drei Konstanten, die an das System anzupassen sind: KP(roportional), KI(ntegral), und KD(ifferential).Code:#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
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
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.Code:#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); }
Initialisierungsbeispiel:
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).Code: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; }
Ein Durchlauf in etwa so:
Eingang ist die Regeldifferenz dDistance, also Sollwert-IstwertCode://calculate Distance regulation value static double dregDistance; PIDCalculate(PIDDistance, dDistance, &dregDistance); //!!! Wenn Neutral = 15000, dann hier aufaddieren int16_t newPWM = 15000 + (int16_t) dRegDistance;
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.
Zitieren
Zitat von MechMac
Lesezeichen