TACHYON

	: jmservo8.fth  ." SERVO DRIVER TOOLKIT 120908.1551 "  ;

	{																
				120908.1551 - BBR - refined AUTORUN code, pulse len of 0 causes
				 			- that servo to be skipped, added/refined commentsss
				120907.1545 - BBR/PJ - added AUTORUN for background task
	  UPDATED:	120907.1130 - BBR - scrapped menu in favor of useful words
	 						- stabilized driver background performance
	  CREATED:  120905.1800 - Brian Riley - Underhill Center, VT, USA
	  BASEDON:  100624.0000 - Jon "JonnyMac" McPhalen 

	  CODE CURRENT WITH V1.1 KERNAL: 120905.0000
						V2.0 KERNAL: 120908.0200
						
      CHANGE CODE TO RUN ON VERSION V5r3	- 8/7/28 KDewey					
	}

	CLKFREQ #1_000_000 / := cntsperusec
	cntsperusec #20,000 * := cnts20msec	

				\ these numbers may have to be edited for different servos
	#400  := servomin			\ 0.4MS -  400 USECS
	#1500 := servoctr			\ 1.5MS - 1500 USECS
	#2400 := servomax			\ 2.4MS - 2400 USECS

	#8    := MAXSERV			\ leave this number alone. 8 servos at possible
	 								\ 2.4 msecs each is 19.2 msecs and all that will
									\ fit in 20 msec frame

				\ user supplies the number of the first pin in series and total
				\ number of pins. the code assumes pins in sequence
	
	#2	  := numserv			\ edit as needed
	#14	  := startpin			\ edit as needed
																																				
	TABLE servo MAXSERV 4 * ALLOT	\ build servo table


	\ *******************************************************************
	{
		the "#10 HIGH" and its companion LOW operates as a debug using an LED from P10
		to ground so  SERVO8 can signal that it is up and running. Those two lines of code
		can be removed once the code is running to your liking.
	
	}
	\ *******************************************************************

	: SERVO8	( -- )					\ background servo pulse driver
										\ setup pins for output, then set to 0
		  startpin FROM numserv FOR I LOW NEXT
		  
		 #10 HIGH		\ **** DEBUG **** LIGHT LED

		cnts20msec WAITX		\ sets up a new 20 ms frame with WAITCNT. in the absence
								\ of another WAITX command, any subseqent WAITCNT will
								\ also expire in 20 msecs
		
		BEGIN

			0 FROM numserv FOR
				servo I 2* 2* + @  	\ compute address and get pulse length
									\ TF Goodie - "2* 2*" is much faster than "4 *"
				DUP 0<> IF			\ save pulse len, if not 0 process pulse value
					servomin MAX	\ verify pulse is in range
					servomax MIN	\ 400 < pulse < 2400
					startpin I + 	\ calculate pin
					DUP HIGH       \ start pulse
					SWAP			\ bring pulse len forward
					#21 - us		\ 21 usec correction for pulse duration
					LOW 		\ end pulse
				THEN
			NEXT

			WAITCNT					\ done all servos, wait until 20 msec frame ends	
		AGAIN
		#10 LOW		\ **** DEBUG **** TURN OFF LED
		;

\ *********** Useful Words/Commands **********************************

: SERVOFILL ( value -- ) servo numserv ROT LONGFILL ;

 : SVCTR		( -- )	servoctr SERVOFILL ;
 : SVMAX		( -- )	servomax SERVOFILL ;
 : SVMIN		( -- )	servomin SERVOFILL ;

 : SVZRO		( -- )	servo MAXSERV 0 LONGFILL ;

: D1		( -- )	servo $20 DUMP ;		\ dump servo tables

: S1		( pulseval -- ) servo ! ;		\ set individual servos
: S2		( pulseval -- ) servo 4 + ! ;
: S3		( pulseval -- ) servo 8 + ! ;
: S4		( pulseval -- ) servo #12 + ! ;
: S5		( pulseval -- ) servo #16 + ! ;
: S6		( pulseval -- ) servo #20 + ! ;
: S7		( pulseval -- ) servo #24 + ! ;
: S8		( pulseval -- ) servo #28 + ! ;


\ ****** Setting SERVO8 as a background task in another COG *****************

					\ these will execute now
 \ ' SERVO8 >PFA := USERVO
					\ 'tick' commands cannot be included in a word definition,
					\ at 'install' time this links the actual location of SERVO8 to
					\ a := which can be included in a word definition
: SERVODRV
	' SERVO8  TASK? RUN	\ start SERVO8 in background COG 4
	;

{
					\ these are commented out and are options you may select
					
	SERVODRV		\ manually  type this on console
					\
					\			OR

	AUTORUN SERVODRV	\ set SERVODRV for AUTORUN
}
\ *******************************************************************
{	
					\ these also are commented out and are options you may select

	' SERVO8 >PFA 4 RUN
					\ straight forward "IMMEDIATE" command to run SERVO8 in another COG

	1 4 RUN			\ to terminate SERVO8 yet leave Tachyon VM IDLE in control and
					\ available for re-use, send a "1" to COG 4 where ATN? will 
					\ generate a FALSE and drop out of the BEGIN/UNTIL loop.

	4 STOP			\ the good news is, it will stop SERVO8 in COG 4 ... the bad news
					\ is, the Tachyon VM and COG 4 itself don't survive either,
					\ DO NOT USE THIS COMMAND
}
\ *******************************************************************
{
	COMMENTS on Servo Applications and Power
	
	I developed this code on a PropBOE in default configuration, that is
	jumpered for regulated +5v. Just because the BOE has 6 possible servo
	connections and just because the software can handle 8 servos doesn't
	mean that it will. On the 5v buss the BOE can handle 2 servos. Add a
	third servo you will experience the joy snd frustration of brownout 
	resets. If you need 3+ servos jumper them for power from Vin and 
	get a solid 6-7.5 v battery pack ... cheers ... BBR

}
\ *******************************************************************

		
	END