' ---------------------------------------------
' program to position multiple servos
' servos connected to the four servo ports
' PS1 -> PS4 on the RC40
'
' D. Evans
' September 7, 2004
' ----------------------------------------------

#include "rc40.inc"

' constant for loop
Forever	con		1

' misc variables
I			var		byte
pulseTime	var		word

' misc constants
ServoLeft	con		500

' Array to hold the positions for the servos
ServoPos	var		byte(4)
' Array to hold ports for servos
ServoPort	var		byte(4)

' variable used to accumulate loop iterations
loopCounter	var		word

' variable to hold animatronic state
machineState var	byte

' constants to define machine states
ST_START	con		0
ST_LEFT		con		1
ST_RIGHT	con		2
ST_UP		CON		3
ST_DOWN		con		4

' ---------
' main loop
' ---------
Main:

	' initialize everything
	gosub Init
	
	' never-ending loop
	while (Forever)
	
		' periodically change to the next machine state
		' if this loop executes every 30 ms, this would 
		' be about every 3.3 seconds
		if (loopCounter = 100) then
			gosub servoState
			loopCounter = 0
		endif
	
		' increment loop counter
		loopCounter = loopCounter + 1
		
		' kick the servos
		gosub kickServos
		
		' pause a little
		pause 20
		
		' if you are doing other things, you might 
		' place additional code here and reduce or 
		' eliminate the pause statement
		
	wend
	
END


' ------------------------
' initialize all variables
' ------------------------
Init:

	' initialize loop counter
	loopCounter = 0
	
	' initialize machine state
	machineState = ST_START

	' initialize servo positions to center
	for i = 0 to 3
		servoPos(1) = 0
	next

	' set servo ports
	servoPort(0) = 1 	' PS1
	servoPort(1) = 2	' PS2
	servoPort(2) = 8	' PS3
	servoPort(3) = 9	' PS4

RETURN

' -----------------------------------------
' state machine for servo positioning
' this routine positions the servos
' based on the current machine state
' all of these servo values are nonsense,
' you'll have to figure out the real stuff
' based on your application
' -----------------------------------------
servoState:

	debug "machineState=", dec machineState, CR

	branch machineState, [State0, State1, State2, State3, State4]
	
	' safety feature!
	return

State0:

	' set the servo positions for this state
	servoPos(0) = 1
	servopos(1) = 4
	servoPos(2) = 7
	servoPos(3) = 5
	
	' set next state
	machineState = ST_LEFT
	
	return
	
State1:
	' set the servo positions for this state
	servoPos(0) = 2
	servopos(1) = 3
	servoPos(2) = 8
	servoPos(3) = 1
	
	' set next state
	machineState = ST_RIGHT

	return
	
State2:

	' set the servo positions for this state
	servoPos(0) = 0
	servopos(1) = 5
	servoPos(2) = 2
	servoPos(3) = 9
	
	' set next state
	machineState = ST_UP

	return
	
State3:

	' set the servo positions for this state
	servoPos(0) = 3
	servopos(1) = 8
	servoPos(2) = 1
	servoPos(3) = 4
	
	' set next state 
	machineState = ST_DOWN

	return	
	
State4:

	' set the servo positions for this state
	servoPos(0) = 1
	servopos(1) = 4
	servoPos(2) = 3
	servoPos(3) = 2
	
	' set next state (back to start state)
	machineState = ST_START
	
' -------------------------------------------
' routine to periodically kick all servos
' we'll assume that each servo has a range
' of positions 
'  
'	0	far left
'	5	center
'	10	far right
'
' My servos use a 1500us pulse
' center value, with +-500us for the complete
' range of motion. Your mileage may vary...
'
' our timing for PULSOUT is 2us per tick, so
' the range is 500-1000 ticks for the full range
' -------------------------------------------
KickServos:

	' kick all four servos, using the
	' current positions stored in servopos(x)
	for i = 0 to 3
		pulseTime = servoPOS(i) * 50 + servoLeft
		pulsout servoPort(i), pulseTime
	next

Return