Genetischer Algorithmus für QuadroCopter

[Che Guevara]

Hallo,

da mich die PID-Einstellerei meines Quadros ziemlich nervt (auch, wenns jetzt schon getan ist ;) ) möchte ich versuchen, einen genetischen Algorithmus zu programmieren (ATMEGA328P @ 16MHz in BASCOM), welcher die Parameter selbstständig einstellt. Habe mich auch schon ein bisschen in die Materie eingelesen, jedoch scheint es nicht gerade sehr einfach zu sein. Ich wollte mal Fragen, ob hier schonmal jemand einen genet. Algo. programmiert hat und mir evtl. den SourceCode mal zeigen könnte / möchte?!
Der Grundsätzliche Ablauf ist mir mittlerweile mehr oder weniger geläufig:
- N zufällige Individuen erzeugen
- diese in ihrer "Überlebenschance" bewerten mit sog. Fitness-Punkten
- 2 versch. Individuen auswählen und Rekombinieren
- diese durch Mutation verändern (Bits an best. Stellen negieren)
- diese Vorgänge wiederhole, bis eine neue Population mit N Individuen entstanden ist

Allerdings bin ich mir nicht darüber im klaren, wie genau ich das nun im Programm umsetzen kann.
1. Problem:
Wie kann ich einem Individuum eine Fitnesszahl zuortnen? Im konkreten Fall des QuadroCopters müsste ich dazu ja das Flugverhalten (sprich Aufschaukeln & Reaktionszeit) irgendwie herausfinden (nur wie!?!?)
2. Problem:
Der Prozess der Mutation ist mir eig. nicht sehr gefäufig, könnte mir das jemand mal bitte am Beispiel des QuadroCopters erklären?

Wäre nett, wenn jemand mal ein paar Worte dazu schreiben könnte, ob ich alles richtig verstanden / wiedergegeben habe.
Vielen Dank & Gruß
Chris

[rossir]

Hallo,

ASURO:
So richtig genetisch hab ich das letztlich nicht gemacht. Meine Methode hat eher Ähnlichkeit mit simulated anealing. Immerhin habe ich die Kp, Ki, und Kd Werte für den PID Controller meines ASURO Roboters automatisch bestimmt.
Folgendes sollte der PID Controller beim Versuchsaufbau leisten:
Aus den Stillstand soll sich ASURO im nächsten Moment um seine eigene Achse drehen. Dies dann mit möglichst konstanter Drehgeschwindigkeit (vSoll) für 4000 ms. Dann soll er wieder still stehen.
Die Encoderscheiben liefern dazu das nötige Feedback. (v wird hierbei einfach als Encoderticks pro Zeiteinheit bestimmt.)

Der Kurvenverlauf von vSoll ist also rechteckförmig: Zum Zeitpunkt t0 beginnt er mit einer Flanke von 0 nach vSoll. Bleibt dann 4000 ms auf dem Niveau von vSoll und endet zum Zeitpunkt t4000 mit einer Flanke von vSoll nach 0.
vIst hat natürlich eine ganz andere kurvenform und wird hautsächlich von Kp, Ki, Kd, den zu überwindenden Trägheiten und dem Ladezustand der Batterie bestimmt. vIst wird 8000 ms lang gemessen. Als Fitnessfunktion f nehme ich die Summe der Fehlerquadrate zwischen vIst und vSoll.
f: summe(i in [0, 8000]| sqr(vIst(i)-vSoll(i)))
Je kleiner die Summe der Fehlerquadrate (f) ist, desto besser sind Kp, Ki und Kd gewählt.

Der Suchprozess beginnt mit einem zunächst "nur" plausiblen Wertvektor w(0):=(Kp(0), Ki(0), Kd(0)) und einer Temperatur von T:=1.0. Die Fitness f(0) von w(0) wird bestimmt.

Loop:
Es werden z.B. 5 (oder mehr) neue zufällige Vektoren erzeugt die sich alle in der "Nähe" von w(0) befinden. Z.B. so:
w(1)=(Kp(0)+T*fp*rp, Ki(0)+T*fi*ri, Kd(0)+T*fd*rd)
Wobei rp, ri und rd Zufallszahlen sind die in [-1.0, 1.0] liegen. Und fp, fi und fd sind konstante Faktoren mit denen ich die Dynamikunterschiede zwischen Kp, Ki und Kd anpasse. (Kp darf sich im Unterschied zu Ki stark ändern. Ki liegt meiner Erfahrung nach eher in der Nähe von 0.0)
Für w(1) bis w(5) wird jeweils der oben beschriebene Versuch gemacht und dadurch f(1) bis f(5) bestimmt.
Wenn f(i) (z.B. i=4) die beste Fitness hat wird w(0):=w(i) gesetzt, die Temperatur halbiert (T:=0.5*T) und der Vorgang bei Loop: fortgesetzt.


Idee zu QuadroCopter:
Versuchsaufbau:
Der QuadroCopter ist an einem Ende einer Wippe befestigt. Die Wippe hat ein Gegengewicht so, dass die Wippe im Ruhezustand in waage ist.
Aus den Ruhezustand soll der QuadroCopter im nächsten Moment genau 1m Höhe gewinnen. Diese dann möglichst konstant (hSoll) für 4000 ms halten. Dann soll er wieder in den Ruhezustand gehen.
Ein Distanzsensor (Distanz zwischen Boden und einer Wippenseite) liefert dazu das nötige Feedback.

[Che Guevara]

Hallo,

deine Umsetzung der Problemstellung finde ich sehr gut, aber:
Was hat das ganze mit der Temperatur zu tun und warum wird diese halbiert? Benutzt du diesen Wert als zufälligen Faktor? Könntest du mir den Code zeigen?

Anscheinend habe ich mich falsch ausgedrückt: Es geht mir nicht um eine Höhenregelung des Copters, sondern um die Lagestabilisierung der 3 Achsen (Gemessen durch die Gyros und evtl. demnächt durch einen ACC). Ich möchte sozusagen das Aufschaukeln irgendwie feststellen und anhand diesen Wertes meine PID-Parameter einstellen. Nur habe ich eben momentan keine Idee, wie ich 1. zwischen der normalen Steuerung (also KEIN Aufschaukeln) und 2. dem Aufschaukeln (aufgrund zu großer Werte) bzw. der zu geringen Regelleistung unterscheiden soll. Wie erkennt also mein Programm, dass die Werte nicht optimal sind und deswegen das Flugverhalten gestört ist?!

Gru0
Chris

[rossir]

simulated annealing = simulierte Abkühlung
- Hat natürlich was mit Temperatur zu tun und ist ein heuristisches Optimierungsverfahren.
- Nein, T ist gerade kein zufälliger Faktor sondern ein systematischer. Denn durch das kleiner werdende T wird das Suchintervall/Umgebung um w(0) immer enger.
- Zufällig sind allein rp, ri und rd. Mit denen wird die Umgebung von w(0) "erkundet".
- Halbieren von T muß nicht sein man kann auch allgemein mit a abkühlen d.h. T:=a*T mit a in ]0.0, 1.0[. (a:=0.5 ist nur die bequeme Mitte.)
- Nein, einen vernünftigen ASURO Quellcode habe ich hier nicht an zu bieten. Denn der ist heute "verseucht" mit "tausend" kleinen Abkürzungen die das zu Grunde liegende simulated annealing Prinzip leider nur verschleiern.

Aber mein C-code sieht ca. wie folgt aus:

typedef struct {double Kp, Ki, Kd;} Pid;

int simulated_annealing(int N) {
int test;
Pid w0, wcandidate={1000, 500, 30};
double f0, fcandidate;
double T=1.0;
double fp=50, fi=10, fd=6;
double a=0.99;
fcandidate=fitness(wcandidate);
w0=wcandidate;
f0=fcandidate;
for(test=0; fcandidate>10; test++) {
int i;
for(i=0; i<N; i++) {
Pid wi={w0.Kp + T*fp*r(), w0.Ki + T*fi*r(), w0.Kd + T*fd*r()};
double f=fitness(wi);
if (f<fcandidate) {
fcandidate=f;
wcandidate=wi;
}
}
if (fcandidate<f0) T*=a;
w0=wcandidate;
f0=fcandidate;
printf("%d T=%.4f, fitness=%4.1f: Kp=%6.1f, Ki=%6.1f, Kd=%6.1f\n", test, T, f0, w0.Kp, w0.Ki, w0.Kd);
}
}


Und ausgehend von Kp= 1000, Ki= 500 und Kd= 30 auf der Suche nach den (angenommenen) optimalen Parametern z.B. Kp= 845.3, Ki= 555.2 und Kd= 4 liefert ein Beispiellauf folgende Ausgabe:


0 T=0.9900, fitness=14650.0: Kp= 950.7, Ki= 501.5, Kd= 29.4
1 T=0.9801, fitness=8098.1: Kp= 920.4, Ki= 510.6, Kd= 25.8
2 T=0.9703, fitness=3977.0: Kp= 881.8, Ki= 507.4, Kd= 22.8
3 T=0.9606, fitness=2280.7: Kp= 840.4, Ki= 512.2, Kd= 24.1
4 T=0.9510, fitness=2254.4: Kp= 863.2, Ki= 515.8, Kd= 23.5
5 T=0.9415, fitness=2073.1: Kp= 824.1, Ki= 520.2, Kd= 23.9
6 T=0.9321, fitness=1795.5: Kp= 855.7, Ki= 521.1, Kd= 27.0
7 T=0.9321, fitness=1795.5: Kp= 855.7, Ki= 521.1, Kd= 27.0
8 T=0.9227, fitness=1782.5: Kp= 843.6, Ki= 523.9, Kd= 32.3
9 T=0.9135, fitness=1075.5: Kp= 842.3, Ki= 532.9, Kd= 27.9
10 T=0.9044, fitness=807.9: Kp= 857.9, Ki= 539.5, Kd= 24.0
11 T=0.8953, fitness=418.5: Kp= 842.2, Ki= 544.8, Kd= 21.4
12 T=0.8953, fitness=418.5: Kp= 842.2, Ki= 544.8, Kd= 21.4
13 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
14 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
15 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
16 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
17 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
18 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
19 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
20 T=0.8864, fitness=381.6: Kp= 840.5, Ki= 552.3, Kd= 22.7
21 T=0.8775, fitness=332.9: Kp= 845.7, Ki= 545.4, Kd= 19.4
22 T=0.8775, fitness=332.9: Kp= 845.7, Ki= 545.4, Kd= 19.4
23 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
24 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
25 T=0.8687, fitness=213.1: Kp= 846.1, Ki= 554.1, Kd= 18.5
26 T=0.8601, fitness=205.8: Kp= 838.1, Ki= 558.9, Kd= 15.8
27 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
28 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
29 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
30 T=0.8515, fitness=134.7: Kp= 838.6, Ki= 557.0, Kd= 13.3
31 T=0.8429, fitness=82.4: Kp= 851.1, Ki= 557.6, Kd= 10.6
32 T=0.8345, fitness=68.6: Kp= 840.1, Ki= 560.9, Kd= 6.8
33 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
34 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
35 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
36 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
37 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
38 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
39 T=0.8262, fitness=67.7: Kp= 850.8, Ki= 555.5, Kd= 10.1
40 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
41 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
42 T=0.8179, fitness=65.1: Kp= 838.7, Ki= 556.1, Kd= 8.5
43 T=0.8097, fitness=60.7: Kp= 838.8, Ki= 551.1, Kd= 5.5
44 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
45 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
46 T=0.8016, fitness=42.1: Kp= 847.0, Ki= 549.2, Kd= 2.3
47 T=0.7936, fitness=23.1: Kp= 847.1, Ki= 552.0, Kd= 7.1
48 T=0.7936, fitness=23.1: Kp= 847.1, Ki= 552.0, Kd= 7.1
49 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
50 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
51 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
52 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
53 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
54 T=0.7857, fitness=20.8: Kp= 842.7, Ki= 551.7, Kd= 2.7
55 T=0.7778, fitness= 2.6: Kp= 844.9, Ki= 555.5, Kd= 5.5



Lagestabilisierung der 3 Achsen hört sich einfacher an als Höhenregelung. Hier mein Vorschlag für eine Fitnessfunktion:
f: summe(i in [0, tmax| sqr(ax(i-1)-ax(i))+sqr(ay(i-1)-ay(i))+sqr(az(i-1)-az(i)))

Und ax(i), ay(i) und az(i) sind die Werte der 3 Achsen Gyros zum Zeitpunkt i.

[Che Guevara]

Hallo,

sry dass ich mich erst jetzt melde, hatte in der Zwischenzeit einiges zu tun. Deine Funktion verstehe ich nicht ganz. Was meinst du mit summe? f ist der Fitness-wert? Soll ich die Werte in der Summe-Klammer summieren? :D
Ist es sinnvoll alle 3 Parameter gleichzeitig zu probieren oder sollte ich lieber erst KP, dann KI und dann KD bestimmen?

Vielen Dank & Gruß
Chris

[Willa]

Hi,
das Bewerten der Fitness ist wohl der schwierigste Teil. Ich würde vorschlagen mal zu loggen: Einmal mit guten Parametern und einmal mit schlechten. Dann kannst du mit den aufgezeichneten Daten erstmal herumprobieren ob du irgendwelche Parameter finden kannst, die die Fitness beschreiben. Das wird nicht ganz leicht, denn der Copter kann nicht zwischen Windböen und schlechten Parametern unterscheiden (bzw. vielleicht schon, aber das wird nicht trivial sein). Ein Schwingen wird vielleicht nur per Fourieranalyse von Windböen zu unterscheiden sein, keine Ahnung...
Ich bin mir nicht sicher, ob dein Hauptcontroller diese Aufgabe zusätzlich übernehmen kann. Nach meiner Einschätzung, braucht so eine Analyse eine Menge Daten die über einen mehr oder wenigen langen Zeitraum aufgezeichnet und verrechnet werden. Das ist für so einen armen mega328p anstrengend...
ki, kp und kd würde ich gleichzeitig optimieren, denn die beeinflussen sich alle gegenseitig. Wenn ein kp Wert gut ist und man dann z.B. ki erhöht, ist der Wert für kp plötzlich sehr suboptimal. Da bin ich mal gespannt auf die weitere Entwicklung :-D

[Che Guevara]

Hallo Willa,

das freut mich, dass so ein Profi wie du sich für das Thema interressiert :D
Hm die Idee mit dem Datenloggen gefällt mir, evtl. werde ich mal mit einer SD-Karte experimentieren, welche dann aber wohl von einem zweiten µC angesteuert werden muss, damit das Regelverhalten sich nicht ändert. Die Windböen sollten kein Problem darstellen, ich werde einfach versuchen, die Daten nur bei ruhigen Ausseneinflüssen zu loggen oder ich machs irgendwo im Haus...
Allerdings habe ich doch starke Bedenken, nicht dass das Ganze nach hinten losgeht und ich demnächst wieder mal alles neu machen muss.... Evtl. sollte ich den Copter irgendwie mit ein oder zwei Schnüren etwas absichern. Ansich finde ich aber das Thema sehr interressant, da es doch etwas anspruchsvoller ist und auch einen praktischen Wert hat :)
Werde mal sehen, was sich als Datenlogger eignet und melde mich dann wieder.

@rossir:
Mir ist gerade erst der Sinn deiner vorgeschlagenen Fitnessfunktion aufgefallen: Je mehr hochfrequente Drehwinkelgeschwindigkeitsveränderungen *gg* es gibt, desto größer ist der Wert der Fitnessfunktion, d.h. desto schlechter sind die Werte. Ist das so richtig?
Aber wofür ist dann die Wurzel? Diese hat für mich (bis jetzt) keinen tieferen Sinn, könntest du mir das bitte erklären? :D

Vielen Dank & Gruß
Chris

[Bammel]

Hi,

das ist echt interesant.. wie wäre es damit diesen algo auf einem extra µC plus IMU zu machen? diese übermittelt nur die Kp, Ki und Kp werte an den haupt-µC. so kann man gut mitloggen oder der einstell-µC gibt irgendwann eine art okay zeichen... nun bruacht man nurnoch die werte auslesen und kann diese fest im copter verankern. dies müsste man natürlich wiederholen wnen man den copter irgendwie verändert...

gruß, bammel

[rossir]

Ja, f ist der Fitnesswert:

double f=fitness(wi);
Diese Fitness wird für wi bestimmt:

Pid wi={w0.Kp + T*fp*r(), w0.Ki + T*fi*r(), w0.Kd + T*fd*r()};
Der Vektor wi besteht aus den neuen Testkandidaten für Kp, Ki und Kd.

Ich stelle mir vor, dass innerhalb der Funktion fitness(wi) Folgendes passiert:
Der Quadrokopter fliegt schon und wird durch die alten Kp, Ki und Kd Werte (w(i-1)) des PID Controller leidlich Lage stabilisiert. Jetzt werden für den PID Controller die neuen Testkandidaten w(i)= Kp, Ki und Kd eingestellt. Der Test läuft z.B. 30 sec (tmax=30), d.h die Sekunden laufen von t=0 bis t=30.

Jede Sekunde werden die Werte ax(t), ay(t) und az(t) des 3 Achsen Gyros ausgelesen. Wenn zwischen zwei aufeinander folgenden Messungen eine möglichst kleine Differenz ist dann ist die Lagestabilität gut. Das ist die Grundidee hinter der Fitnessfunktion! Diese habe ich wie folgt in einer Formel ausgedrückt:

f: summe(i in [1, tmax| sqr(ax(t-1)-ax(t))+sqr(ay(t-1)-ay(t))+sqr(az(t-1)-az(t)))
Genau wie Du gesagt hast: Die Werte in der "Summe-Klammer" werden summiert. Nach 30 sek habe ich dadurch eine Fitness f für meine Testkandidaten Kp, Ki und Kd.

Dann probiere ich neue Testkandidaten usw.

Ja, es ist sinnvoll alle 3 Parameter gleichzeitig zu probieren, "denn die beeinflussen sich alle gegenseitig".

Anders als Willa meint, ist das keine Aufgabe die den mega328p überanstrengt. Eher ein Ansatz der die armen Akkus quält.

Nachtrag:
Wurzel wäre sqrt(x). Ich schreibe aber sqr(x) und das meint x*x. Damit schlägt man zwei Fliegen mit einer Klappe:
a) x*x ist immer positiv
b) große x werden überproportional "böse" bewertet.

[rossir]

@Bammel
Der simulated annealing Algorithmus benötigt wirklich keine besondere Rechenzeit. Ich würde ihn sogar, auch später im Einsatz, permanent (bei kleiner Temperatur T) mitlaufen lassen.
- Kleine Temperatur bedeutet, dass neue Testkandidaten für Kp, Ki und Kd nur in der Nähe der bisher besten Kp, Ki und Kd Werte gesucht werden.
- Permanent bedeutet, das ich ca. alle 60 sek einen neuen Testkandidaten probiere d.h. live einsetze. (Das ist aber, wie oben gesagt, nur eine kleine und damit "ungefährliche" Variante der bisher besten Werte.)

Dadurch wird das ganze System robust, flexibel und letztendlich selbstadaptiv. (Genau das Richtige also für den Thread "Software, Algorithmen und Ki" ;-) )

[Che Guevara]

@Bammel:
das hört sich gut an, ich werd mir die Tage mal was überlegen und ein bisschen testen :) Evtl. hole ich mir noch einen zweiten Gyro zum experimentieren, damit ich nicht immer hin-und herlöten muss.
Noch besser wäre es aber, wenn man diese Funktion auf dem HauptµC unterbringen könnte, so könnte man ohne irgendwelche Veränderungen jederzeit eine optimale PID-Parameter Einstellung vornehmen.

@rossir:
So langsam verstehe ich, wie du das alles meinst. Danke für die ausführliche Erklärung :)
Tja, da hätte ich mal wieder genauer lesen müssen, dann wäre mir dieser Fehler nicht passiert. Und ich muss sagen, diese Variante gefällt mir sehr gut, da damit dann präziser zwischen gut und schlecht unterschieden werden kann :)
Aber denkst du, 30s sind nötig? Ich hätte jetzt eher bei 2-3s angesetzt...

Vielen Dank & Gruß
Chris

[Bammel]

@che guevara: naja umlöten geht ja nicht.. du bruchst ja ein gyro zur flugstabilisierung und einen für die auswertung ob die werte gut sind.

das ganze muss man aber wie rossir gesagt hat über längere zeiten messen da ja noch vibrationen der motoren hinzukomme und auch steuerbefehle des piloten da ja alleine nur mit gyros keine eigenstabilität erreicht werden kann.

ich find die idee klasse und ich glaub ich werd mich da auch mal an eine lösung probieren. aber erstmal habe ich eine andere baustelle die gelöst werden will.

was mir da aber grade einfällt man muss ja die ganzen ergebnisse speichern. oder? das kostst viel speicher. bei mir, also in der firmware der shrediquette von willa, ist zwar noch speicher frei aber irgendwann ist da auch mal ende... man müsste sich warscheinlich am ende des fluges den besten wert extra ins eeprom speichern und dann beim nächsten flug weiter machen. später wenn kaum noch verbesserungen getroffen werden, sollte der algo sich auf minimalste änderungen der werte einstellen.

gruß, bammel

[Che Guevara]

Ich wollte mir ja ein Testboard bauen, damit ich erstmal ohne Risiko, den Copter zu schrotten, das Programm ausprobieren kann ;) Den auch, wenn man im Programm Code keinen Fehler erkennen kann, heißt das noch lange nicht, dass da auch kein Fehler ist...
Hm über die Zeit muss ich mir nochmal Gedanken machen, am besten wird es wohl sein, ein paar Tests zu machen. Trotzdem denke ich, eine etwas kürzere Zeit sollte besser sein, den: Innerhalb von ein paar Sekunden muss der Pilot nicht zwingend nachregeln, was die ganze Methode doch um einiges genauert macht. Folgendes Szenario: Der Pilot steuert beim 1. Test etwas stärker, da sich der Copter zu stark bewegt. Somit beinflusst der Pilot die Fitness sehr negativ. Beim 2. Test muss der Pilot nicht / nur wenig steuern, die Fitness wird somit nicht vom Piloten beeinflusst. Dann kann es passieren, dass die Werte des 2. Tests besser abschneiden als die des 1., obwohl die ersten Werte eigentlich die besseren wären. Stimmt ihr mir zu, oder habe ich hier einen Denkfehler?

Also Speichermäßig sollte auf dem Mega328P (der ist ja auf der shrediquette auch drauf, oder?) doch einiges vorhanden sein. Hab mich gerade mal an einer Lösung probiert, ist zwar noch nicht fertig, aber ich denke, ich habe bis auf ein paar Kleinigkeiten alles wichtige drin (7% Flash Used):


$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 19200



Declare Sub Get_fitness()
Declare Sub Get_new_individuen()




Const Max_time = 50 'maximale Zeit zum Testen


Dim I As Byte 'Zeitpunkt i
Dim Gyro_yaw(max_time) As Integer 'Gyro Wert zum Zeitpunkt i
Dim Gyro_roll(max_time) As Integer 'Gyro Wert zum Zeitpunkt i
Dim Gyro_pitch(max_time) As Integer 'Gyro Wert zum Zeitpunkt i
Dim Gyro_yaw_sqr As Integer 'Gyro Differenz Wert von (i-1) bis i
Dim Gyro_roll_sqr As Integer 'Gyro Differenz Wert von (i-1) bis i
Dim Gyro_pitch_sqr As Integer 'Gyro Differenz Wert von (i-1) bis i
Dim Rnd_p As Single 'Zufallszahl zu Erstellung neuer Individuen
Dim Rnd_i As Single 'Zufallszahl zu Erstellung neuer Individuen
Dim Rnd_d As Single 'Zufallszahl zu Erstellung neuer Individuen
Dim Temperatur As Single 'simulated annealing
Dim Temperatur_int As Integer
Dim T_a As Single
Dim Kp_startup As Single 'Startup P Wert
Dim Ki_startup As Single 'Startup I Wert
Dim Kd_startup As Single 'Startup D Wert
Dim Kp_mom As Single 'momentaner P Wert
Dim Ki_mom As Single 'momentaner I Wert
Dim Kd_mom As Single 'momentaner D Wert
Dim Kp_best As Single 'bester P Wert
Dim Ki_best As Single 'bester I Wert
Dim Kd_best As Single 'bester D Wert


Dim Tmp As Byte
Dim Tmp_int As Integer
Dim Tmp_single As Single


Dim Fitness As Single
Dim Fitness_int As Integer
Dim Best_fitness As Single
Dim Best_fitness_int As Integer


Dim Fp As Single 'Dynamikunterschied
Dim Fi As Single 'Dynamikunterschied
Dim Fd As Single 'Dynamikunterschied


Dim _yawnow As Integer 'aktueller Gyro Wert
Dim _rollnow As Integer 'aktueller Gyro Wert
Dim _pitchnow As Integer 'aktueller Gyro Wert


Temperatur = 1

T_a = 0.95 'Abweichung


Kp_startup = 0.3
Ki_startup = 0.0001
Kd_startup = 0.000007


Fp = 0.2 'Dynamikunterschied
Fi = 0.05 'Dynamikunterschied
Fd = 0.05 'Dynamikunterschied


Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_p = Tmp_int * 0.1 '-1 < rnd_p < 1

Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_i = Tmp_int * 0.1 '-1 < rnd_i < 1

Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_d = Tmp_int * 0.1 '-1 < rnd_d < 1



Do


Call Get_new_individuen()

'teste neue Individuen
For I = 0 To 30
Gyro_yaw(i) = _yawnow
Gyro_roll(i) = _rollnow
Gyro_pitch(i) = _pitchnow
Waitms 1
Next I

Call Get_fitness()

If Fitness < Best_fitness Then
Best_fitness = Fitness
Kp_best = Kp_mom
Ki_best = Ki_mom
Kd_best = Kd_mom
End If

Temperatur = Temperatur * T_a

Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_p = Tmp_int * 0.1 '-1 < rnd_p < 1

Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_i = Tmp_int * 0.1 '-1 < rnd_i < 1

Tmp_int = Rnd(20) - 10 '-10 < tmp_int < 10
Rnd_d = Tmp_int * 0.1 '-1 < rnd_d < 1


Loop


Sub Get_new_individuen()

Kp_mom = Temperatur * Fp
Kp_mom = Kp_mom * Rnd_p
Kp_mom = Kp_mom + Kp_startup

Ki_mom = Temperatur * Fi
Ki_mom = Ki_mom * Rnd_i
Ki_mom = Ki_mom + Ki_startup

Kd_mom = Temperatur * Fd
Kd_mom = Kd_mom * Rnd_d
Kd_mom = Kd_mom + Kd_startup

End Sub


Sub Get_fitness()

For I = 1 To 30

Tmp = I - 1

Tmp_int = Gyro_yaw(i) - Gyro_yaw(tmp)
Tmp_int = Tmp_int ^ 2
Gyro_yaw_sqr = Gyro_yaw_sqr + Tmp_int

Tmp_int = Gyro_roll(i) - Gyro_roll(tmp)
Tmp_int = Tmp_int ^ 2
Gyro_roll_sqr = Gyro_roll_sqr + Tmp_int

Tmp_int = Gyro_pitch(i) - Gyro_pitch(tmp)
Tmp_int = Tmp_int ^ 2
Gyro_pitch_sqr = Gyro_pitch_sqr + Tmp_int

Next I

Fitness_int = Gyro_yaw_sqr + Gyro_roll_sqr
Fitness_int = Fitness_int + Gyro_pitch_sqr
Fitness = Fitness_int

End Sub

End

Es ist jetzt aber schon relativ spät, weshalb der Code noch seeeehr im Anfangsstadium ist. Werds mir morgen nochmal genauer ansehen.

Gruß
Chris

[Willa]

Ich bin sehr gespannt ob das im Prinzip funktioniert... Ich stelle mir das Bewerten wie gesagt sehr schwierig vor. Wenn ich als Mensch manchmal schon Probleme habe zu bewerten wie gut die eingestellten Parameter sind, was soll dann ein armer Computer das alles machen? Richtig spannend und sinnvoll wird so eine Optimierung aber nicht im Schwebeflug. Meine Copter fliegen im Schwebeflug (und bei Windstille) auch mit total verdrehten Regelparametern super. Nur wenn man schneller fliegt und enge Kurven etc. bei Wind, dann merkt man ob die Parameter wirklich was taugen.
Aber klar, man sollte erstmal im Schwebeflug bei optimalen Randbedingungen anfangen. Ich möchte aber auch nicht derjenige sein der immer rumnörgelt und sagt wie schwierig alles ist... Vielleicht hast du ja schon übermorgen fix und fertig evolvierte Parameter...!

[Bammel]

moin,

ich hätte noch folgende idee...
man stellt den copter in schwebeflug ein... denn geht der algo los... es leuchtet eine LED nun muss man den copter an einem ausleger nach oben stupsen... fitness wird bewertet... dann werden neue PID werte übermittelt udn das ganze szenario geht neu los.

das ganze wäre so eine "extrem" situation. den william hat recht. mein copter fliegt im schwebeflug auch astrein nur bei wind und beim umherfliegen zuckt dieser.

evtl kann man das starten der auswertung der fitness auch über einen schalter am sender machen!?

gruß, bammel

[Willa]

evtl kann man das starten der auswertung der fitness auch über einen schalter am sender machen!?
Das ist eine gute Idee... Man könnte einfach verschiedene Arten von Störungen auf den Copter legen und danach bewerten wie gut diese ausgeregelt werden. Z.B. ein Szenario ist, dass man ein Rauschen auf die Motor-Stellwerte gibt und danach beurteilt wie gut diese Störung kompensiert wird. Oder man gibt auf einen Motor einen sprunghaften Fehler und beurteilt das Ausregeln. Das kingt dann für mich schon deutlich einfacher und "kontrollierter"... Vor allem könnte man diese Versuche wirklich auf einem Teststand machen, d.h. man riskiert nicht mehr so viele Abstürze.

[Che Guevara]

Hallo,

mit dem Schwebeflug habt ihr natürlich vollkommen recht, daran hatte ich nicht gedacht... war auch schon etwas spät :)
Eure Ideen gefallen mir sehr gut! Was haltet ihr den davon, wenn man diese kombiniert?! Ich stelle mir vor, den Copter auf eine bestimmte Höhe zu fliegen und dann einen Taster zu drücken - dieser startet dann den Algorithmus: Es werden neue Parameter eingestellt und nach einer kurzen Zeit wird auf einen (oder evtl. mehreren) Motor ein sprunghafter Fehler gegeben. Somit hätte man (theoretisch) immer das gleiche Szenario und könnte die einzelnen Parameter immer sehr objektiv bewerten lassen.

Gruß
Chris

[Bammel]

hallo...

ich finde die idee mit dem schalter und dem prüfstand super!

klar muss man anch dem prüfstand noch die parameter weiter optimieren aber das könnte ja ggf. in sehr kleinen schritten direkt beim fliegen passieren.

den ein copter an einem prüfstand gebunden verhält sich ja anders als frei schwebend.

prüfstand würde ich mir als eine art wippe über eine oder ggf auch zwei achsen vorstellen. für die gier regelung könnte man dann noch einen prüfstand bauen in dem der copter über die hochachse drehend gelagert wird.

ich finde das ganze klingt nach einem sehr schönes winterprojekt.

was mir aber seit anfang an im kopf rumschwirt warum das nicht schon andere, namenhafte, kopter bauer verwirklicht haben.

gruß, bammel

[Willa]

was mir aber seit anfang an im kopf rumschwirt warum das nicht schon andere, namenhafte, kopter bauer verwirklicht haben.
Ich glaube der TT-Copter optimiert seine Parameter selbstständig. Aber ich weiss nicht wie.

[Che Guevara]

Ja, vom TT-Copter habe ich das auch schon gelesen. Wie weiß ich auch nicht, aber evtl. bekomme ich ja ein paar Infos, werde denen mal ne Mail schreiben :)

Leider hatte ich gestern eine Kollision, deswegen muss ich jetzt erstmal einen Ausleger austauschen....

Gruß
Chris

[Bammel]

Hallo,

gibt es schon neuigkeiten?

Gruß, Bammel

[Che Guevara]

Hallo,

nein, leider nicht! Ich hatte eine E-Mail an TT-Copter geschrieben und gefragt, ob sie mir freundlicherweise sagen könnten, mit welchem Verfahren ihr Copter seine Parameter optimiert. Aber anscheinend ist das ein Geheimnis .... Habe nämlich keine E-Mail erhalten.
Mit dem Prüfstandaufbau kann ich auch noch nicht beginnen, da ich derzeit den Feinschliff meiner FA mache und nach den Ferien auch noch ein paar Klausuren schreibe. Ich denke, vor Weihnachten wird sich da nicht allzuviel ergeben, Schule geht vor.
Aber sobald ich weitergemacht habe, poste ich hier natürlich meine Erfahrungen.

Gruß
Chris

[Che Guevara]

Hallo,

nun, da wieder 2 Klausuren vorbei sind, habe ich wieder Zeit, an dem Projekt weiterzumachen.
Zuerst habe ich allerdings eine Frage; folgender Code:


if (fcandidate<f0) T*=a;
w0=wcandidate;
f0=fcandidate;


Wird das so:


if fcandidate < f0 then
temperatur = temperatur * t_a
w0_kp = wcandidate_kp
w0_ki = wcandidate_ki
w0_kd = wcandidate_kd
f0 = fcandidate
endif

oder so:


if fcandidate < f0 then
temperatur = temperatur * t_a
endif
w0_kp = wcandidate_kp
w0_ki = wcandidate_ki
w0_kd = wcandidate_kd
f0 = fcandidate

nach BASIC übersetzt?
Mich verwirrt diese IF-Abfrage. C habe ich nie gelernt, nur in der Schule etwas JAVA, aber ich denke, das sollte in etwa eine ähnliche Syntax haben?!
Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?

Mittlerweile habe ich auch schon versucht, den kompletten Code anzupassen, hier mal mein aktuelles Geschreibsel:


$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 250
$hwstack = 250
$swstack = 250
$baud = 38400



Config Serialin = Buffered , Size = 100
Config Serialout = Buffered , Size = 100


Declare Sub Simulated_annealing(byval N As Byte)

Declare Function Getfitness() As Single

Declare Sub Log_data()

Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.28 '0.32
_roll_kp = 0.28 '0.32
_pitch_kp = 0.25 '0.32

_yaw_ki = 0.0001 '0.000135
_roll_ki = 0.0001 '0.000135
_pitch_ki = 0.0001 '0.000135

_yaw_kd = 0.000007 '0.0000096
_roll_kd = 0.000007 '0.0000096
_pitch_kd = 0.000007 '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte



Dim Extmp As Single 'Integer
Dim Eytmp As Single 'Integer
Dim Eztmp As Single 'Integer
Dim Ax(50) As Single
Dim Ay(50) As Single
Dim Az(50) As Single
Dim Ex As Single 'Integer
Dim Ey As Single 'Integer
Dim Ez As Single 'Integer

Dim Atmp As Byte
Dim A As Byte

Dim F As Single



Dim Fcandidate As Single
Dim Kp_mom As Single
Dim Ki_mom As Single
Dim Kd_mom As Single
Dim Temperatur As Single
Dim T_a As Single
Dim Fp As Single
Dim Fi As Single
Dim Fd As Single
Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim Wcandidate_kp As Single
Dim Wcandidate_ki As Single
Dim Wcandidate_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single
Dim F0 As Single


Dim Test As Word 'Byte


Dim Log_cnt As Byte

Dim Send_logged_data As Byte
Dim Rx_in As String * 30
'Dim Var(50) As Byte

Dim Kanal_6_old As Byte

Dim B As String * 1
Dim Lenght As Byte


Dim Itmp As Byte
Dim Itmp_old As Byte

Dim Xtmp As Byte



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
If Rc_on < 65000 Then Incr Rc_on
Else
If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
Goto Rc_test
End If


'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms



Wcandidate_kp = 0.3
Wcandidate_ki = 0.0001
Wcandidate_kd = 0.000007
Temperatur = 1.0
T_a = 0.8 '0.95
Fp = 0.1 '0.1
Fi = 0.0001
Fd = 0.000005

Fcandidate = 500 '150
'Fcandidate = Getfitness()
W0_kp = Wcandidate_kp
W0_ki = Wcandidate_ki
W0_kd = Wcandidate_kd
F0 = Fcandidate





Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
Call Mixer()
Call Failsave()
_bl(1) = 113
_bl(4) = 113
Call Set_pwm()


If State > 0 Then

Incr Test
If Itmp < 32 Then
If Itmp > Itmp_old Then

Rp_i = Rnd(20) - 10
Rp = Rp_i / 10
Wi_kp = Temperatur * Fp
Wi_kp = Wi_kp * Rp
Wi_kp = Wi_kp + W0_kp

Ri_i = Rnd(20) - 10
Ri = Ri_i / 10
Wi_ki = Temperatur * Fi
Wi_ki = Wi_ki * Ri
Wi_ki = Wi_ki + W0_ki

Rd_i = Rnd(20) - 10
Rd = Rd_i / 10
Wi_kd = Temperatur * Fd
Wi_kd = Wi_kd * Rd
Wi_kd = Wi_kd + W0_kd

If Wi_kp < 0 Then Wi_kp = 0
If Wi_ki < 0 Then Wi_ki = 0
If Wi_kd < 0 Then Wi_kd = 0 '#######

_roll_kp = Wi_kp
_pitch_kp = Wi_kp
_roll_ki = Wi_ki
_pitch_ki = Wi_ki
_roll_kd = Wi_kd
_pitch_kd = Wi_kd

End If

Itmp_old = Itmp

Incr Xtmp
If Xtmp = 31 Then
Incr Itmp
Xtmp = 0
F = Getfitness()
If F < Fcandidate Then
Fcandidate = F
Wcandidate_kp = Wi_kp
Wcandidate_ki = Wi_ki
Wcandidate_kd = Wi_kd
End If
Elseif Xtmp < 31 Then
Ax(xtmp) = _roll_err
End If

End If

If Itmp = 32 Then
Itmp = 0
If Fcandidate < F0 Then
Temperatur = Temperatur * T_a
W0_kp = Wcandidate_kp
W0_ki = Wcandidate_ki
W0_kd = Wcandidate_kd
F0 = Fcandidate
End If
End If

Elseif State = 0 Then

If Ischarwaiting() > 0 Then
Input Rx_in Noecho
B = Left(rx_in , 1)
If Asc(b) = 10 Or Asc(b) = 13 Then
Lenght = Len(rx_in)
Lenght = Lenght - 1
Rx_in = Right(rx_in , Lenght)
Clear Serialin '######
End If
Send_logged_data = Instr(rx_in , "send!")
If Send_logged_data = 1 Then
Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
Test = 0
Send_logged_data = 0
Clear Serialin '######
End If
End If

End If


Loop


Sub Simulated_annealing(byval N As Byte)
Wcandidate_kp = 0.3
Wcandidate_ki = 0.0001
Wcandidate_kd = 0.000007
Temperatur = 1.0
T_a = 0.90
Fp = 0.1
Fi = 0.0002
Fd = 0.000005
Fcandidate = Getfitness()
W0_kp = Wcandidate_kp
W0_ki = Wcandidate_ki
W0_kd = Wcandidate_kd
F0 = Fcandidate
Test = 0
While Fcandidate > 50
Incr Test
For I = 1 To N
Rp_i = Rnd(20) - 10
Rp = Rp_i / 10 '################
Wi_kp = Temperatur * Fp
Wi_kp = Wi_kp * Rp
Wi_kp = Wi_kp + W0_kp

Ri_i = Rnd(20) - 10
Ri = Ri_i / 10
Wi_ki = Temperatur * Fi
Wi_ki = Wi_ki * Ri
Wi_ki = Wi_ki + W0_ki

Rd_i = Rnd(20) - 10
Rd = Rd_i / 10
Wi_kd = Temperatur * Fd
Wi_kd = Wi_kd * Rd
Wi_kd = Wi_kd + W0_kd

F = Getfitness()
If F < Fcandidate Then
Fcandidate = F
Wcandidate_kp = Wi_kp
Wcandidate_ki = Wi_ki
Wcandidate_kd = Wi_kd
End If
Next I
If Fcandidate < F0 Then
Temperatur = Temperatur * T_a
'End If
W0_kp = Wcandidate_kp '#########
W0_ki = Wcandidate_ki
W0_kd = Wcandidate_kd
F0 = Fcandidate
End If
'Print Test ; " " ; Temperatur ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
Wend
End Sub


Sub Log_data()
If State > 0 Then
Incr Log_cnt
If Log_cnt < 30 Then
Ax(log_cnt) = _rollnow
'Ay(log_cnt) = _pitchnow
'Az(log_cnt) = _yawnow
Elseif Log_cnt = 250 Then
Log_cnt = 200
End If
Elseif State = 0 Then
If Ischarwaiting() > 0 Then
Input Rx_in Noecho
B = Left(rx_in , 1)
If Asc(b) = 10 Or Asc(b) = 13 Then
Lenght = Len(rx_in)
Lenght = Lenght - 1
Rx_in = Right(rx_in , Lenght)
End If
Send_logged_data = Instr(rx_in , "send!")
If Send_logged_data = 1 Then
F = Getfitness()
Print F
Send_logged_data = 0
End If
End If
End If
End Sub


Function Getfitness() As Single
'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2
Ex = 0
Ey = 0
Ez = 0
For A = 1 To 30
If A > 1 Then
Atmp = A - 1
Else
Atmp = 30
End If
Extmp = Ax(a) - Ax(atmp)
Extmp = Extmp * Extmp
Eytmp = Ay(a) - Ay(atmp)
Eytmp = Eytmp * Eytmp
Eztmp = Az(a) - Az(atmp)
Eztmp = Eztmp * Eztmp
Ex = Ex + Extmp
Ey = Ey + Eytmp
Ez = Ez + Eztmp
Next A
Getfitness = Ex + Ey
Getfitness = Getfitness + Ez
End Function


Sub Set_pwm()
If _bl(1) > 250 Then _bl(1) = 250
If _bl(1) < 113 Then _bl(1) = 113
If _bl(2) > 250 Then _bl(2) = 250
If _bl(2) < 113 Then _bl(2) = 113
If _bl(3) > 250 Then _bl(3) = 250
If _bl(3) < 113 Then _bl(3) = 113
If _bl(4) > 250 Then _bl(4) = 250
If _bl(4) < 113 Then _bl(4) = 113
Pwm2a = _bl(4) 'links vorne (US)
Pwm2b = _bl(2) 'links hinten (GUS)
Pwm1a = _bl(3) 'rechts vorne (GUS)
Pwm1b = _bl(1) 'rechts hinten (US)
End Sub


Sub Failsave()
If Channel >= 11 Then
If Failure < 255 Then
Incr Failure
End If
End If
If Channel < 11 Then
If Failure > 0 Then
Decr Failure
End If
End If
End Sub


Measure:
If Channel > 0 And Channel < 9 Then
Kanal(channel) = Timer0
End If
Timer0 = 6
Incr Channel
Return


Pausedetect:
If Channel <> 0 Then
Newvalsreceived = 1
End If
Channel = 0
Return


Sub Filter_gyro_data()
Call Read_wmp_data()

_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

If Newvalsreceived = 1 Then
Newvalsreceived = 0

For I = 1 To Maxchannel
Fkanal(i) = Fkanal(i) * 3
Fkanal(i) = Fkanal(i) + Kanal(i)
Shift Fkanal(i) , Right , 2
Hempf(i) = Fkanal(i) + 17
Lempf(i) = Fkanal(i) - 17
If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
Kanal(i) = Fkanal(i)
End If
Next I

If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
_sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
_sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
End If

For I = 2 To Maxchannel
If Kanal(i) >= 62 And Kanal(i) < 139 Then
_sbl(i) = Kanal(i) - 100
End If
Next I

If Kanal(5) < 80 Then State = 0
If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
If Kanal(5) >= 115 Then State = 2


Setpoint_yaw = _sbl(_yawchannel) * 1.5

If State < 2 Then
Setpoint_roll = _sbl(_rollchannel) * 0.6
Setpoint_pitch = _sbl(_pitchchannel) * 0.6
Else
Setpoint_roll = _sbl(_rollchannel) * 1.5
Setpoint_pitch = _sbl(_pitchchannel) * 1.5
End If


If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
Call Set_wmp_offset()
End If

Oldstate = State

End If

End Sub


Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub


Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub


Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 4
Shift Roll , Right , 4
Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
_yawoffset_int = _yawoffset
_rolloffset_int = _rolloffset
_pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
_yaw_err = Setpoint_yaw - _yawnow
_yaw_kp_err = _yaw_err * _yaw_kp
_yaw_ki_sum = _yaw_ki_sum + _yaw_err
If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
_yaw_ki_err = _yaw_ki_sum * _yaw_ki
_yaw_kd_err = _yaw_err - _yaw_kd_old
_yaw_kd_old = _yaw_err
_yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
_roll_err = Setpoint_roll - _rollnow
_roll_kp_err = _roll_err * _roll_kp
_roll_ki_sum = _roll_ki_sum + _roll_err
If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
_roll_ki_err = _roll_ki_sum * _roll_ki
_roll_kd_err = _roll_err - _roll_kd_old
_roll_kd_old = _roll_err
_roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
_pitch_err = Setpoint_pitch - _pitchnow
_pitch_kp_err = _pitch_err * _pitch_kp
_pitch_ki_sum = _pitch_ki_sum + _pitch_err
If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
_pitch_ki_err = _pitch_ki_sum * _pitch_ki
_pitch_kd_err = _pitch_err - _pitch_kd_old
_pitch_kd_old = _pitch_err
_pitch_kd_err = _pitch_kd_err * _pitch_kd


_yaw_pid = _yaw_kp_err + _yaw_ki_err
_yaw_pid = _yaw_pid + _yaw_kd_err
_yaw_pid_int = _yaw_pid

_roll_pid = _roll_kp_err + _roll_ki_err
_roll_pid = _roll_pid + _roll_kd_err
_roll_pid_int = _roll_pid

_pitch_pid = _pitch_kp_err + _pitch_ki_err
_pitch_pid = _pitch_pid + _pitch_kd_err
_pitch_pid_int = _pitch_pid

If _yaw_pid_int < -68 Then _yaw_pid_int = -68
If _yaw_pid_int > 68 Then _yaw_pid_int = 68
If _roll_pid_int < -68 Then _roll_pid_int = -68
If _roll_pid_int > 68 Then _roll_pid_int = 68
If _pitch_pid_int < -68 Then _pitch_pid_int = -68
If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

_bl(1) = 182 + _sbl(_throttlechannel)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = _bl(1)

If State <> 0 And Failure < 20 Then

Portc.2 = 0
Portc.3 = 1

_bl(1) = _bl(1) + Minthrottle
_bl(2) = _bl(2) + Minthrottle
_bl(3) = _bl(3) + Minthrottle
_bl(4) = _bl(4) + Minthrottle

_bl(1) = _bl(1) - _pitch_pid_int
_bl(4) = _bl(4) + _pitch_pid_int

_bl(2) = _bl(2) - _roll_pid_int
_bl(3) = _bl(3) + _roll_pid_int

_bl(1) = _bl(1) - _yaw_pid_int
_bl(2) = _bl(2) + _yaw_pid_int
_bl(3) = _bl(3) + _yaw_pid_int
_bl(4) = _bl(4) - _yaw_pid_int

Else

Portc.2 = 0

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

Incr _blink
If _blink = 100 Then
Portc.3 = 1
_blink = 101
End If
If _blink = 200 Then
Portc.3 = 0
_blink = 0
End If

_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_yaw_ki_err = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_roll_ki_err = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
_pitch_ki_err = 0
_yaw_pid_int = 0
_roll_pid_int = 0
_pitch_pid_int = 0
_yaw_pid = 0
_roll_pid = 0
_pitch_pid = 0
Yawstickvel = 0
Rollstickvel = 0
Pitchstickvel = 0
Yawstickold = 0
Rollstickold = 0
Pitchstickold = 0

End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Ich weiß, manche Variablen haben komische Namen, das wird noch ausgebessert. Zur Info: XTMP und ITMP sind reine Zählvariablen. Vielleicht hat jemand von euch ja die Geduld, sich das ganze mal kurz anzusehen (also nur / hauptsächlich den Simulated-Annealing-Teil im Main-loop)?!

Vielen Dank & Gruß
Chris

EDIT:
Kennt jemand ein gutes Buch, in dem ausführlich auf sowohl Theorie, als auch Praxis des Simulated Annealing eingegangen wird? Am besten mit ein paar Programmcodes als Beispiel ... Wäre nett :)

EDIT2:
Hab mich jetzt selbst mal an einem SA versucht, vielleicht wäre jemand ja mal so nett, um kurz drüberzuschauen, ob grobe Fehler drin sind.


$regfile = "m32def.dat"
$crystal = 16000000
$framesize = 80
$hwstack = 80
$swstack = 80
$baud = 38400



Declare Function Getfitness() As Single



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Single
Dim F0 As Single

'------evtl. als long!!!------------------------------
Dim Extmp As Single 'Integer
Dim Eytmp As Single 'Integer
Dim Eztmp As Single 'Integer
Dim Ax(50) As Single 'Integer
Dim Ay(50) As Single 'Integer
Dim Az(50) As Single 'Integer
Dim Ex As Single 'Integer
Dim Ey As Single 'Integer
Dim Ez As Single 'Integer

Dim Atmp As Word
Dim A As Word

Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word


Dim _roll_kp As Single
Dim _roll_ki As Single
Dim _roll_kd As Single

Dim _rollnow As Integer

Dim X As Integer


'-------initialize the values
T = 1.0
T_a = 0.98
F0 = 500.0
F = F0
W0_kp = 5
W0_ki = 0.1
W0_kd = 0.043
Fp = 2
Fi = 0.08
Fd = 0.005
Max_log_cnt = 15
Max_tries = 250



Enable Interrupts



Do


'-------randomize according to the current temperature--------
Rp_i = Rnd(200) - 100
Rp = Rp_i * 0.01
Rp = Rp * T
Ri_i = Rnd(200) - 100
Ri = Ri_i * 0.01
Ri = Ri * T
Rd_i = Rnd(200) - 100
Rd = Rd_i * 0.01
Rd = Rd * T

'-------generate new parameters------------------------------
Wi_kp = Rp * Fp
Wi_kp = Wi_kp + W0_kp
Wi_ki = Ri * Fi
Wi_ki = Wi_ki + W0_ki
Wi_kd = Rd * Fd
Wi_kd = Wi_kd + W0_kd

'-------test current parameters------------------------------
_roll_kp = Wi_kp
_roll_ki = Wi_ki
_roll_kd = Wi_kd

'-------log data for fitness function------------------------
Incr Log_cnt
If Log_cnt < Max_log_cnt Then
Ax(log_cnt) = _rollnow '################################################# ###############################
Elseif Log_cnt = Max_log_cnt Then
F = Getfitness()
Log_cnt = 0
End If

'-------better then current solution?------------------------
If F < F0 Then
F0 = F
W0_kp = Wi_kp
W0_ki = Wi_ki
W0_kd = Wi_kd
T = T * T_a
Print Tries ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
End If

'-------counter for max tries---------------------------------
Incr Tries


Loop


Function Getfitness() As Single
'f = (ax(i) - ax(i-1))^2 + (ay(i) - ay(i-1))^2 + (az(i) - az(i-1))^2
Ex = 0
Ey = 0
Ez = 0
For A = 1 To Max_log_cnt
If A > 1 Then
Atmp = A - 1
Else
Atmp = Max_log_cnt
End If
Extmp = Ax(a) - Ax(atmp)
Extmp = Extmp * Extmp
Eytmp = Ay(a) - Ay(atmp)
Eytmp = Eytmp * Eytmp
Eztmp = Az(a) - Az(atmp)
Eztmp = Eztmp * Eztmp
Ex = Ex + Extmp
Ey = Ey + Eytmp
Ez = Ez + Eztmp
Next A
Getfitness = Ex + Ey
Getfitness = Getfitness + Ez
End Function

End

[021aet04]

Warum ist bei der IF-Abfrage keine {-Klammer vor T*=a?
Hinter der IF Abfrage ist keine {-Klammer weil nur ein Kommando ("T*=a") steht.
Man könnte stattdessen auch "if (fcandidate<f0) { T*=a;}" schreiben.
Ist aber das gleiche. Wenn man mehrere Befehle/Kommandos hat muss man eine {-Klammer schreiben.

MfG Hannes

[Bernina]

Tip: Ich kenne mich zwar mit Optimierungsmethoden wenig aus, weiß aber, dass MATLAB eine Menge Algorithmen hierfür bereithält.

[Che Guevara]

Oh... ok :)
Ist zwar jetzt sowieso nicht mehr relevant, da ich meinen eigenen Algorithmus geschrieben habe, aber trotzdem danke, Wissen ist nie verkehrt!

Hm also eigentlich soll der Algorithmus später dann die ganze Zeit auf dem Quadrocopter mitlaufen, damit die Parameter ständig angepasst werden können, falls nötig. Deswegen muss ich auf einen PC usw.. verzichten. Trotzdem danke für den Tipp.

Heute werde ich mal, sofern ich Zeit finde, meinen Algorithmus testen und sehen, was dabei rauskommt. Sobald ich Ergebnisse habe, poste ich sie hier.

Gruß
Chris

EDIT:
Hier ist jetzt mal mein aktueller Code:


$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 100
$hwstack = 100
$swstack = 100
$baud = 38400


Declare Sub Serial0charmatch()

Config Serialin = Buffered , Size = 30 , Bytematch = 13


Declare Sub Simulated_annealing()



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Double
Dim F0 As Double

'------evtl. als long!!!------------------------------
Dim Extmp As Double
Dim Eytmp As Double
Dim Eztmp As Double
Dim Ax(50) As Double
Dim Ay(50) As Double
Dim Az(50) As Double
Dim Ex As Double
Dim Ey As Double
Dim Ez As Double

Dim Atmp As Word
Dim A As Word

Dim Log_step As Byte
Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word

Dim Test_anz As Long


Dim X As Integer



'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.4 '0.32
_roll_kp = 0.4 '0.32
_pitch_kp = 0.4 '0.32

_yaw_ki = 0.0008 '0.0008
_roll_ki = 0.0015 '0.0018
_pitch_ki = 0.0015 '0.0018

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0

'_yaw_ki = 0.0001 '0.000135
'_roll_ki = 0.0001 '0.000135
'_pitch_ki = 0.0001 '0.000135

'_yaw_kd = 0.000007 '0.0000096
'_roll_kd = 0.000007 '0.0000096
'_pitch_kd = 0.000007 '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte


'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms


'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########




'-------initialize the values

'_roll_kp = 0.4
'_roll_ki = 0.0015

T = 1.0
T_a = 0.75
F0 = 500
F = F0
W0_kp = 0.3
W0_ki = 0.001
W0_kd = 0.000005
Fp = 0.2
Fi = 0.0008
Fd = 0.00001
Log_step = 3
Max_log_cnt = 90
Max_tries = 30



Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
If Rc_on < 65000 Then Incr Rc_on
Else
If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
Goto Rc_test
End If




Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
Call Mixer()
Call Failsave()
'#####
_bl(1) = 113
_bl(4) = 113
'#####
Call Set_pwm()
Call Simulated_annealing()


Loop


Sub Simulated_annealing()

If State > 0 Then

Incr Test_anz

'-------randomize according to the current temperature--------
Rp_i = Rnd(200) - 100
Rp = Rp_i * 0.01
Rp = Rp * T
Ri_i = Rnd(200) - 100
Ri = Ri_i * 0.01
Ri = Ri * T
Rd_i = Rnd(200) - 100
Rd = Rd_i * 0.01
Rd = Rd * T

'-------generate new parameters------------------------------
Wi_kp = Rp * Fp
Wi_kp = Wi_kp + W0_kp
Wi_ki = Ri * Fi
Wi_ki = Wi_ki + W0_ki
Wi_kd = Rd * Fd
Wi_kd = Wi_kd + W0_kd

'-------cut off parameters-----------------------------------
If Wi_kp < 0 Then Wi_kp = 0
If Wi_ki < 0 Then Wi_ki = 0

'-------test current parameters------------------------------
_roll_kp = Wi_kp
_roll_ki = Wi_ki
_roll_kd = Wi_kd

'-------log data and get fitness-----------------------------
Incr Log_cnt
If Log_cnt < Max_log_cnt Then
Ax(log_cnt) = _roll_err
'####
If Log_cnt > Log_step Then
Atmp = Log_cnt - Log_step
Else
Atmp = 1
End If
Extmp = Ax(log_cnt) - Ax(atmp)
Extmp = Extmp ^ 2
Eytmp = Ay(log_cnt) - Ay(atmp)
Eytmp = Eytmp ^ 2
Eztmp = Az(log_cnt) - Az(atmp)
Eztmp = Eztmp ^ 2
Ex = Ex + Extmp
Ey = Ey + Eytmp
Ez = Ez + Eztmp
'####
Elseif Log_cnt = Max_log_cnt Then
F = Ex + Ey
F = F + Ez
Ex = 0
Ey = 0
Ez = 0
Log_cnt = 0
Incr Tries
End If

'-------better then current solution?------------------------
If F < F0 Then
F0 = F
W0_kp = Wi_kp
W0_ki = Wi_ki
W0_kd = Wi_kd
End If

'-------counter for max tries---------------------------------
If Tries = Max_tries Then
Toggle Portc.2
T = T * T_a
Tries = 0
End If

End If

End Sub


Sub Serial0charmatch()
Clear Serialin
If State = 0 Then
'Print F
Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
End If
End Sub


Sub Set_pwm()
If _bl(1) > 250 Then _bl(1) = 250
If _bl(1) < 113 Then _bl(1) = 113
If _bl(2) > 250 Then _bl(2) = 250
If _bl(2) < 113 Then _bl(2) = 113
If _bl(3) > 250 Then _bl(3) = 250
If _bl(3) < 113 Then _bl(3) = 113
If _bl(4) > 250 Then _bl(4) = 250
If _bl(4) < 113 Then _bl(4) = 113
Pwm2a = _bl(4) 'links vorne (US)
Pwm2b = _bl(2) 'links hinten (GUS)
Pwm1a = _bl(3) 'rechts vorne (GUS)
Pwm1b = _bl(1) 'rechts hinten (US)
End Sub


Sub Failsave()
If Channel >= 11 Then
If Failure < 255 Then
Incr Failure
End If
End If
If Channel < 11 Then
If Failure > 0 Then
Decr Failure
End If
End If
End Sub


Measure:
If Channel > 0 And Channel < 9 Then
Kanal(channel) = Timer0
End If
Timer0 = 6
Incr Channel
Return


Pausedetect:
If Channel <> 0 Then
Newvalsreceived = 1
End If
Channel = 0
Return


Sub Filter_gyro_data()
Call Read_wmp_data()

_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

If Newvalsreceived = 1 Then
Newvalsreceived = 0

For I = 1 To Maxchannel
Fkanal(i) = Fkanal(i) * 3
Fkanal(i) = Fkanal(i) + Kanal(i)
Shift Fkanal(i) , Right , 2
Hempf(i) = Fkanal(i) + 17
Lempf(i) = Fkanal(i) - 17
If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
Kanal(i) = Fkanal(i)
End If
Next I

If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
_sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
_sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
End If

For I = 2 To Maxchannel
If Kanal(i) >= 62 And Kanal(i) < 139 Then
_sbl(i) = Kanal(i) - 100
End If
Next I

If Kanal(5) < 80 Then State = 0
If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
If Kanal(5) >= 115 Then State = 2


Setpoint_yaw = _sbl(_yawchannel) * 1.5

If State < 2 Then
Setpoint_roll = _sbl(_rollchannel) * 0.6
Setpoint_pitch = _sbl(_pitchchannel) * 0.6
Else
Setpoint_roll = _sbl(_rollchannel) * 1.5
Setpoint_pitch = _sbl(_pitchchannel) * 1.5
End If


If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
Call Set_wmp_offset()
End If

Oldstate = State

End If

End Sub


Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub


Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub


Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 4
Shift Roll , Right , 4
Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
_yawoffset_int = _yawoffset
_rolloffset_int = _rolloffset
_pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
_yaw_err = Setpoint_yaw - _yawnow
_yaw_kp_err = _yaw_err * _yaw_kp
_yaw_ki_sum = _yaw_ki_sum + _yaw_err
If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
_yaw_ki_err = _yaw_ki_sum * _yaw_ki
_yaw_kd_err = _yaw_err - _yaw_kd_old
_yaw_kd_old = _yaw_err
_yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
_roll_err = Setpoint_roll - _rollnow
_roll_kp_err = _roll_err * _roll_kp
_roll_ki_sum = _roll_ki_sum + _roll_err
If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
_roll_ki_err = _roll_ki_sum * _roll_ki
_roll_kd_err = _roll_err - _roll_kd_old
_roll_kd_old = _roll_err
_roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
_pitch_err = Setpoint_pitch - _pitchnow
_pitch_kp_err = _pitch_err * _pitch_kp
_pitch_ki_sum = _pitch_ki_sum + _pitch_err
If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
_pitch_ki_err = _pitch_ki_sum * _pitch_ki
_pitch_kd_err = _pitch_err - _pitch_kd_old
_pitch_kd_old = _pitch_err
_pitch_kd_err = _pitch_kd_err * _pitch_kd


_yaw_pid = _yaw_kp_err + _yaw_ki_err
_yaw_pid = _yaw_pid + _yaw_kd_err
_yaw_pid_int = _yaw_pid

_roll_pid = _roll_kp_err + _roll_ki_err
_roll_pid = _roll_pid + _roll_kd_err
_roll_pid_int = _roll_pid

_pitch_pid = _pitch_kp_err + _pitch_ki_err
_pitch_pid = _pitch_pid + _pitch_kd_err
_pitch_pid_int = _pitch_pid

If _yaw_pid_int < -68 Then _yaw_pid_int = -68
If _yaw_pid_int > 68 Then _yaw_pid_int = 68
If _roll_pid_int < -68 Then _roll_pid_int = -68
If _roll_pid_int > 68 Then _roll_pid_int = 68
If _pitch_pid_int < -68 Then _pitch_pid_int = -68
If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

_bl(1) = 182 + _sbl(_throttlechannel)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = _bl(1)

If State <> 0 And Failure < 20 Then

_bl(1) = _bl(1) + Minthrottle
_bl(2) = _bl(2) + Minthrottle
_bl(3) = _bl(3) + Minthrottle
_bl(4) = _bl(4) + Minthrottle

_bl(1) = _bl(1) - _pitch_pid_int
_bl(4) = _bl(4) + _pitch_pid_int

_bl(2) = _bl(2) - _roll_pid_int
_bl(3) = _bl(3) + _roll_pid_int

_bl(1) = _bl(1) - _yaw_pid_int
_bl(2) = _bl(2) + _yaw_pid_int
_bl(3) = _bl(3) + _yaw_pid_int
_bl(4) = _bl(4) - _yaw_pid_int

Else

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_yaw_ki_err = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_roll_ki_err = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
_pitch_ki_err = 0
_yaw_pid_int = 0
_roll_pid_int = 0
_pitch_pid_int = 0
_yaw_pid = 0
_roll_pid = 0
_pitch_pid = 0
Yawstickvel = 0
Rollstickvel = 0
Pitchstickvel = 0
Yawstickold = 0
Rollstickold = 0
Pitchstickold = 0

End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Im Unterschied zu meiner vorherigen Version ist hier die Fitness-Funtkion anders aufgebaut. Es wird nicht, wie vorher, die Fitness der geloggten Daten auf einmal bestimmt, sondern es wird nach jedem Datenloggen wieder ein Teil der Fitness bestimmt. Das hat den Vorteil, dass die Rechenzeit für die Fitness-Funktion gleichmäßiger verteilt ist und nicht mehr auf einen kurzen Zeitpunkt fixiert. Somit kann die Regelung besser funktionieren.
Das einzige, was jetzt noch fehlt, ist die "kontrollierte unkontrollierte" Abweichung im System, z.b. eine Windböe in der Software nachgebildet ^^
Das möchte ich über die Variable Log_cnt realisieren: Wenn diese in dem Bereich von x1 bis x2 ist, wird eine RC-Eingabe simuliert (z.b. Vollausschlag links)... Hier kann ich dann versch. beliebige Muster einprogrammieren, evtl. sogar über eine GUI (wird sicherlich noch folgen).
Habe mir jetzt auch zwei Bücher bestellt, welche das Thema naturanaloge Algorithmen behandeln (also auch Simulated Annealing, genet. Algos, usw...). Vielleicht finde ich ja darin dann noch ein paar Tipps zur besseren Umsetzung... Es gäbe da noch einiges, z.b. die Möglichkeit, einen Kandidaten mit schlechterer Fitness trotzdem zu übernehmen, um lokale Minima zu vermeiden, usw... Das kommt aber erst, wenn der Grundalgo mal läuft. Aber so wies aussieht, läuft er schon :)
Wenn Interresse besteht, kann ich ja mal ein Video von meinem 5 Minuten Teststand reinstellen...

[Che Guevara]

Hallo,

nun bin ich wieder etwas weiter, nachdem ich die Fitness-Funktion debuggt habe. Jetzt kommen richtige Werte raus, vorher nicht! Außerdem habe ich jetzt noch eine Art Startup Prozedur eingebaut, d.h. erst schwingt sich der Quadro ein und dann wird die Fitness von den Startparametern bestimmt.
Hier der Code:


$regfile = "m328pdef.dat"
$crystal = 16000000
$framesize = 200
$hwstack = 200
$swstack = 200
$baud = 38400


Declare Sub Serial0charmatch()

Config Serialin = Buffered , Size = 30 , Bytematch = 13


Declare Sub Simulated_annealing()



Dim T As Single
Dim T_a As Single

Dim Wi_kp As Single
Dim Wi_ki As Single
Dim Wi_kd As Single

Dim W0_kp As Single
Dim W0_ki As Single
Dim W0_kd As Single

Dim Rp As Single
Dim Ri As Single
Dim Rd As Single
Dim Rp_i As Integer
Dim Ri_i As Integer
Dim Rd_i As Integer

Dim Fp As Single
Dim Fi As Single
Dim Fd As Single

Dim F As Long
Dim F0 As Long

'------evtl. als long!!!------------------------------
Dim Extmp As Long
Dim Eytmp As Long
Dim Eztmp As Long
Dim Ax(50) As Integer 'Integer
Dim Ay(50) As Integer 'Integer
Dim Az(50) As Integer 'Integer
Dim Ex As Long
Dim Ey As Long
Dim Ez As Long

Dim Atmp As Word
Dim A As Word

Dim Log_step As Byte
Dim Log_cnt As Word
Dim Max_log_cnt As Word

Dim Tries As Word
Dim Max_tries As Word

Dim Test_anz As Long


Dim X As Integer



'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
Declare Sub Init_system()

Declare Sub Filter_gyro_data()
Declare Sub Filter_rx_data()
Declare Sub Mixer()

Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Failsave()

Declare Sub Set_pwm()



Config Portc.2 = Output
Config Portc.3 = Output
Portc.2 = 0
Portc.3 = 0


Config Portb.1 = Output
Config Portb.2 = Output
Config Portd.5 = Output
Config Portd.6 = Output


Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm1a = 113
Pwm1b = 113

Config Timer2 = Pwm , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 64
Pwm2a = 113
Pwm2b = 113


Config Pind.2 = Input
Portd.2 = 0

Config Int0 = Falling
On Int0 Measure
Enable Int0

Config Timer0 = Timer , Prescale = 256
On Timer0 Pausedetect
Enable Timer0


$lib "I2C_TWI.LBX"
Config Scl = Portc.5
Config Sda = Portc.4
Config Twi = 100000
I2cinit


Const Minthrottle = 50 '30
Const Maxchannel = 8
Const _throttlechannel = 1
Const _rollchannel = 2
Const _pitchchannel = 3
Const _yawchannel = 4
Const _statechannel = 5
Const _datachannel = 6


Dim I As Byte
Dim J As Byte
Dim Newvalsreceived As Bit
Dim _blink As Byte

Dim _yaw_kp As Single
Dim _roll_kp As Single
Dim _pitch_kp As Single

Dim _yaw_ki As Single
Dim _roll_ki As Single
Dim _pitch_ki As Single

Dim _yaw_kd As Single
Dim _roll_kd As Single
Dim _pitch_kd As Single

_yaw_kp = 0.4 '0.32
_roll_kp = 0.4 '0.32
_pitch_kp = 0.4 '0.32

_yaw_ki = 0.0008 '0.0008
_roll_ki = 0.0015 '0.0018
_pitch_ki = 0.0015 '0.0018

_yaw_kd = 0
_roll_kd = 0
_pitch_kd = 0

'_yaw_ki = 0.0001 '0.000135
'_roll_ki = 0.0001 '0.000135
'_pitch_ki = 0.0001 '0.000135

'_yaw_kd = 0.000007 '0.0000096
'_roll_kd = 0.000007 '0.0000096
'_pitch_kd = 0.000007 '0.0000096


'###################################
'#########RC-EMPFÄNGER##############
'###################################
Dim Bufferbyte As Byte
Dim Kanal(maxchannel) As Word
Dim Fkanal(maxchannel) As Word
Dim Channel As Byte
Dim _bl(4) As Word
Dim _sbl(maxchannel) As Integer
Dim State As Byte
Dim Oldstate As Byte
'###################################
'###################################
'###################################


'###################################
'###########I2C-Inputs##############
'###################################
Dim Wmplus_buffer(6) As Byte
Dim Ar(6) As Byte
'###################################
'###################################
'###################################


'###################################
'#########GYRO######################
'###################################
Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer
'###################################
'###################################
'###################################

'##################################
'#########PID-REGLER###############
'##################################
Dim _yaw_kp_err As Single
Dim _roll_kp_err As Single
Dim _pitch_kp_err As Single

Dim _yaw_ki_err As Single
Dim _roll_ki_err As Single
Dim _pitch_ki_err As Single

Dim _yaw_ki_sum As Single
Dim _roll_ki_sum As Single
Dim _pitch_ki_sum As Single

Dim _yaw_kd_err As Single
Dim _roll_kd_err As Single
Dim _pitch_kd_err As Single

Dim _yaw_kd_old As Single
Dim _roll_kd_old As Single
Dim _pitch_kd_old As Single

Dim _yaw_pid_int As Integer
Dim _roll_pid_int As Integer
Dim _pitch_pid_int As Integer

Dim _yaw_pid As Single
Dim _roll_pid As Single
Dim _pitch_pid As Single

Dim _yaw_err_int As Integer
Dim _roll_err_int As Integer
Dim _pitch_err_int As Integer

Dim _yaw_err As Single
Dim _roll_err As Single
Dim _pitch_err As Single
'#################################
'#################################
'#################################

Dim Setpoint_yaw As Single
Dim Setpoint_roll As Single
Dim Setpoint_pitch As Single

Dim Yawstickvel As Integer
Dim Rollstickvel As Integer
Dim Pitchstickvel As Integer

Dim Yawstickold As Integer
Dim Rollstickold As Integer
Dim Pitchstickold As Integer

Dim Hempf(maxchannel) As Word
Dim Lempf(maxchannel) As Word

Dim _x1 As Single
Dim _x2 As Single

Dim Rc_on As Word

Dim Failure As Byte

Dim New_parameters As Bit

Dim Sim_an_oldstate As Byte


'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

'PWM = 113 --> 0.900us
'PWM = 250 --> 2.000ms


'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########
'################################################# ################################################## ################################################## ###########




'-------initialize the values

'_roll_kp = 0.4
'_roll_ki = 0.0015

T = 1.0
T_a = 0.98 '0.8
F0 = 99999999
F = F0
W0_kp = 0.4
W0_ki = 0.0015 '0.001
W0_kd = 0.0 '0.000005
Fp = 0.2
Fi = 0.001 '0.0008
Fd = 0.00001 '0.00001
Log_step = 5
Max_log_cnt = 80 '50
Max_tries = 50 '25
New_parameters = 1


Dim Einschwingen As Word
Einschwingen = 0


Call Init_system()
Waitms 500
Enable Interrupts
Waitms 500



Rc_test:
If Kanal(_throttlechannel) <= 65 And Kanal(_statechannel) < 80 Then
If Rc_on < 65000 Then Incr Rc_on
Else
If Rc_on > 0 Then Decr Rc_on
End If
If Rc_on < 500 Then
Goto Rc_test
End If




Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
Call Mixer()
Call Failsave()
'#####
_bl(1) = 113
_bl(4) = 113
'#####
Call Set_pwm()
If State > 0 Then
Incr Einschwingen
End If


Loop Until Einschwingen = 3000

Portc.2 = 1





Do


Call Filter_rx_data()
Call Filter_gyro_data()


If State > 0 Then

_roll_kp = W0_kp
_roll_ki = W0_ki
_roll_kd = W0_kd

'-------log data and get fitness-----------------------------
Incr Log_cnt
If Log_cnt < Max_log_cnt Then
Ax(log_cnt) = _roll_err '_rollnow
If Log_cnt > Log_step Then
Atmp = Log_cnt - Log_step
Extmp = Ax(log_cnt) - Ax(atmp)
Extmp = Extmp * Extmp
Ex = Ex + Extmp
If Log_cnt >= 10 And Log_cnt < 20 Then
Setpoint_roll = 20
Elseif Log_cnt >= 40 And Log_cnt < 50 Then
Setpoint_roll = -20
End If
End If
Elseif Log_cnt = Max_log_cnt Then
F = Ex + Ey
F = F + Ez
F0 = F
Print "Fitness: " ; F
Ex = 0
Ey = 0
Ez = 0
Log_cnt = 0
Incr Tries
New_parameters = 1
Incr Test_anz
End If

End If


Call Pid_regulator()
Call Mixer()
Call Failsave()
'#####
_bl(1) = 113
_bl(4) = 113
'#####
Call Set_pwm()


Loop Until F <> 99999999





Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Simulated_annealing()
Call Pid_regulator()
Call Mixer()
Call Failsave()
'#####
_bl(1) = 113
_bl(4) = 113
'#####
Call Set_pwm()


Loop


Sub Simulated_annealing()

If State > 0 Then

'-------new parameters needed?--------------------------------
If New_parameters = 1 Then

New_parameters = 0

'-------randomize according to the current temperature--------
Rp_i = Rnd(200) - 100
Rp = Rp_i * 0.01
Rp = Rp * T
Ri_i = Rnd(200) - 100
Ri = Ri_i * 0.01
Ri = Ri * T
Rd_i = Rnd(200) - 100
Rd = Rd_i * 0.01
Rd = Rd * T

'-------generate new parameters------------------------------
Wi_kp = Rp * Fp
Wi_kp = Wi_kp + W0_kp
Wi_ki = Ri * Fi
Wi_ki = Wi_ki + W0_ki
Wi_kd = Rd * Fd
Wi_kd = Wi_kd + W0_kd

'-------cut off parameters-----------------------------------
If Wi_kp < 0.25 Then Wi_kp = 0.25
If Wi_kp > 0.5 Then Wi_kp = 0.5
If Wi_ki < 0.0005 Then Wi_ki = 0.0005
If Wi_ki > 0.003 Then Wi_ki = 0.003

'-------test current parameters------------------------------
_roll_kp = Wi_kp
_roll_ki = Wi_ki
_roll_kd = Wi_kd

End If

'-------log data and get fitness-----------------------------
Incr Log_cnt
If Log_cnt < Max_log_cnt Then
Ax(log_cnt) = _roll_err '_rollnow
If Log_cnt > Log_step Then
Atmp = Log_cnt - Log_step
Extmp = Ax(log_cnt) - Ax(atmp)
Extmp = Extmp * Extmp
Ex = Ex + Extmp
If Log_cnt >= 10 And Log_cnt < 20 Then
Setpoint_roll = 20
Elseif Log_cnt >= 40 And Log_cnt < 50 Then
Setpoint_roll = -20
End If
End If
Elseif Log_cnt = Max_log_cnt Then
F = Ex + Ey
F = F + Ez
'Print "Fitness: " ; F
Ex = 0
Ey = 0
Ez = 0
Log_cnt = 0
Incr Tries
New_parameters = 1
Incr Test_anz
End If

'-------better then current solution?------------------------
If F < F0 Then
F0 = F
W0_kp = Wi_kp
W0_ki = Wi_ki
W0_kd = Wi_kd
Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
End If

'-------counter for max tries---------------------------------
If Tries = Max_tries Then
Toggle Portc.2
T = T * T_a
Tries = 0
End If

End If

End Sub


Sub Serial0charmatch()
Clear Serialin
If State = 0 Then
Print Test_anz ; " " ; T ; " " ; F0 ; " " ; W0_kp ; " " ; W0_ki ; " " ; W0_kd
End If
End Sub


Sub Set_pwm()
If _bl(1) > 250 Then _bl(1) = 250
If _bl(1) < 113 Then _bl(1) = 113
If _bl(2) > 250 Then _bl(2) = 250
If _bl(2) < 113 Then _bl(2) = 113
If _bl(3) > 250 Then _bl(3) = 250
If _bl(3) < 113 Then _bl(3) = 113
If _bl(4) > 250 Then _bl(4) = 250
If _bl(4) < 113 Then _bl(4) = 113
Pwm2a = _bl(4) 'links vorne (US)
Pwm2b = _bl(2) 'links hinten (GUS)
Pwm1a = _bl(3) 'rechts vorne (GUS)
Pwm1b = _bl(1) 'rechts hinten (US)
End Sub


Sub Failsave()
If Channel >= 11 Then
If Failure < 255 Then
Incr Failure
End If
End If
If Channel < 11 Then
If Failure > 0 Then
Decr Failure
End If
End If
End Sub


Measure:
If Channel > 0 And Channel < 9 Then
Kanal(channel) = Timer0
End If
Timer0 = 6
Incr Channel
Return


Pausedetect:
If Channel <> 0 Then
Newvalsreceived = 1
End If
Channel = 0
Return


Sub Filter_gyro_data()
Call Read_wmp_data()

_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int
End Sub


Sub Filter_rx_data()

If Newvalsreceived = 1 Then
Newvalsreceived = 0

For I = 1 To Maxchannel
Fkanal(i) = Fkanal(i) * 3
Fkanal(i) = Fkanal(i) + Kanal(i)
Shift Fkanal(i) , Right , 2
Hempf(i) = Fkanal(i) + 17
Lempf(i) = Fkanal(i) - 17
If Kanal(i) < Lempf(i) Or Kanal(i) > Hempf(i) Then
Kanal(i) = Fkanal(i)
End If
Next I

If Kanal(_throttlechannel) >= 60 And Kanal(_throttlechannel) <= 140 Then
_sbl(_throttlechannel) = Kanal(_throttlechannel) - 100
_sbl(_throttlechannel) = _sbl(_throttlechannel) * 2
If _sbl(_throttlechannel) => 60 Then _sbl(_throttlechannel) = 60
If _sbl(_throttlechannel) < -68 Then _sbl(_throttlechannel) = -68
End If

For I = 2 To Maxchannel
If Kanal(i) >= 62 And Kanal(i) < 139 Then
_sbl(i) = Kanal(i) - 100
End If
Next I

If Kanal(5) < 80 Then State = 0
If Kanal(5) >= 80 And Kanal(5) < 115 Then State = 1
If Kanal(5) >= 115 Then State = 2


Setpoint_yaw = _sbl(_yawchannel) * 1.5

If State < 2 Then
Setpoint_roll = _sbl(_rollchannel) * 0.6
Setpoint_pitch = _sbl(_pitchchannel) * 0.6
Else
Setpoint_roll = _sbl(_rollchannel) * 1.5
Setpoint_pitch = _sbl(_pitchchannel) * 1.5
End If


If Kanal(3) > 120 And Kanal(4) > 120 And Kanal(1) < 70 And State = 0 Then
Call Set_wmp_offset()
End If

Oldstate = State

End If

End Sub


Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub


Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub


Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 4
Shift Roll , Right , 4
Shift Pitch , Right , 4
End Sub


Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
_yawoffset_int = _yawoffset
_rolloffset_int = _rolloffset
_pitchoffset_int = _pitchoffset
End Sub


Sub Pid_regulator()

'-------------YAW---------------
_yaw_err = Setpoint_yaw - _yawnow
_yaw_kp_err = _yaw_err * _yaw_kp
_yaw_ki_sum = _yaw_ki_sum + _yaw_err
If _yaw_ki_sum < -10000 Then _yaw_ki_sum = -10000
If _yaw_ki_sum > 10000 Then _yaw_ki_sum = 10000
_yaw_ki_err = _yaw_ki_sum * _yaw_ki
_yaw_kd_err = _yaw_err - _yaw_kd_old
_yaw_kd_old = _yaw_err
_yaw_kd_err = _yaw_kd_err * _yaw_kd

'-------------ROLL--------------
_roll_err = Setpoint_roll - _rollnow
_roll_kp_err = _roll_err * _roll_kp
_roll_ki_sum = _roll_ki_sum + _roll_err
If _roll_ki_sum < -10000 Then _roll_ki_sum = -10000
If _roll_ki_sum > 10000 Then _roll_ki_sum = 10000
_roll_ki_err = _roll_ki_sum * _roll_ki
_roll_kd_err = _roll_err - _roll_kd_old
_roll_kd_old = _roll_err
_roll_kd_err = _roll_kd_err * _roll_kd

'-------------PITCH-------------
_pitch_err = Setpoint_pitch - _pitchnow
_pitch_kp_err = _pitch_err * _pitch_kp
_pitch_ki_sum = _pitch_ki_sum + _pitch_err
If _pitch_ki_sum < -10000 Then _pitch_ki_sum = -10000
If _pitch_ki_sum > 10000 Then _pitch_ki_sum = 10000
_pitch_ki_err = _pitch_ki_sum * _pitch_ki
_pitch_kd_err = _pitch_err - _pitch_kd_old
_pitch_kd_old = _pitch_err
_pitch_kd_err = _pitch_kd_err * _pitch_kd


_yaw_pid = _yaw_kp_err + _yaw_ki_err
_yaw_pid = _yaw_pid + _yaw_kd_err
_yaw_pid_int = _yaw_pid

_roll_pid = _roll_kp_err + _roll_ki_err
_roll_pid = _roll_pid + _roll_kd_err
_roll_pid_int = _roll_pid

_pitch_pid = _pitch_kp_err + _pitch_ki_err
_pitch_pid = _pitch_pid + _pitch_kd_err
_pitch_pid_int = _pitch_pid

If _yaw_pid_int < -68 Then _yaw_pid_int = -68
If _yaw_pid_int > 68 Then _yaw_pid_int = 68
If _roll_pid_int < -68 Then _roll_pid_int = -68
If _roll_pid_int > 68 Then _roll_pid_int = 68
If _pitch_pid_int < -68 Then _pitch_pid_int = -68
If _pitch_pid_int > 68 Then _pitch_pid_int = 68

End Sub


Sub Mixer()

'_bl(1) = rechts hinten (US)
'_bl(2) = links hinten (GUS)
'_bl(3) = rechts vorne (GUS)
'_bl(4) = links vorne (US)

_bl(1) = 182 + _sbl(_throttlechannel)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = _bl(1)

If State <> 0 And Failure < 20 Then

_bl(1) = _bl(1) + Minthrottle
_bl(2) = _bl(2) + Minthrottle
_bl(3) = _bl(3) + Minthrottle
_bl(4) = _bl(4) + Minthrottle

_bl(1) = _bl(1) - _pitch_pid_int
_bl(4) = _bl(4) + _pitch_pid_int

_bl(2) = _bl(2) - _roll_pid_int
_bl(3) = _bl(3) + _roll_pid_int

_bl(1) = _bl(1) - _yaw_pid_int
_bl(2) = _bl(2) + _yaw_pid_int
_bl(3) = _bl(3) + _yaw_pid_int
_bl(4) = _bl(4) - _yaw_pid_int

Else

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_yaw_kp_err = 0
_yaw_ki_sum = 0
_yaw_kd_err = 0
_yaw_ki_err = 0
_roll_kp_err = 0
_roll_ki_sum = 0
_roll_kd_err = 0
_roll_ki_err = 0
_pitch_kp_err = 0
_pitch_ki_sum = 0
_pitch_kd_err = 0
_pitch_ki_err = 0
_yaw_pid_int = 0
_roll_pid_int = 0
_pitch_pid_int = 0
_yaw_pid = 0
_roll_pid = 0
_pitch_pid = 0
Yawstickvel = 0
Rollstickvel = 0
Pitchstickvel = 0
Yawstickold = 0
Rollstickold = 0
Pitchstickold = 0

End If

End Sub


Sub Init_system()

_bl(1) = 113
_bl(2) = 113
_bl(3) = 113
_bl(4) = 113

_sbl(1) = -68
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0
_sbl(5) = -600
_sbl(6) = -600
_sbl(7) = 0
_sbl(8) = 0

Fkanal(1) = 63
Fkanal(2) = 100
Fkanal(3) = 100
Fkanal(4) = 100
Fkanal(5) = 63
Fkanal(6) = 63
Fkanal(7) = 100
Fkanal(8) = 100

Reset Newvalsreceived

State = 0
Oldstate = 0

_yawnow = 0
_rollnow = 0
_pitchnow = 0

Call Wmp_init()
Waitms 500
For I = 1 To 10
Call Read_wmp_data()
Next I
Waitms 500
Call Set_wmp_offset()

Rc_on = 0

End Sub

End

Jetzt muss ich nur noch die Simulated-Annealing Parameter richtig einstellen xD Aber vom Prinzip her funktionierts.
Evtl. werd ich das ganze morgen mal, mit kleiner Temperatur, im richtigen Flug (also ohne Teststand) probieren... Mal sehen.

Gruß
Chris

EDIT:
Um das ganze besser testen zu können, verwende ich nicht die Gyro-werte für die Fitness-Berechnung, sondern den Fehler aus Gyro-wert minus RC-wert. Somit kann auch mit einbezogen werden, wie gut die Parameter dem Piloten folgen. Was haltet ihr davon?

[heinzchr]

Hallo,

das sieht sehr interessant aus. hab mal nen genalg am standartbeispiel handlungsreisende in java gemacht. das ganze für einen pid regler ist ne feine sache. gibts schon ein video oder ist es angedacht?

[Che Guevara]

Hallo,

ja, das beispiel kenne ich auch, aber ehrlich gesagt finde ich das komplizierter als für die PID-Regelung...
Ein Video wird folgen, sobald die Ergebnisse einigermassen brauchbar sind. Momentan kämpfe ich mit den richtigen Parametern fürs Simulated Annealing. Da ich jetzt aber auch 2 Bücher zum Thema habe, werde ich auch mal versuchen, einen genet. Algo dafür zu schreiben, weil ich glaube, dass sich dieser besser eignet.... mal sehen.

Gruß
Chris

[Che Guevara]

Hallo,

nachdem ich nun längere Zeit nichts mehr gemacht habe, melde ich mich mal wieder. Gestern Abend habe ich auf die schnelle mal eine Quick&Dirty Umsetzung eines SA in vb.net geschrieben. Das Programm funktioniert m.E.n. sehr gut, man kann gut erkennen, wie sich die Parameter aufs Ergebnis auswirken. Wers mal testen möchte, das Prog ist im Anhang. Einfach Ta und Y eingeben und auf Start drücken (bitte für Fließkommaeingaben nur den . verwenden). Wie gesagt, ist Quick&Dirty, aber es stellt für mich einen großen Ansporn dar, weiter zu machen. Heute werde ich mich mal an einem GA in vb.net versuchen.

Gruß
Chris

[Che Guevara]

Hallo,

gestern habe ich die ersten erfolgreichen Tests mit meinem GA durchgeführt. Das Programm ist wieder Quick&Dirty, funktioniert aber. Hier zeigen sich auch schon die großen Unterschiede zw. SA und GA. Beim GA passiert es des öfteren, dass er nie eine optimale Lösung findet und in irgendeinem lokalen Minimum hängen bleibt. Außerdem benötigt man eine relativ große Population, um mehr oder minder sicher und schnell das optimale Ergebnis zu erreichen. Als guten Wert hat sich jetzt bei mir der Wert 500 (für m) herauskristallisiert. Allerdings muss ich dazu sagen, dass bis jetzt keine Mutation integriert ist (kommt aber noch) und die Rekombination ist eine einfache Mittelwertbildung von den 2 Werten. Auch das Heiratsschema ist nicht optimal, allerdings habe ich bis jetzt noch keinerlei Quellen gefunden, wie man ein einigermaßen gutes Heiratsschema implementiert... Wenn jemand da einen Tipp hat, immer her damit :D

Das Programm ist wieder im Anhang, setzt jedoch, genauso wie das SA das .net Framework vorraus.

Gruß
Chris

EDIT:
Gerade eben habe ich das Heiratsschema verändert. Jetzt werden zufällig einzelne Individuen miteinander rekombiniert, das brachte eine viel kürzere Laufzeit bei nur m = 50. Das neue Programm ist im Anhang.

[ManuB93]

Hallo,

der Thread ist zwar schon etwas älter aber hat die Parameterbestimmung so gut funktioniert, dass der Quadrokopter auch in der Luft stabil fliegt? Hast du an den Algorithmen noch weitergearbeitet oder ruht das Projekt?

Gruß Manu

[Che Guevara]

Hi,

ja es hat sehr gut funktioniert! Wenn man den richtigen Parameter zur "Abkühlung" gefunden hat, ist der Algo unglaublich schnell und kommt zu guten Ergebnissen! Irgendwann hab ich den Algo dann aber wieder aus dem Quadro geschmissen, weil ich die Regelalgorithmen verbessert habe und ich ihn somit nicht mehr brauche.

Gruß
Chris

[ManuB93]

Hast du dann zur Parameterbestimmung eine bestimmte Auslenkung programmiert wie du beschrieben hast? Also eine Auslenkung aus der Gleichgewichtslage, die eine Windböe simuliert und dann vom Quadrokopter ausgesteuert werden muss? Und dann wurde dieser Vorgang so lange wiederholt, bis das zurückkehren in die gleichgewichtslage schnell ging und die Parameter gut waren?
Danke ! Sehr interessantes Thema.

Gruß Manu

[Che Guevara]

Das hab ich mal ausprobiert, war allerdings nicht der richtige Weg! Letzendlich hab ichs so gelöst, dass der gewollte Fehler durch die Funke (also der Soll-Wert) als sozusagen Auslöser zur Bestimmung des Fitnesswerts genutzt wurde. Also nur, solange aktiv die Lage verändert wird, ist der Algo aktiv.

Gruß
Chris