ATMega32 Autonomes Fahren

Hallo,
ich bin grade am Programmieren des ATMega32.
Mein Programm soll folgendes erfüllen:
1.der Roboter soll gerade fahren
2. und wenn er gegen die taster vorne fährt sich ein bisschen dreht
Bei mir ist das Problem, dass er am Anfang grade aus fährt und wenn der
Roboter gegen was fährt auf einmal rückwärts fährt:confused:
damit ihr euch das angucken könnt, ist hier der Programmiercode:

Code:

---

.NOLIST                    ; List-Output unterdrücken
 .INCLUDE <m32def.inc>      ; das gibt es für jeden Controllertyp
 .LIST                      ; List-Output wieder aufdrehen
 .CSEG                      ; was nun folgt, gehört in den FLASH-Speicher


.def tmp          = r16
.def count0              = r17
.def count1              = r18
.def count2              = r19
.def compare_value = r23
.def reload        = r24
.def softcounter  = r25
.def sollwert          = r20
.def pulseMotor2  = r21
.def calc                  = r22
.def sollwert_M1  = r0
.def pulseMotor1  = r1
.def compare_value_M1 = r2

.equ LED              = PC7
.equ Taster4  = PD2
.equ Taster6  = PD3
.equ PWM_1    = PD4
.equ PWM_2    = PD5
.equ Pulse1    = PB0
.equ Pulse2    = PB2
.equ Richtung1 = PB1
.equ Richtung2 = PB3
.EQU  clk = PA4
.EQU  richtung = PA5
.EQU  strom = PA6



 ;------------------------------------------------------
 ;    Warteschleife ca. 1 ms multipliziert
 ;    mit dem übergebenen Parameter
 ;------------------------------------------------------

.MACRO  warten
        ldi  count0, @0  ; Lade direkten Wert in R17 1 Zyklus
LOOP2:  ldi  count1, 100  ; Lade direkten Wert in R18 1 Zyklus
LOOP1:  ldi  count2, 52  ; Lade direkten Wert in R19 1 Zyklus
LOOP0:  dec  count2        ; Dekrementiere R19 -> Z ist gesetzt, wenn R19 == 0
                                                ; 1 Zyklus
        brne LOOP0          ; Teste Zero Flag (Z)
                                ; 1 Zyklus, wenn wahr, 2 Zyklen, wenn falsch
        dec  count1                  ; Dekrementiere R18 -> Z ist gesetzt, wenn R18 == 0
                                                ; 1 Zyklus
        brne LOOP1          ; Teste Zero Flag (Z)
                                ; 1 Zyklus, wenn wahr, 2 Zyklen, wenn falsch
        dec  count0              ; Dekrementiere R17 -> Z ist gesetzt, wenn R17 == 0
                                                ; 1 Zyklus
        brne LOOP2          ; Teste Zero Flag (Z)

.ENDM

.MACRO  warten10msMalX
        ldi  count0, @0  ; Lade direkten Wert in R17 1 Zyklus
LOOP2:  ldi  count1, 226  ; Lade direkten Wert in R18 1 Zyklus
LOOP1:  ldi  count2, 235  ; Lade direkten Wert in R19 1 Zyklus
LOOP0:  dec  count2        ; Dekrementiere R19 -> Z ist gesetzt, wenn R19 == 0
                                                ; 1 Zyklus
        brne LOOP0          ; Teste Zero Flag (Z)
                                ; 1 Zyklus, wenn wahr, 2 Zyklen, wenn falsch
        dec  count1                  ; Dekrementiere R18 -> Z ist gesetzt, wenn R18 == 0
                                                ; 1 Zyklus
        brne LOOP1          ; Teste Zero Flag (Z)
                                ; 1 Zyklus, wenn wahr, 2 Zyklen, wenn falsch
        dec  count0              ; Dekrementiere R17 -> Z ist gesetzt, wenn R17 == 0
                                                ; 1 Zyklus
        brne LOOP2          ; Teste Zero Flag (Z)

.ENDM



 ;------------------------------------------------------
 ;    Start Adresse 0000
 ;------------------------------------------------------
 RESET:
    jmp INIT          ; springen nach "INIT"

 ;------------------------------------------------------
 ;    ISR VECTORS
 ;------------------------------------------------------
.ORG 0x002
    jmp EXTERN0      ;Interruptvektor für INT0, hier
                      ;spring das Programm hin, wenn an Pin D.2 ein Flankenübergang
                                          ;erfolgt.
.ORG 0x004
    jmp EXTERN1      ;Interruptvektor für INT1, hier
                      ;spring das Programm hin, wenn an Pin D.3 ein Flankenübergang
                                          ;erfolgt.
.ORG 0x006
    jmp EXTERN2      ;Interruptvektor für INT2, hier
                      ;spring das Programm hin, wenn an Pin B.2 ein Flankenübergang
                                          ;erfolgt.

.ORG 0x012
    jmp Timer1OVF    ;Interruptvektor für Timer1, hier
                      ;spring das Programm hin, wenn Timer 1 überläuft
                                         


 .ORG INT_VECTORS_SIZE    ; dadurch haben wir für die Vektoren Platz gelassen
 INIT: 
 ;------------------------------------------------------
 ;    INITIALIZE
 ;------------------------------------------------------
    ldi tmp,high(RAMEND)    ;Stack Pointer setzen
    out SPH,tmp              ; "RAMEND" ist in m32def.inc (s.o.) festgelegt
    ldi tmp,low(RAMEND)      ;
    out SPL,tmp              ;


 ;------------------------------------------------------
 ;  eigene Initialisierungen
 ;------------------------------------------------------
   
        LDI tmp, 0
        LDI tmp, (1<<PWM_1)|(1<<PWM_2)  ; PWM Bits auf eins
    OUT DDRD, tmp                  ; das kommt ins Control-Register f. PortD
                                    ; dadurch sind Pullups an Port an                                         
    LDI tmp, 0
    LDI tmp, (1<<Richtung1)|(1<<Richtung2) ; Richtungs Bits auf eins
    OUT DDRB, tmp                          ; das kommt ins Control-Register f. PortB
                                          ; dadurch sind Pullups an Port an
                                                                                 
       
                                                                 
    ldi tmp, 255
        out DDRC, tmp
        out PortC, tmp
                                         
       
        ; Timer Register werden belegt
    ; Timer 1
        in  tmp, TCCR1A
        ldi tmp,  (1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM10);
        out TCCR1A, tmp
        in  tmp, TCCR1B
        ldi tmp,  (1<<WGM12)|(1<<CS12)|(0<<CS11)|(0<<CS10)            ;Vorteiler ist 256
        out TCCR1B, tmp


    ; Timer 0
    in  tmp, TCCR0
        ldi tmp,(1<<CS02) | (1<<CS01)    ;Externe Clock, fallende Flanke
    out TCCR0,tmp                                    ;Register TCCR0 ist für die Clock


    in  tmp, MCUCR                ;MCU Control Register einlesen
        ldi tmp, (1<<ISC11)|(1<<ISC01) ;Interrupt Sense Control
        out MCUCR, tmp                                        ;für INT0 und INT1 auf fallende Flanke
    in  tmp, GICR                  ;General Interrupt Control Register einlesen
        ldi tmp, (1<<INT2)|(1<<INT1)|(1<<INT0)        ;Externe Interrupts 0, 1 und 2 an
    out GICR, tmp
    in tmp, TIMSK                  ;Timer Interrupt Mask Register einlesen
        ldi tmp, (1<<TOIE1)                        ;Timer 1 Overflow IR an
    out TIMSK, tmp


        sei                                                      ;Interrupts allgemein zulassen

        ldi compare_value, 40        ;PWM erst mal auf 50%
        ldi tmp, 50
        mov compare_value_M1, tmp
        ldi reload, 39                ;T1 Overflow erfolgt nach 217 * 256 * 62,5 ns
                                                                        ;-> Interrupt alle 3,427 ms
        ldi softcounter, 144                        ;-> für 2 Hz Samplerate nur alle 144 Interrupts
                                                                        ;  Zähler auslesen und zurücksetzen
    ldi pulseMotor2, 0
        out TCNT1L, reload
        sbi PortD, Taster4                                ;erst mal eine 1 auf die Taster
    sbi PortD, Taster6                                ;damit die fallende Flanke
                                                                        ;den IR auslöst.
    sbi PortB, Pulse1       
        sbi PortB, Pulse2

        sbi PortB, Richtung1       
        sbi PortB, Richtung2

        sbi DDRA,clk                      ;clk Pin als Ausgang
    sbi DDRA,richtung                        ;richtung Pin als Ausgang
    sbi DDRA,strom                                ;strom Pin als Ausgang
    sbi PORTA,clk                                ;clk Bit setzen
    cbi PORTA,richtung                        ;Richtung vorgeben
    sbi PORTA,strom                        ;Richtung vorgeben


        ldi sollwert, 20
   

 ;------------------------------------------------------
 ;  HAUPTSCHLEIFE
 ;------------------------------------------------------
 Hauptschleife:

    sbis PINA, PA0
        call change_left
        sbis PINA, PA1
        call change_dir
        sbis PINA, PA2
        call change_right

       

    rjmp Hauptschleife            ; immer wiederholen

 Ende: 
        rjmp Ende

;------------------------------------------------------
 ;  ISR Extern0
 ;------------------------------------------------------
 EXTERN0:

        ; Erst mal alles auf den Stack

        PUSH tmp                ;benutzte Register speichern
        IN tmp, SREG        ;Statusregister retten
    PUSH tmp                ;
   
    ;Hier kommt unsere eigentliche Bearbeitung
        ;
        warten 1                ;1 ms warten, da Taster prellt
       
    ;Richtung umkehren
        sbi PortB, Richtung1       
        sbi PortB, Richtung2
        warten10msMalX 40
        ;Kurve einleiten
        cbi PortB, Richtung2
        warten10msMalX 20
        ;und wieder gerade
        cbi PortB, Richtung1
   
        in  tmp, GIFR                                 
    ldi tmp, (1<<INTF0)            ;durch Prellen des Tasters wurde schon
        out GIFR, tmp                                ;ein neuer Interrrupt getriggert
                                                                ;manuelles setzen des Flags verhindert
                                                                ;eine erneute Ausführung der Routine
                                                                ;nach dem Rücksprung
           
        ; Jetzt Register vom Stack wiederholen         
    POP tmp                ;Statusregister wiederherstellen
        OUT SREG, tmp        ;
        POP tmp                ;benutzte Register wiederherstellen

    RETI                        ;Rücksprung

 ;------------------------------------------------------
 ;  ISR Extern1
 ;------------------------------------------------------
 EXTERN1:

        ; Erst mal alles auf den Stack
        PUSH tmp                ;benutzte Register speichern
        IN tmp, SREG        ;Statusregister retten
    PUSH tmp                ;
   
    ;Hier kommt unsere eigentliche Bearbeitung
        warten 1                ;1 ms warten, da Taster prellt
        ;Richtung umkehren
        sbi PortB, Richtung1       
        sbi PortB, Richtung2
        warten10msMalX 40
        ;Kurve einleiten
        cbi PortB, Richtung1
        warten10msMalX 20
        ;und wieder gerade
        cbi PortB, Richtung2
                                                   
    in  tmp, GIFR
    ldi tmp, (1<<INTF1)            ;durch Prellen des Tasters wurde schon
        out GIFR, tmp                                ;ein neuer Interrrupt getriggert
                                                                ;manuelles setzen des Flags verhindert
                                                                ;eine erneute Ausführung der Routine
                                                                ;nach dem Rücksprung
           
        ; Jetzt Register vom Stack wiederholen         
    POP tmp                ;Statusregister wiederherstellen
        OUT SREG, tmp        ;
        POP tmp                ;benutzte Register wiederherstellen

    RETI                        ;Rücksprung

 ;------------------------------------------------------
 ;  ISR Extern2
 ;------------------------------------------------------
 EXTERN2:

        ; Erst mal alles auf den Stack
        PUSH tmp                ;benutzte Register speichern
        IN tmp, SREG        ;Statusregister retten
    PUSH tmp                ;
   
    ;Hier kommt unsere eigentliche Bearbeitung
        inc pulseMotor2
           
        ; Jetzt Register vom Stack wiederholen         
    POP tmp                ;Statusregister wiederherstellen
        OUT SREG, tmp        ;
        POP tmp                ;benutzte Register wiederherstellen

    RETI                        ;Rücksprung
 ;------------------------------------------------------
 ;  ISR Timer1 Overflow
 ;------------------------------------------------------
 Timer1OVF:
    dec softcounter
        brne go_back

    ; Erst mal alles auf den Stack
        PUSH tmp                ;benutzte Register speichern
        IN tmp, SREG        ;Statusregister retten
    PUSH tmp                ;
   
    ;Hier kommt unsere eigentliche Bearbeitung
        mov tmp, pulseMotor2         
        call regelung_M2             
        com tmp          ;Zählerstand auf
        out PortC, tmp    ;LEDs ausgeben
   
        ldi tmp, 0
        out TCNT2, tmp
        out TCNT0, tmp
        ldi pulseMotor2, 0
        out TCNT1L, reload
        ldi softcounter, 144

               
                                                               
        ; Jetzt Register vom Stack wiederholen         
    POP tmp                ;Statusregister wiederherstellen
        OUT SREG, tmp        ;
        POP tmp                ;benutzte Register wiederherstellen
go_back:
    RETI                        ;Rücksprung

 ;------------------------------------------------------
 ;  Unterprogramm Regelung Motor 2 nach Sollwert
 ;------------------------------------------------------
regelung_M2:
   
        push tmp
       
        mov sollwert_M1, pulseMotor2        ;aktuelle Pulse sind Sollwert für Motor1
        in pulseMotor1, TCNT0                        ;pulse für Motor1 festhalten

        clc
        cp sollwert, pulseMotor2
        brne checkDirection
        call regelung_M1
        pop tmp
        ret
 checkDirection:
    brcs langsamer        ;Carry gesetzt, wenn pulseMotor2 > Sollwert
 schneller:
    mov calc, sollwert
    sub calc, pulseMotor2                        ;Pulse vom Sollwert abziehen
        mov tmp, compare_value                        ;schauen, ob Maximaldrehzahl schon erreicht
        clc
        add tmp, calc                                        ;Regelabweichung addieren
        brcs back                                                ;größer 255
        mov calc, tmp                  ;Ergebnis der Addition in calc                               
        mov compare_value, calc               
        out OCR1AL, compare_value                ;compare Wert schreiben
        call regelung_M1
        pop tmp
        ret
 langsamer:
    mov calc, pulseMotor2
    sub calc, sollwert                                ;Sollwert von Pulsen abziehen
        mov tmp, compare_value                        ;schauen, ob Minimaldrehzahl schon erreicht
        sub tmp, calc                                        ;Regelabweichung subtrahieren
        mov calc, tmp                  ;Ergebnis der Subtraktion in calc
    ldi tmp, 55
        clc                                                                ;carry löschen
    cp calc, tmp                                        ;schauen, ob calc (eigentlich neuer compare_value)                                       
        brcs back                                                ;kleiner 55
    mov compare_value, calc        ;wenn nicht, übernehmen               
        out OCR1AL, compare_value                ;compare Wert schreiben
 back:
    call regelung_M1
    pop tmp
    ret

 ;------------------------------------------------------
 ;  Unterprogramm Regelung Motor 1 nach Motor 2
 ;------------------------------------------------------

 regelung_M1:
   
        push tmp
        clc
       
        cp sollwert_M1, pulseMotor1
        brne checkDirection_M1
        pop tmp
        ret
 checkDirection_M1:
    brcs langsamer_M1                        ;Carry gesetzt, wenn pulseMotor2 > Sollwert
 schneller_M1:
    mov calc, sollwert_M1
    sub calc, pulseMotor1                        ;Pulse vom Sollwert abziehen
        mov tmp, compare_value_M1                        ;schauen, ob Maximaldrehzahl schon erreicht
        clc
        add tmp, calc                                        ;Regelabweichung addieren
        brcs back_M1                                                ;größer 255
        mov calc, tmp                  ;Ergebnis der Addition in calc                               
        mov compare_value_M1, calc
        out OCR1BL, compare_value_M1                ;compare Wert schreiben
        pop tmp
        ret
 langsamer_M1:
    mov calc, pulseMotor1
    sub calc, sollwert_M1                                ;Sollwert von Pulsen abziehen
        mov tmp, compare_value_M1                        ;schauen, ob Minimaldrehzahl schon erreicht
        sub tmp, calc                                        ;Regelabweichung subtrahieren
        mov calc, tmp                  ;Ergebnis der Subtraktion in calc
    ldi tmp, 55
        clc                                                                ;carry löschen
    cp calc, tmp                                        ;schauen, ob calc (eigentlich neuer compare_value)                                       
        brcs back_M1                                                ;kleiner 55
    mov compare_value_M1, calc        ;wenn nicht, übernehmen
        out OCR1BL, compare_value_M1                ;compare Wert schreiben
 back_M1:
        pop tmp       
    ret

change_left:
      call step
    ;Richtung umkehren

        cbi PortB, Richtung1       
        cbi PortB, Richtung2
        warten10msMalX 20
        ;Kurve einleiten
        sbi PortB, Richtung2
        warten10msMalX 10
        ;und wieder gerade
        sbi PortB, Richtung1
        ret

change_right:
    call step
    cbi PortB, Richtung1       
        cbi PortB, Richtung2
        warten10msMalX 20
        ;Kurve einleiten
        sbi PortB, Richtung1
        warten10msMalX 10
        ;und wieder gerade
        sbi PortB, Richtung2
        ret

change_dir:
    call step
    ;Richtung umkehren
        cbi PortB, Richtung1       
        cbi PortB, Richtung2
        ret

step:
    sbis PORTA,CLK            ;prüfe, ob CLK gesetzt ist
    rjmp aus                ;dann überspringe diese Zeile
    cbi PORTA,CLK            ;und schalte CLK ein
        warten10msMalX 10
    ret       
aus:
    sbi PORTA,CLK              ;CLK war gesetzt -> ausschalten
        warten10msMalX 10
    ret

---

danke schon im Vorraus,
mit freundlichen Grüßen, technik97

EDIT: Ich porgrammmiere in AVR Studio 4

EDIT2:
Ich habe was geändert
Der zweite Interrupt geht jetzt!
Der Code sieht so aus:

Zitat:

---

EXTERN1:

; Erst mal alles auf den Stack
PUSH tmp ;benutzte Register speichern
IN tmp, SREG ;Statusregister retten
PUSH tmp ;

;Hier kommt unsere eigentliche Bearbeitung
warten 1 ;1 ms warten, da Taster prellt
;Richtung umkehren
cbi PortB, Richtung1
cbi PortB, Richtung2
warten10msMalX 40
;Kurve einleiten
sbi PortB, Richtung1
warten10msMalX 20
;und wieder gerade
sbi PortB, Richtung2

in tmp, GIFR
ldi tmp, (1<<INTF1) ;durch Prellen des Tasters wurde schon
out GIFR, tmp ;ein neuer Interrrupt getriggert
;manuelles setzen des Flags verhindert
;eine erneute Ausführung der Routine
;nach dem Rücksprung

; Jetzt Register vom Stack wiederholen
POP tmp ;Statusregister wiederherstellen
OUT SREG, tmp ;
POP tmp ;benutzte Register wiederherstellen

RETI ;Rücksprung

---