' Name        : SERVOSX.pbp
' Compiler    : PICBASIC PRO Compiler 2.6
' Assembler   : PM or MPASM
' Target PIC  : 40-pin 16F877, 877A or similar
' Hardware    : LAB-X1 Experimenter Board
' Oscillator  : 4MHz external crystal
' Keywords    : ON INTERRUPT, TIMER0
' Description : PICBASIC PRO program for servo control 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 on LAB-X1 keypad are
' used to move servos.
'

' 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 = 500  ' Center servos
   Servo2Pos = 500

   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

   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 (RB0=output=LOW)

   ' Check for button press to move servo
   IF PORTB.4 = 0 Then  ' SW1 on LAB-X1 keypad
       GoSub left1
   Endif
   IF PORTB.5 = 0 Then  ' SW2 on LAB-X1 keypad
      GoSub right1
   Endif
   IF PORTB.6 = 0 Then  ' SW3 on LAB-X1 keypad
      GoSub left2
   Endif
   IF PORTB.7 = 0 Then  ' SW4 on LAB-X1 keypad
      GoSub right2
   Endif

   Lcdout $fe, $80, "Position1 = ", #Servo1Pos, "   "
   Lcdout $fe, $c0, "Position2 = ", #Servo2Pos, "   "
   GoTo mainloop   ' Do it all forever

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

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

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

' Move servo 2 right
right2:
   IF Servo2Pos != 0 Then
      Servo2Pos = Servo2Pos - 1
   Endif
   Pause 10	' Debounce
   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