PICBasic Pro Program:

‘Rem John Iovine
‘Five servomotor controller
‘Rem 16F873 20 MHz microcontroller using 16 MHz xtal

‘Start up Servomotor Position (EEPROM values)

DEFINE OSC 16

EEPROM 5, [ 150, 150, 150, 150, 150]

adcon1 = 7		     ‘Set Port A to digital I/O

‘Declare Variables

B0 VAR BYTE		     ‘Variable for PW on servo 1
B1 VAR BYTE		     ‘Variable for PW on servo 2
B2 VAR BYTE		     ‘Variable for PW on servo 3
B3 VAR BYTE		     ‘Variable for PW on servo 4
B4 VAR BYTE		     ‘Variable for PW on servo 5
B7 VAR WORD
s1 VAR BYTE		     ‘delay variable 1 unassigned
s2 VAR BYTE		     ‘delay variable 2 assigned

‘Initialize Servomotor Variables

Read 5, B0		     ‘start up position servo 1 
Read 6, B1		     ‘start up position servo 2
Read 7, B2 		     ‘start up position servo 3 
Read 8, B3		     ‘start up position servo 4 
Read 9, B4		     ‘start up position servo 5

s1 = 0

‘Adjust Speed control

IF PORTB.1 = 1 Then 
           s2 = 1		‘Go Fast
EndIF
IF PORTB.1 = 0 Then		‘Go Slow
           s2 = 4
EndIF
	
start:				

‘Output servomotor position 

     PORTB.7 = 0	     ‘Prevent potential signal inversion
     B7 = B0 * 4
     PulsOut PORTB.7, B7     ‘send current servo 5 position out
     PORTB.6 = 0	     ‘Prevent potential signal inversion
     B7 = B1 * 4
     PulsOut PORTB.6, B7     ‘send current servo 4 position out
     PORTB.5 = 0	     ‘Prevent potential signal inversion
     B7 = B2 * 4
     PulsOut PORTB.5, B7     ‘send current servo 3 position out
     PORTB.4 = 0	     ‘Prevent potential signal inversion
     B7 = B3 * 4
     PulsOut PORTB.4, B7     ‘send current servo 2 position out
     PORTB.3 = 0	     ‘Prevent potential signal inversion
     B7 = B4 * 4
     PulsOut PORTB.3, B7     ‘send current servo 1 position out

Pause 10	

‘ Check for switch closures

     IF PORTB.2 = 0 Then pcc	   ‘Is PC control enabled?
     IF PORTC.3 = 0 Then left5	   ‘is sw1 left active?
     IF PORTC.2 = 0 Then right5	   ‘is sw1 right active?
     IF PORTC.1 = 0 Then left4	   ‘is sw2 left active?
     IF PORTC.0 = 0 Then right4	   ‘is sw2 right active?
     IF PORTA.5 = 0 Then left3	   ‘is sw3 left active?
     IF PORTA.4 = 0 Then right3	   ‘is sw3 right active?
     IF PORTA.3 = 0 Then left2	   ‘is sw4 left active?
     IF PORTA.2 = 0 Then right2	   ‘is sw4 right active?
     IF PORTA.1 = 0 Then left1	   ‘is sw5 left active?
     IF PORTA.0 = 0 Then right1	   ‘is sw5 right active?
     IF PORTC.6 = 0 Then save	   ‘Is store button pressed?
	

GoTo start


‘Routines for Servomotor 1
left1:
     S1 = S1 + 1
     IF S1 = S2 Then
     B0 = B0 + 1		   ‘increase the pulse width
     S1 = 0
     EndIF
     IF B0 > 254 Then 
     B0 = 254
     EndIF              	   ‘maximum 2.54 millisecond
     GoTo start
right1:
     S1 = S1 + 1
     IF S1 = S2 Then
     B0 = B0 - 1		   ‘decrease the pulse width
     S1 = 0
     EndIF
     IF B0 < 75 Then 
     B0 = 75			   ‘minimum .75 millisecond
     EndIF
     GoTo start

‘Routines for Servomotor 2
left2:
     S1 = S1 + 1
     IF S1 = S2 Then
     B1 = B1 + 1		   ‘increase the pulse width
     S1 = 0
     EndIF
     IF B1 > 254 Then 
     B1 = 254       		   ‘maximum 2.54 millisecond
     EndIF
     GoTo start
right2:
     S1 = S1 + 1
     IF S1 = S2 Then
     B1 = B1 - 1		   ‘decrease the pulse width
     S1 = 0
     EndIF
     IF B1 < 75 Then 
     B1 = 75			   ‘minimum .75 millisecond
     EndIF
     GoTo start

‘Routines for Servomotor 3
left3:
     S1 = S1 + 1
     IF S1 = S2 Then
     B2 = B2 + 1		   ‘increase the pulse width
     S1 = 0
     EndIF
     IF B2 > 254 Then
     B2 = 254			   ‘maximum 2.54 millisecond
     EndIF
     GoTo start
right3:
     S1 = S1 + 1
     IF S1 = S2 Then
     B2 = B2 - 1		   ‘decrease the pulse width
     S1 = 0
     EndIF
     IF B2 < 75 Then 
     B2 = 75			   ‘minimum .75 millisecond
     EndIF
     GoTo start

‘Routines for Servomotor 4
left4:
     S1 = S1 + 1
     IF S1 = S2 Then
     B3 = B3 + 1		   ‘increase the pulse width
     S1 = 0
     EndIF
     IF B3 > 254 Then
     B3 = 254			   ‘maximum 2.54 millisecond
     EndIF
     GoTo start
right4:
     S1 = S1 + 1
     IF S1 = S2 Then
     B3 = B3 - 1		   ‘decrease the pulse width
     S1 = 0
     EndIF
     IF B3 < 75 Then
     B3 = 75			   ‘minimum .75 millisecond
     EndIF
     GoTo start

‘Routines for Servomotor 5
left5:
     S1 = S1 + 1
     IF S1 = S2 Then
     B4 = B4 + 1		   ‘increase the pulse width
     S1 = 0
     EndIF
     IF B4 > 254 Then 
     B4 = 254			   ‘maximum 2.54 millisecond
     EndIF
     GoTo start
right5:
     S1 = S1 + 1
     IF S1 = S2 Then
     B4 = B4 - 1		   ‘decrease the pulse width
     S1 = 0
     EndIF
     IF B4 < 75 Then
     B4 = 75			   ‘minimum .75 millisecond
     EndIF
     GoTo start



save:		    ‘Save current servomotor positions to EEPROM

Write 5,B0
Write 6,B1
Write 7,B2
Write 8,B3
Write 9,B4

GoTo start

pcc:		    ‘PC Control

‘Gather updated pulse width values from PC

SerIn PORTB.0,6,20,gout,[7],B4,B3,B2,B1,B0

gout:

GoTo start
end