PIC bestuurde robotwagen Update

De code is ondertussen klaar.

De code in de robotauto:

    Device              16F628A
    Config WDT_OFF, PWRTE_Off, LVP_OFF, MCLRE_on, INTRC_OSC_NOCLKOUT
    ALL_DIGITAL         TRUE 
    PORTB_PULLUPS       = 1
'Declaraties    
    'Sensoren
    Symbol sensorR      = PORTA.6
    Symbol sensorL      = PORTA.7
    'Motoren
    Symbol motorvooruit = PORTA.2
    Symbol motorachteruit  = PORTA.3
    Symbol stuurLinks   = PORTA.1
    Symbol stuurRechts  = PORTA.0
    'Terugkoppeling stuur
    Symbol stuursensL   = PORTB.0
    Symbol stuursensM   = PORTB.1
    Symbol stuursensR   = PORTB.2
    'Leds
    Symbol led1         = PORTB.3
    Symbol led2         = PORTB.4
    Symbol led3         = PORTB.5
    Symbol led4         = PORTB.6
    Symbol led5         = PORTB.7
    

'Wachten voor dat er bogonnen wordt 
'(De sensoren hebben tijd nodig om op te starten)    
    DelayMS 1000
    
'Stuur initialiseren
    'Staat het stuur in het midden?
    If stuursensM = 0 Then
        GoTo klaarinit      'Direct naar klaarinit gaan
    EndIf
    'Staat het stuur links?
    If stuursensL = 0 Then 
        GoTo links         'Naar links gaan
    EndIf
    'Staat het stuur rechts?
    If stuursensR = 0 Then
        GoTo rechts        'Naar rechts gaan
    EndIf
    'In geen stand (geen terugkoppeling)
    'Eerst volledig naar rechts draaien
    High    stuurRechts
    While   stuursensR = 1 
        ;Lusje totdat de stuursensor rechts laag wordt
    Wend
    Low     stuurRechts
    DelayMS 500 
    'Naar gedeelte gaan voor als het stuur naar rechst staat    
    GoTo    rechts      
    
rechts:
    'Naar links sturen    
    High    stuurLinks
    While   stuursensM = 1 
        ;Lusje totdat de stuursensor midden laag wordt
    Wend
    Low     stuurLinks
    DelayMS 500
    GoTo    klaarinit
    
links:
    ;Naar rechts sturen
    High    stuurLinks
    While   stuursensM = 1
        ;Lusje totdat de stuursensor midden laag wordt
    Wend
    Low     stuurLinks
    DelayMS 500

klaarinit:        
    High motorvooruit
    Low  motorachteruit
    High led3
    GoTo main
    

'Hoofdprogramma
main:   
    ;links opstakel
    If sensorL = 0 Then
        DelayMS 10
        High    led1
        GoSub   linksDect
    EndIf
    Low led1 
    ;rechts opstakel
    If sensorR = 0 Then
        DelayMS 10
        High    led5
        GoSub   rechtsDect 
    EndIf
    Low led5  

GoTo main
            
rechtsDect:  
    High led4  
    ;stukje achteruit
    Low     motorvooruit
    DelayMS 10
    High    motorachteruit
    DelayMS 1000
    Low  motorachteruit
    Low     led4
    ;Draaien
    High    stuurLinks    
    While stuursensL = 1
        ;Lusje totdat de stuursensor links laag wordt
    Wend
    Low     stuurLinks   
    ;Stukje vooruit
    DelayMS 10
    High    motorvooruit
    DelayMS 1000
    Low     motorvooruit
    ;Stuur  terug recht zetten
    High    stuurRechts
    While   stuursensM = 1
        ;Lusje totdat de stuursensor midden laag wordt
    Wend
    Low     stuurRechts
    DelayMS 10
    High    motorvooruit

Return

linksDect:
    High  led2
    ;stukje achteruit
    Low     motorvooruit
    DelayMS 10
    High    motorachteruit
    DelayMS 1000
    Low  motorachteruit
    Low     led2
    ;Draaien
    High    stuurRechts    
    While stuursensR = 1
        ;Lusje totdat de stuursensor links laag wordt
    Wend
    Low     stuurRechts
    ;Stukje vooruit
    DelayMS 10
    High    motorvooruit
    DelayMS 1000
    Low     motorvooruit
    ;Stuur  terug recht zetten
    High    stuurLinks
    While   stuursensM = 1
        ;Lusje totdat de stuursensor midden laag wordt
    Wend
    Low     stuurLinks
    DelayMS 10
    High    motorvooruit

Return

;bij fout
GoTo main
End

 

De code in de tank:

Device 16F628A
Config WDT_OFF,PWRTE_OFF,LVP_OFF,MCLRE_ON,INTRC_OSC_NOCLKOUT, BODEN_OFF

ALL_DIGITAL true 

Symbol      motorLinksV = PORTA.2
Symbol      motorLinksA = PORTA.3
Symbol      motorRechtsV = PORTA.6

Symbol      motorRechtsA = PORTA.7

Symbol      sensorL = PORTA.0
Symbol      sensorR = PORTA.1

Symbol      led1 = PORTB.0
Symbol      led2 = PORTB.1

Symbol      led3 = PORTB.2
Symbol      led4 = PORTB.3
Symbol      led5 = PORTB.4 

Clear 

High motorLinksV
DelayMS 10
High motorRechtsV

Low motorRechtsA
Low motorLinksA

main:
If sensorL = 0 Then
            'hoog maken van led1
            High led1
            'stoppen van motoren vooruit
            Low motorLinksV
            Low motorRechtsV
            '100ms wachten
            DelayMS 100
            'achteruit rijden
            High motorLinksA
            DelayMS 10
            High motorRechtsA
            'hoogmaken van led3 en Led4
            High led3
            High led4
            '1 s wachten
            DelayMS 1000
            'stoppen met rijden 
            Low motorLinksA
            Low motorRechtsA
            Low motorLinksV
            Low motorRechtsV
            'laag maken van led3 en led4
            Low led3
            Low led4
            '100 ms wachten
            DelayMS 100
            'led2 hoogmaken
            High led2
            'draaien van tank
            High motorLinksV
            DelayMS 10
            High motorRechtsA
            '500ms draaien
            DelayMS 500
            'laagmaken led2            
            Low led2
            'stoppen met draaien
            Low motorLinksV
            Low motorRechtsA
            '10 ms stoppen
            DelayMS 10
            'terug vooruit rijden
            High motorLinksV
            DelayMS 10
            High motorRechtsV


Else
            'als sensor links niets detecteert gaat led1 niet branden
            Low led1
EndIf
If sensorR = 0 Then
            'hoog maken van led5
            High led5
            'stoppen van motoren vooruit
            Low motorLinksV
            Low motorRechtsV
            '100ms wachten
            DelayMS 100
            'achteruit rijden
            High motorLinksA
            DelayMS 10
            High motorRechtsA
            'hoogmaken van led3 en Led4
            High led3
            High led4
            '1 s wachten
            DelayMS 1000
            'stoppen met rijden 
            Low motorLinksA
            Low motorRechtsA
            Low motorLinksV
            Low motorRechtsV
            'laag maken van led3 en led4
            Low led3
            Low led4
            '100 ms wachten
            DelayMS 100
            'led2 hoogmaken
            High led2
            'draaien van tank
            High motorLinksA
            DelayMS 10
            High motorRechtsV
            '500ms draaien
            DelayMS 500
            'laagmaken led2            
            Low led2
            'stoppen met draaien
            Low motorLinksA
            Low motorRechtsV
            '10 ms stoppen
            DelayMS 10
            'terug vooruit rijden
            High motorLinksV
            DelayMS 10
            High motorRechtsV


Else
            'als sensor rechts niets detecteert gaat led5 niet branden
            Low led5
EndIf

GoTo main
End

Geef een reactie

Jouw e-mailadres wordt niet gepubliceerd.

Deze website gebruikt Akismet om spam te verminderen. Bekijk hoe je reactie-gegevens worden verwerkt.