Ich hab jetzt eigentlich alles fertig ausser die Funktionen zur regullierten Fahrt. Mir macht die funktion zum Feradefahren zu schaffen. Er will einfach nicht gerade fahren. Kann irgendtjemand einen Fehler finden. danke.



Code:
$regfile = "m128def.dat"       'es handelt sich um einen ATmega128
$framesize = 42
$swstack = 32
$hwstack = 32
$crystal = 14745600       'Systemtakt 14.7456 MHZ
$baud = 19200       'UART Übertragungsgeschwindigkeit

Encoderled Alias Portb.0       'es wird auf beide Encoder der Radencoder
Config Encoderled = Output       'zugegriffen

Motorleft Alias Portb.6       'Motor-PWM-Leitung-Links
Config Motorleft = Output

Motorright Alias Portb.5       'Motor-PWM-Leitung-Rechts
Config Motorright = Output

Motorenable Alias Portb.7       'Motorenable
Config Motorenable = Output

Encoderled = 0       'Die Encoder-Leds sind wieder über Ground Angeschlossen
Motorenable = 0       'Motoren sind aus

Declare Sub Moveatspeed(byval Sleft As Byte , Byval Sright As Byte)       'Stelt die Motor-PWM ein  

Declare Sub Encled(byval Status As Byte)       'schaltet die Encoder-Led bei 1 ein

Declare Sub Intodometrie()       'konfiguriert die Encoder der Odmetrie und setzt Encleft und encright auf null  

Declare Sub Geradefahren()       'vergleicht die Odometriewerte und regelt nach

Dim Encleft As Word       'der Zwischenspeicher für die Werte des linken Odometrie Encoders
Dim Encright As Word       'der Zwischenspeicher für die Werte des rechten Odmetrie Encoders

Dim Encoderenable As Byte       'nur wenn diese Varriable 1 werden die Odometrie-Encoder-Impulse gezählt  

Dim Sw0ys As Long       'Stopwatch 0 wird für die Sub Gerdefahren benötigt
Dim Sw0ms As Long 

Config Timer3 = Timer , Prescale = 8       'Timer 3 (Stopwatches) wird als Timer eingestellt und mit den Prescaler 8 versehen
Timer3 = 63693       'Timer 3 wird auf 63693 eingestellt dies passiert auch nach jedem Interrupt
On Ovf3 On_ovf3       'wenn Timer 3 überläuft springt das Programm zu On_ovf3
Enable Ovf3       'der Overflow von Timer 3 wird aktiviert
Enable Interrupts       'Interrups werden eingeschaltet
Start Timer3       'Timer 3 wird gestartet

Call Intodometrie()
Do

Call Moveatspeed(240 , 240)
Call Geradefahren()

Loop

End

Sub Encled(byval Status As Byte)       'Eigentlich unnötig aber der Vollständigkeit halber
If Status = 1 Then
Encoderled = 1
Elseif Status = 0 Then
Encoderled = 1
End If

Sub Intodometrie()       'Konfiguriert die Odometrie

Encleft = 0       'Encleft wird zurückgesetzt
Encright = 0       'Encright wird zurückgesetzt
Encoderenable = 1       'die Odometrie-Encoder-Impulse können nun gezählt werden
Call Encled(1)       'die Encoder-Leds werden eingeschaltet
On Int7 Encoleft       'wenn ein Impuls vom linken Encoder kommt zu Encoleft springen
On Int6 Encoright       'wenn ein Impuls vom rechten Encoder kommt zu Encoright springen
Config Int7 = Falling       'springen wenn ein Impuls abgeschlossen ist
Config Int6 = Falling       'springen wenn ein Impuls abgeschlossen ist
Enable Int7       'Int7 freischalten
Enable Int6       'Int6 freischalten
Enable Interrupts       'Interrups aktivieren
End Sub

Sub Geradefahren()       'Sub für die Gerade Fahrt des Roboters
Local Encdif As Integer       'Lokale Varriable für den Unterschied von Encleft und Encright
Local Encregler As Integer       'Lokale Varriable für das nachregeln der Geschwindigkeit
Sw0ms = Sw0ys / 10       'die ys von Stopwatch 0 werden in ms umgewandelt
If Sw0ms = 10 Then       'sind 10ms vergangen ?
Sw0ys = 0 'Stopwatch 0 zurück setzen
If Encleft > Encright Then       'werden mehr Odometrie-Encoder-Impulse links gezählt ?
Encdif = Encleft. - Encright       'Encoder Differenz berechnen
Encregler = Encdif * 0.1       'Nachregelungsvarriable berechnen, diese berechnet sich aus 10% der Encoder Differenz
Compare1a = Compare1a + Encregler       'PWM-Links um Nachregelungsvarriable erhöhen
Compare1b = Compare1b - Encregler       'PWM-Rechts  um Nachregelungsvarriable verringern
Encleft = 0
Encright = 0
Elseif Encleft < Encright Then       'werden mehr Odometrie-Encoder-Impulse rechts gezählt?
Encdif = Encright - Encleft       'Encoder Differenz berechnen
Encregler = Encdif * 0.1       'Nachregelungsvarriable berechnen
Compare1a = Compare1a - Encregler       'PWM-Links um Nachregelungsvarriable verringern
Compare1b = Compare1b + Encregler       'PWM-Rechts um Nachregelungsvarriable erhöhen
Encleft = 0
Encright = 0
End If
End If
End Sub

Sub Moveatspeed(byval Sleft As Byte , Byval Sright As Byte)       'Sub zur Regelung der Motorgeschwindikeiten

Config Timer1 = Pwm , Pwm = 8 , Compare A Pwm = Clear Down , Compare B Pwm = Clear Down , Prescale = 1       'Timer 1 wird als zwei 8-Bit PWM Leitungen mit dem Prescaler 1 konfiguriert
Start Timer1       'Timer 1 wird gestartet
Motorenable = 1       'Motoren werden entsperrt

Compare1a = Sright       'PMW-A entspricht der linken Geschwindikeit
Compare1b = Sleft       'PWM-B entspricht der rechten Geschwindikeit

End Sub

Encoleft: 'Interrupt-Rotine die ausgelöst wird wenn links ein Odometrie-Encoder-Impuls kommt
If Encoderenable = 1 Then
Encleft = Encleft + 1
End If
Return

Encoright:       'Interrupt-Rotine die ausgelöst wird wenn rechts ein Odometrie-Encoder-Impuls kommt
If Encoderenable = 1 Then
Encright = Encright + 1
End If
Return

On_ovf3:  'Hierher wird wenn Timer 3 überläuft
Timer3 = 63693
Sw0ys = Sw0ys + 1
If Sw1enable = 1 Then
Sw1ys = Sw1ys + 1
End If
Return