ucl 2.0

# Copyright (c) 2006 by Wayne C. Gramlich
# All rights reserved.

# This module uses a PIC16F688:
library $pic16f688

# The system is running at 20MHz:
library clock20mhz
# A microsecond takes 4 cycles at 20MHz:
constant microsecond = 5

# The {$eusart} library defines some baud rate generator constants:
constant $eusart_clock = clock_rate
constant $eusart_factor = 4
library $eusart

# The library of bus access routines for use by a PIC16F688.
library rb2bus_pic16f688

# This module uses 20MHz ceramic resonator; hence mode FOSC=HS:
configure fosc=hs

# We want the TRM0 to interrupt every 2.5mS.  With FOSC=20MHz,
# we need to interrupt every 2.5 * 1000 * 5 = 12,500 instructions.
# With the TMR0 prescaler set to 64, we want TMR0 every 195 ticks
# (195 * 64 = 12480):
constant tmr0_instructions = 2500 * microsecond
constant tmr0_power = 6
constant tmr0_prescale = 1 << tmr0_power
constant tmr0_ps = tmr0_power - 1
constant tmr0_value = tmr0_instructions / tmr0_prescale + 1
constant tmr0_value_negative = (0 - tmr0_value) & 0xff

constant servos = 4
constant servos_mask = servos - 1
constant enables_mask = (1 << servos) - 1

# 1.5ms pulse width is supposed to be the "center" position
# for a servo.  We will initialize for center.
# 7500 * .20us = 1.5ms.   7500 = 0x1d4c
constant servo_center = 3500
constant servo_center_low = servo_center & 0xff
constant servo_center_high = servo_center >> 8

global enables byte
global servo_index byte
global servos_high[servos] array[byte]
global servos_low[servos] array[byte]

package pdip
    pin 1 = power_supply
    pin 2 = osc1
    pin 3 = osc2
    pin 4 = ra3_nc
    pin 5 = rx, name=rx, bit=rx_bit
    pin 6 = tx, name=tx, bit=tx_bit
    pin 7 = rc3_out, name=servo3, mask=servo3_mask, bit=servo_bit3
    pin 8 = rc2_out, name=servo2, mask=servo2_mask, bit=servo_bit2
    pin 9 = rc1_out, name=servo1, mask=servo1_mask, bit=servo_bit1
    pin 10 = rc0_out, name=servo0, mask=servo0_mask, bit=servo_bit0
    pin 11 = ra2_nc
    pin 12 = ra1_nc
    pin 13 = ra0_nc
    pin 14 = ground

constant all_mask = servo0_mask | servo1_mask | servo2_mask | servo3_mask

origin 0

procedure start
    arguments_none
    returns_nothing

    assemble
	goto main

origin 4

procedure interrupt
    arguments_none
    returns_nothing

    # We can arrive here from either a TMR0 or TMR1 interrupt.

    # For a TMR1 interrupt, we just clear all of the servo outputs
    # to zero.  Since it is always safe to do this for a TMR0 interrupt,
    # we just clear the servo outputs no matter what:

    # Clear servos:
    $rc := all_mask
    # Clear the TMR1 Interrupt Flag:
    $tmr1if := $false
    # Turn TMR1 off:
    $tmr1on := $false

    # Now deal with TMR0:
    if $t0if
	# Load the 1's complement of the select servo into 
	servo_index := (servo_index + 1) & servos_mask
	$tmr1h := 0xff ^ servos_high[servo_index]
	$tmr1l := 0xff ^ servos_low[servo_index]

	# Reload TRM0:
	$t0if := $false
	$tmr0 := tmr0_value_negative

	# Turn on the appropriate servo:
	switch servo_index
	  case 0
	    if enables@0
		servo0 := $false
	  case 1
	    if enables@1
		servo1 := $false
	  case 2
	    if enables@2
		servo2 := $false
	  case 3
	    if enables@3
		servo3 := $false
	# Fire up TRM1:
	$tmr1on := $true
    # All done:

procedure main
    arguments_none
    returns_nothing

    local command byte
    local id_index byte
    local index byte
    local servo byte
    local high byte
    local low byte

    # Initialize the RB2 module:
    call rb2bus_initialize(11)

    # Initialize everything else:
    
    # Initialize the pulses:
    index := 0
    while index < servos
    	servos_high[index] := servo_center_high
    	servos_low[index] := servo_center_low
	index := index + 1
    servo_index := 0

    # FIXME: Initialize to servos disabled!!!
    enables := 0x0f

    # Initialize TRM0:
    $option_reg := tmr0_ps
    # {$t0cs}=0 (use instrucion clock)
    # {$psa}=0 (prescaler for TMR0)
    # {$ps2,$ps1,$ps0}=101 (1/64)

    # Initialize TRM1:
    $t1con := 0
    # {$t1ginv} := x (no care; 0 = not inverted)
    # {$tmr1ge} := 0 (Timer 1 on)
    # {$t1ckps} := 00 (prescale = 1:1)
    # {$t10scen} := 0 (lp osc. off)
    # {$t1sync} := x (no care;)
    # {$tmr1cs} := 0 (Fosc/4)
    # {$tmr1on} := 0 (off for now; interrupt routine turns it on)

    # Turn on TRM0:
    $tmr1if := $false
    $tmr1ie := $true
    $tmr0 := tmr0_value_negative
    $t0if := $false
    $t0ie := $true
    $peie := $true
    $gie := $true

    id_index := 0

    loop_forever
	# Make sure that we have been selected:
	rb2bus_error := $true
	while rb2bus_error
	    call rb2bus_select_wait()
	    command := rb2bus_byte_get()

	switch command >> 6
	  case 0
	    # 00xx xxxx:
	    servo := command & servos_mask
	    switch (command >> 3) & 7
	      case 0
		if command@2
		    # 0000 01ss (Low Set):
		    low := rb2bus_byte_get()
		    if !rb2bus_error
			servos_low[servo] := low
		else
		    # 0000 00ss (High Low Set):
		    high := rb2bus_byte_get()
		    low := rb2bus_byte_get()
		    if !rb2bus_error
			servos_high[servo] := high
			servos_low[servo] := low
	      case 1
		# 0000 1xxx:
		if command@2
		    # 0001 11ss (Low Get)
		    call rb2bus_byte_put(servos_low[servo])
		else
		    # 0001 10ss (HighGet)
		    call rb2bus_byte_put(servos_high[servo])
	      case 2, 3
		# 0001 eeee (Enables Set):
		enables := command & enables_mask
	      case 4
		# 0010 0xxx:
		switch command & 7
		  case 0
		    # 0010 0000 (Enables Get)
		    call rb2bus_byte_put(enables)
		  case_maximum 7
	      case_maximum 7
	  case 3
	    switch (command >> 3) & 7
	      case 7
		switch command & 7
		  case 5
		    # 1111 1101 (Id_next):
		    if id_index < id.size
			call rb2bus_byte_put(id[id_index])
			id_index := id_index + 1
		  case 6
		    # 1111 1110 (Id_start):
		    id_index := 0
		  case 7
		    # 1111 1111 (Deselect):
		    call rb2bus_deselect()


procedure wait
    arguments_none
    returns_nothing

    # This procedure is repeatably called whenever the software
    # is waiting for a byte to arrive from the bus.

    do_nothing


string id = "\16,0,11,1,3,8\Servo4-A\7\Gramson"