'Slowspeed
'Manual control of five servomotors using 5 SPDT switches
'Microcontroller PIC 16F873

adcon1 = 7 'Set port a to digital I/O

'Declare variables

b0 var byte 	'Use b0 as hold pulse width variable for servo 1
b1 var byte 	'Use b1 to hold pulse width variable for servo 2
b2 var byte 	'Use b2 to hold pulse width variable for servo 3
b3 var byte 	'Use b3 to hold pulse width variable for servo 4
b4 var byte 	'Use b4 to hold pulse width variable for servo 5
b6 var byte 	'Variable for pause routine
b7 var word 	'Variable for pause routine
s1 var byte 	'Unassigned delay variable
s2 var byte 	'Assigned delay variable

'Initialize servomotor variables

b0 = 150 	'Start up position servo 1
b1 = 150 	'Start up position servo 2
b2 = 150 	'Start up position servo 3
b3 = 150 	'Start up position servo 4
b4 = 150 	'Start up position servo 5
s2 = 4 		'Delay variable

start:

'Output servomotor position

portb = 0 		'Prevents potential signal inversion on reset
pulsout portb.7, b0 	'Send current servo 1 position out
pulsout portb.6, b1 	'Send current servo 2 position out
pulsout portb.5, b2 	'Send current servo 3 position out
pulsout portb.4, b3 	'Send current servo 4 position out
pulsout portb.3, b4 	'Send current servo 5 position out

'Routine to adjust pause value (nom 18) to generate approx 50 Hz update

b7 = b0 + b1 + b2 + b3 + b4
b6 = b7/100
b7 = 15 - b6
pause b7

'Check for switch closures

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?
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 max0 		'Maximum 2.54 milliseconds
goto start
right1:
s1 = s1 + 1
if s1 = s2 then
b0 = b0 - 1			'Decrease the pulse width
s1 = 0
endif
if b0 < 75 then min0 		'Minimum .75 millisecond
goto start
max0:
b0 = 254 			'Cap max b1 at 2.54 milliseconds
goto start
min0:
b0 = 75 			'Cap min b1 at .75 millisecond
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 max1 		'Maximum 2.54 milliseconds
goto start
right2:
s1 = s1 + 1
if s1 = s2 then
b1 = b1 - 1			'Decrease the pulse width
s1 = 0
endif
if b1 < 75 then min1 		'Minimum .75 millisecond
goto start
max1:
b1 = 254			'Cap max b1 at 2.54 milliseconds
goto start
min1:
b1 = 75				'Cap min b1 at .75 millisecond
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 max2		'Maximum 2.54 milliseconds
goto start
right3:
s1 = s1 + 1
if s1 = s2 then
b2 = b2-1			'Decrease the pulse width
s1 = 0
endif
if b2 < 75 then min2		'Minimum .75 millisecond
goto start
max2:
b2 = 254			'Cap max b2 at 2.54 milliseconds			
goto start
min2:
b2 = 75				'Cap min b2 at .75 millisecond
goto start

'Routines for servomotor 4
left4:
s1 = s1 + 1			'Increase the pulse width
if s1 = s2 then
b3 = b3 + 1
s1 = 0
endif
if b3 > 254 then max3		'Maximum 2.54 milliseconds
goto start
right4:
s1 = s1 + 1
if s1 = s2 then
b3 = b3-1			'Decrease the pulse width
s1 = 0
endif
if b3 < 75 then min3		'Minimum .75 millisecond
goto start
max3:
b3 = 254 			'Cap max b3 at 2.54 milliseconds
goto start
min3:
b3 = 75 			'Cap min b3 at .75 millisecond
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 max4 		'Maximum 2.54 milliseconds
goto start
right5:
s1 = s1 + 1
if s1 = s2 then
b4 = b4-1			'Decrease the pulse width
s1 = 0
endif
if b4 < 75 then min4 		'Minimum .75 millisecond
goto start
max4:			
b4 = 254 			'Cap max b4 at 2.54 milliseconds
goto start
min4:
b4 = 75 			'Cap min b4 at .75 millisecond
goto start
end