ucl 2.0 # Copyright (c) 2006-2007 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 5 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 rb2_constants 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 = 1500 * microsecond 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 all servo pulses: $rc := 0 # 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 := $true case 1 if enables@1 servo1 := $true case 2 if enables@2 servo2 := $true case 3 if enables@3 servo3 := $true # Fire up TRM1: $tmr1on := $true # All done: # The tables below provide pulse widths from .75uS to 2.25uS: string quick_highs = "\14,15,16,17,18,19,20,21,21,22,23,24,25,26,27,28,29,30,31,32,32,33,34,35,36,37,38,39,40,41,42,43\" string quick_lows = "\166,144,123,101,80,58,36,15,249,227,206,184,163,141,119,98,76,54,33,11,246,224,202,181,159,137,116,94,73,51,29,8\" 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 local position byte # Initialize the RB2 module: call rb2bus_initialize(rb2_servo4_address) # 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 enables := 0 # 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() # Decode {command}: servo := command & servos_mask switch command >> 6 case 0,1 # 0ppp ppss (Quick Set): position := command >> 2 servos_high[servo] := quick_highs[position] servos_low[servo] := quick_lows[position] case 2 # 01xx xxxx: switch (command >> 3) & 7 case 0 # 1000 0xxx: high := rb2bus_byte_get() if command@2 # 1000 01ss (Short High Low Set): low := high << 5 high := high >> 3 else # 1000 00ss (High Low Set): low := rb2bus_byte_get() if !rb2bus_error servos_high[servo] := high servos_low[servo] := low case 1 # 1000 1xxx: if command@2 # 1000 11ss (High Set) high := rb2bus_byte_get() if !rb2bus_error servos_high[servo] := high else # 1000 10ss (Low Set) low := rb2bus_byte_get() if !rb2bus_error servos_low[servo] := low case 2, 3 # 1001 eeee (Enables Set): enables := command & enables_mask case 4 # 1010 0xxx: if command@2 # 1010 01ss (Low Get) call rb2bus_byte_put(servos_low[servo]) else # 1010 00ss (High Get) call rb2bus_byte_put(servos_high[servo]) case 5 # 1010 1xxx: switch command & 7 case 0 # 1010 1000 (Enables Get) call rb2bus_byte_put(enables) case_maximum 7 case_maximum 7 case 3 # 11xx xxxx: switch (command >> 3) & 7 case 7 # 1111 1xxx: call rb2bus_command(command) 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,2,3,8\Servo4-B\7\Gramson"