' timers2.bas - Controlling servo motors
'
' This file is written for the 68HC12 by Kevin Ross (kevinro@nwlink.com)
' An article in the Seattle Robotics Society Encoder accompanies this
' file http://www.seattlerobotics.org/encoder/nov97/68hc12.html
'
' It is written in SBASIC.
' To compile this file, you need to invoke the following commands:
'
' sbasic timers.bas /cf000 /v0000 /m6812 > timers.asm
' as12 first.asm
'
include "regs12.lib"	' PORT definitions and constants

declare c

const CenterPosition = 2550
const HighPosition = CenterPosition + 1900
const LowPosition = CenterPosition - 1900

'
' Program execution starts at the Main: label, as shown here.
'
Main:

'
' This program isn't going to use the COP. It must be turned of
' right away, otherwise it might reset the chip.
'
	pokeb copctl,0
'
' Next, initialize the serial port and print a message saying we started.
'
	' Enable serial port
	poke  sc0bdh, 52	' Set for 9600 baud
	pokeb	sc0cr2,$0c	' Enable transceiver

	print "timers2.bas has started"

'
' Setup channels 0,1, and 7 to be TOC channels.
'
	pokeb	tios,%10000011

'
' Set channels 0 and 1 to clear their output pins when the compare happens
'
	pokeb	tctl2, %00001010

'
' Set the prescalar to roll over on 32ms periods
'
	pokeb	tmsk2,%00110010
'
' Setup TOC7 to handle the start of the pulses by setting the value to 1
' when the TCNT is zero
'
	pokeb	oc7m,%00000011
	pokeb	oc7d,%00000011
	poke	tc7,0

' Preset the positions of the servos to center

	poke  tc0, CenterPosition
	poke  tc1, CenterPosition
'
' Turn on the timer, disable it during background debug mode
'
	pokeb	tscr, %10100000

	do
	    do
    		c = inkey()
            loop until c <> 0
	    c = c AND $ff
	    select c
    		case '4'
			poke tc0, HighPosition
			print "Reverse: tc0 = " , CenterPosition - peek(tc0)	
		endcase
    		case 'r'
			poke tc0, peek(tc0) + 5
			if peek(tc0) > HighPosition
				poke tc0,HighPosition
			endif
			print "Reverse: tc0 = " , CenterPosition - peek(tc0)

                endcase
		case 'f'
			poke tc0, peek(tc0) - 5
			if peek(tc0) < LowPosition
				poke tc0,LowPosition
			endif
			print "Forward: tc0 = " , CenterPosition - peek(tc0)
                endcase
    		case 'v'
			poke tc0, LowPosition
			print "Forward: tc0 = " , CenterPosition - peek(tc0)	
		endcase
		case 'd'
			poke tc0, CenterPosition
			print "Center: tc0 = " , CenterPosition - peek(tc0)	
                endcase


		case '5'
			poke tc1, HighPosition
			print "Reverse: tc1 = " , CenterPosition - peek(tc1)
		endcase
		case 't'
			poke tc1, peek(tc1) + 5
			if peek(tc1) > HighPosition
				poke tc1,HighPosition
			endif

			print "Reverse: tc1 = " , CenterPosition - peek(tc1)
		endcase
		case 'g'
			poke tc1, peek(tc1) - 5
			if peek(tc1) < LowPosition
				poke tc1,LowPosition
			endif
			print "Forward: tc1 = " , CenterPosition - peek(tc1)
                endcase
		case 'b'
			poke tc1, LowPosition
			print "Forward: tc1 = " , CenterPosition - peek(tc1)
                endcase
		case 'h'
			poke tc1, CenterPosition
			print "Center: tc1 = " , CenterPosition - peek(tc1)
                endcase
		print "tc0 controls are 4,r,f,v,d"
		print "tc1 controls are 5,t,g,b,h"

            endselect
	
	loop