Servo motor kontrol uygulamasi

Yukarida ayni anda farkli servolarin kontrolu uygulamasi verilmistir. ornek kodlar su sekilde

yukaridaki devreni kodlaridir

'  Servo program using On Interrupt
'  Uses TMR0 and prescaler.  Watchdog Timer should be
'  set to off at program time and Nap and Sleep should not be used.
'  Buttons may be used to set hours and minutes

' Define LOADER_USED to allow use of the boot loader.
' This will not affect normal program operation.
Define    LOADER_USED    1

Define  LCD_DREG        PORTD   ' Define LCD connections
Define  LCD_DBIT        4
Define  LCD_RSREG       PORTE
Define  LCD_RSBIT       0
Define  LCD_EREG        PORTE
Define  LCD_EBIT        1

Servo1Pos VAR    Word    ' Servo 1 position
Servo2Pos VAR    Word    ' Servo 2 position
ServoSelect VAR    Bit    ' Servo selection

Servo1    VAR    PORTC.1    ' Alias first servo pin
Servo2    VAR    PORTC.0    ' Alias second servo pin


    ServoSelect = 0    ' Set to first servo
    Low Servo1    ' Set up servos
    Low Servo2
    Servo1Pos = 489    ' Center servos
    Servo2Pos = 489


        ADCON1 = 7      ' PORTA and PORTE to digital
        Low PORTE.2     ' LCD R/W low = write
        Pause 100       ' Wait for LCD to startup
        Lcdout $fe, 1    ' Clear screen
        lcdout $fe,1,"Cift Servo Kontrol"
        pause 1000
        OPTION_REG = $7f        ' Enable PORTB pullups

' Set TMR0 to interrupt every 16.384 milliseconds
        OPTION_REG = $55        ' Set TMR0 configuration and enable PORTB pullups
        INTCON = $a0            ' Enable TMR0 interrupts
        ON Interrupt GoTo timerint


' Main program loop
mainloop:

        PORTB = 0       ' PORTB lines low to read buttons
        TRISB = $fe     ' Enable first button row

        ' Check any button pressed to move servo
        IF PORTB.4 = 0 Then
                GoSub left1
        Endif
        IF PORTB.5 = 0 Then
                GoSub right1
        Endif
        IF PORTB.6 = 0 Then
                GoSub left2
        Endif
        IF PORTB.7 = 0 Then
                GoSub right2
        Endif
        if portb.3=0 then
                gosub home
        endif

        Lcdout $fe, $80, "Position1 = ", #Servo1Pos, "   "
        Lcdout $fe, $c0, "Position2 = ", #Servo2Pos, "   "

        GoTo mainloop   ' Do it all forever

home:
        servo1pos=489
        servo2pos=489

        return
' Move servo 1 left
left1:    IF Servo1Pos < 1000 Then
                Servo1Pos = Servo1Pos + 1
        Endif
    Pause 1    ' Debounce default 10ms
        Return

' Move servo 1 right
right1:    IF Servo1Pos != 0 Then
                Servo1Pos = Servo1Pos - 1
        Endif
    Pause 1    ' Debounce default 10ms
        Return

' Move servo 2 left
left2:    IF Servo2Pos < 1000 Then
                Servo2Pos = Servo2Pos + 1
        Endif
    Pause 10    ' Debounce   default 10ms
        Return

' Move servo 2 right
right2:    IF Servo2Pos != 0 Then
                Servo2Pos = Servo2Pos - 1
        Endif
    Pause 10    ' Debounce  default 10ms
        Return


' Interrupt routine to handle timer
        disable         ' Disable interrupts during interrupt handler
timerint: IF ServoSelect Then tiservo2

    Servo1 = 1    ' Do first servo
    PauseUs 1000 + Servo1Pos
    Servo1 = 0
    GoTo tiexit

tiservo2: Servo2 = 1    ' Do second servo
    PauseUs 1000 + Servo2Pos
    Servo2 = 0

tiexit:    ServoSelect = ServoSelect + 1    ' Point to next servo
    INTCON.2 = 0    ' Reset timer interrupt flag
        Resume

        End