PID Regler - Hilfe! Atmega8

[bexter]

Habe ein Problem, und hoffe Ihr könnt helfen!
Ich habe einen Roboter mit 2 gehackten Servos (1-2ms Pulse von rückwärts vollgas - vorwärts vollgas)
Dazu Radencoder mit 72 Inpulsen pro Umdrehung. 1 Rad-Umdrehung benötigt etwa 1 sek..
(das Rad hat 89mm also ca. 28cm Durchmesser) = 3,88mm pro click

So ich habe schon eine PWM für die Geschwindigkeit implementiert Mit Timer0 ovflw
Der Wert 51= stillstand
Der Wert 66= vollgas vorwärts
Der Wert 36= vollgas rückwärts

So jetzt möchte ich eine PID-Regelung. Wie mach ich das?
Die Encoder liefern auf INT0 und INT1 Interrupts.
Hab jetzt den Timer1 auf 1ms gestellt, und zähle bei überlauf einen Zähler hoch.
Kommt z.B. INT0 schaue ich wieviele 1ms vergangen sind zwischen 2 Encoder Pulsen.

Für die die bis jetzt durchgehalten haben ein Bild als Belohnung:


Dh. also bei Vollgas hab ich 13,56 ms zwischen 2 Encoderpulsen

Der einfachheit halber mal ein PD Regler betrachtet:

Sagen wir ich gebe Soll(zwischen 2 Pulsen) = 10ms vor.

Am Anfang hab ich 0ms zwischen 2 Pulsen. D.h

t0:
Error = 10ms-0ms = 10ms

P= 10ms(Error) * Verstärkung Kp(z.B.2x) = 20
D= 0 (Error alt) - 10 (Error neu) / 10ms(oder wie??) = -10 (oder -1?) * Verstärkung Kd(z.b. 1x) = -10 (oder -1)

PD= 20 + -10 = 10 (Stellgrösse)
(oder) PD = 20 + -1 = 19 (Stellgrösse)

t1:
Error = 10ms-5ms(angenommen)

P= 5ms(Error) * Verstärkung Kp(z.b.2x) = 10
D= 10ms(Error alt) - 5ms (Error neu) / 10ms(<-stimmt das?) = 5 (oder 0,5)

PD= 10 + 5 = 15?
(oder PD = 10 + -0,5 = 9,5)

Kenn mich nimmer aus lol...
Und was mach ich dann mit der Stellgrösse?

LG Alex