ucl 1.0 # Copyright (c) 2004-2005 by Wayne C. Gramlich # All rights reserved. library $robobricks_pic16f688 package pdip pin 1 = power_supply pin 2 = ra5_nc pin 3 = ra4_in, name = debug pin 4 = ra3_in, name = cal_mode pin 5 = rx pin 6 = tx pin 7 = rc3_out, name = servo3, bit = servo3_bit, mask = servo3_mask pin 8 = an6, name = sense3, bit = sense3_channel pin 9 = rc1_out, name = servo2, bit = servo2_bit, mask = servo2_mask pin 10 = an4, name = sense2, bit = sense2_channel pin 11 = ra2_out, name = servo1, bit = servo1_bit, mask = servo1_mask pin 12 = an1, name = sense0, bit = sense0_channel pin 13 = ra0_out, name = servo0, bit = servo0_bit, mask = servo0_mask pin 14 = ground constant sense1_channel = 0 #configure cp=on, wdte=on # Define the pin bindings: constant initial_minimum = 1000 * microsecond constant initial_maximum = 2000 * microsecond constant initial_scale = (initial_maximum - initial_minimum) >> 4 constant initial_high = initial_minimum >> 8 constant initial_low = initial_minimum & 0xff global analogs[4] array[byte] # The next {eedata_size} bytes are saved and restored from EEData: constant eedata_size = 13 global base_lows[4] array[byte] global base_highs[4] array[byte] global scales[4] array[byte] global address byte global fines[4] array[byte] global positions[4] array[byte] global interrupts byte global enables byte global command_previous byte global command_last byte global sent_previous byte global sent_last byte global final_lows[4] array[byte] global final_highs[4] array[byte] global interrupt_pending bit constant servo_on = 0 constant servo_off = 1 origin 0 procedure xmain arguments_none returns_nothing assemble goto main origin 4 procedure interrupt arguments_none returns_nothing interrupts := interrupts + 1 servo0 := servo_off servo1 := servo_off servo2 := servo_off servo3 := servo_off $tmr1if := 0 procedure us100 argument count byte returns_nothing # This procedure will delay by {count} * 100uS. loop_exactly count delay 100 * microsecond do_nothing procedure main arguments_none returns_nothing local command byte local glitch byte local id_index byte local temporary byte local servo byte local position byte # Switch over to 8MHz: $osccon := 0x71 # Warm up the UART: $txsta := 0x24 $rcsta := 0x90 $baudctl := 0x08 $spbrg := $eusart_2400_low $spbrgh := $eusart_2400_high # These registers are in data bank 1: $psa := 0 #loop_forever # servo0 := servo_on # servo1 := servo_on # servo2 := servo_on # servo3 := servo_on # # call us100(15) # # servo0 := servo_off # servo1 := servo_off # servo2 := servo_off # servo3 := servo_off # # call us100(190) # These registers are in data bank 0: $t1con := 0x00 interrupts := 0 # Set A/D clock to Fosc/32 (Tad= 1.6uS): $adcon1 := 0x20 #$adcs0 := 1 #$eeadr := 0 #loop_exactly 13 # $rd := 1 # base_lows[$eeadr] := $eedata # $eeadr := $eeadr + 1 #if base_lows[0] = 0xff servo := 0 loop_exactly 4 base_lows[servo] := initial_low base_highs[servo] := initial_high scales[servo] := initial_scale servo := servo + 1 address := 0xff servo := 0 loop_exactly 4 positions[servo] := 0 fines[servo] := 0 servo := servo + 1 # For now: enables := 0xf #enables := 0 # Let's initialize the servo's to off: servo0 := servo_off servo1 := servo_off servo2 := servo_off servo3 := servo_off # According to the specification sheet, TMR1L, TRM1H, and # TMR1IF must be clear before enabling TMR1IE. Since they # do not say why, it is unclear what problem that this addresses. $tmr1l := 0 $tmr1h := 0 $tmr1if := 0 $tmr1ie := 1 $peie := 1 $gie := 1 #$tmr1on := 1 #$tmr1ie := 0 #$peie := 0 #$gie := 0 $tmr1on := 0 glitch := 0 id_index := 0 loop_forever command := byte_get() servo := command & 3 switch command >> 6 case 0 # 00xx xxxx switch (command >> 3) & 7 case 0 # 0000 0xxx: temporary := byte_get() if command@2 # 0000 01ss (Write Base High): base_highs[servo] := temporary else # 0000 00ss (Write Base Low): base_lows[servo] := temporary case 1 # 0000 1xxx: temporary := byte_get() if command@2 # 0000 11ss (Write Scale): scales[servo] := temporary else # 0000 10ss (Write Position) positions[servo] := temporary case 2 # 0001 0xxx: if command@2 # 0001 01ss (Read Base High): temporary := base_highs[servo] else # 0001 00ss (Read Base Low): temporary := base_lows[servo] call byte_put(temporary) case 3 # 0001 1xxx: if command@2 # 0001 11ss (Read Scale): temporary := scales[servo] else # 0001 10ss (Read Position): temporary := positions[servo] call byte_put(temporary) case 4, 5, 6, 7 # 001x xxxx: do_nothing case 1 # 01hh hhhh (Set High): position := positions[servo] position := position & 0xf | (command << 2) & 0xf0 positions[servo] := position case 2 # 10ll llll (Set Low): position := positions[servo] position := position & 0xf0 | (command >> 2) & 0xf positions[servo] := position case 3 # 11xx xxxx: switch (command >> 3) & 7 case 0 # 1100 0ess (Set Enable and Position): positions[servo] := byte_get() temporary := mask_get(servo) if command@2 enables := enables | temporary else enables := enables & (temporary ^ 0xf) case 1 # 1100 1ess (Set Enable Flag Only): temporary := mask_get(servo) if command@2 enables := enables | temporary else enables := enables & (temporary ^ 0xf) case 2 # 1101 0xxx: if command@2 # 1101 01ss (Read Enable): temporary := 0 if enables & mask_get(servo) != 0 temporary@0 := 1 call byte_put(temporary) else # 1101 00ss (Read position): call byte_put(positions[servo]) case 3 # 1101 1xxx: if command@2 # 1101 11ss (Read Current Draw): call byte_put(analogs[servo]) else # 1101 10xx: if command@1 # 1101 101x: if command@0 # 1101 1011 (Set Address): address := byte_get() else # 1101 1010 (Save settings): # Need magic OK byte first: if byte_get() = 0xa5 #FIXME: Compiler problem kludge!!! temporary := 0 $eeadr := 0 $gie := 0 loop_exactly 13 $eedata := base_lows[$eeadr] $wren := 1 $eecon2 := 0x55 $eecon2 := 0xaa $wr := 1 while $wr do_nothing $eeadr := $eeadr + 1 $gie := 1 $wren := 0 call byte_put(0xa5) else call byte_put(0x11) else # 1101 100x: if command@0 # 1101 1001 (Set Enables): enables := byte_get() & 0xf else # 1101 1001 (Read Enables): call byte_put(enables) case 4 # 1110 0xxx: switch command & 7 case_maximum 7 case 0 call byte_put(interrupts) case 1 call byte_put(positions[0]) case 2 call byte_put(final_lows[0]) case 3 call byte_put(final_highs[0]) case 4 call byte_put(base_lows[0]) case 5 call byte_put(base_highs[0]) case 6 call byte_put(scales[0]) case 7 call byte_put(fines[0]) case 5, 6 # 1110 0xxx, 1110 1xxx, 1111 0xxx: do_nothing case 7 # 1111 1xxx switch command & 7 case 0 $osctune := $osctune - $osccal_lsb case 1 $osctune := $osctune + $osccal_lsb case 2 call byte_put($osctune) case 3 call byte_put(0) case 4 temporary := 0 if id_index < id.size temporary := id[id_index] id_index := id_index + 1 call byte_put(temporary) case 5 id_index := 0 case 6 call byte_put(glitch) glitch := 0 case 7 glitch := glitch + 1 #enables := 0xf command := byte_get() servo := command & 3 position := byte_get() temporary := address if address = 0xff temporary := (analogs[3] >> 2) & 0x30 | (analogs[2] >> 4) & 0xc if (command ^ temporary) & 0xfc = 0 positions[servo] := position enables := enables | mask_get(servo) procedure delay arguments_none returns_nothing exact_delay 138 * microsecond # This procedure delays 1/3 of a bit. local counter byte local position_high byte local position_low byte local servo byte local servo_low byte local servo_high byte local scale byte local temporary byte watch_dog_reset #debug_out := 1 counter := (counter + 1) & 0x7f servo := (counter >> 5) & 3 switch counter & 0x1f case_maximum 31 case 0 # Set up for multiply: position_low := positions[servo] servo_low := base_lows[servo] servo_high := base_highs[servo] scale := scales[servo] position_high := position_low >> 4 position_low := position_low << 4 case 1, 2, 3, 4, 5, 6, 7, 8 # Do one multiply cycle: position_low := position_low >> 1 if position_high@0 position_low@7 := 1 position_high := position_high >> 1 if scale@7 servo_high := servo_high + position_high servo_low := servo_low + position_low if $c servo_high := servo_high + 1 scale := scale << 1 case 9 # Do final wrap-up on multiply: servo_low := servo_low + fines[servo] if $c servo_high := servo_high + 1 # Multiply by 2: servo_high := servo_high << 1 if servo_low@7 servo_high@0 := 1 servo_low := servo_low << 1 final_lows[servo] := servo_low final_highs[servo] := servo_high #servo_high := 6 #servo_low := 0 $tmr1l := 0 $tmr1h := servo_high ^ 0xff $tmr1l := servo_low ^ 0xff if servo@1 if servo@0 if enables@3 servo3 := servo_on else if enables@2 servo2 := servo_on else if servo@0 if enables@1 servo1 := servo_on else if enables@0 servo0 := servo_on $tmr1on := 1 case 10 if servo@1 if servo@0 $adcon0 := sense3_channel << 2 else $adcon0 := sense2_channel << 2 else if servo@0 $adcon0 := sense1_channel << 2 else $adcon0 := sense0_channel << 2 $adon := 1 case 11 # Left justified result: $adfm := 0 # Use Vdd for voltage reference: $vcfg := 0 # Fire off the A/D conversion: $go := 1 case 12 analogs[servo] := $adresh if !cal_mode enables := 3 temporary := analogs[2] positions[0] := temporary positions[1] := analogs[3] case 31 servo0 := servo_off servo1 := servo_off servo2 := servo_off servo3 := servo_off procedure mask_get argument bit_number byte returns byte # This procedure will return the mask for {bit_number}. switch bit_number case 0 return 1 case 1 return 2 case 2 return 4 case 3 return 8 constant zero8 = "\0,0,0,0,0,0,0,0\" constant module_name = "\7\Servo4J" constant vendor_name = "\7\Gramson" string id = "\1,0,15,7,1,0,0,0\" ~ zero8 ~ zero8 ~ module_name ~ vendor_name