ucl 1.0 # ***************************************************************************** # # Copyright (c) 2000-2004 by Wayne Gramlich # All rights reserved. # # This is the code that implements the Servo4 module. Basically # it just waits for commands that come in at 2400 baud and responds # to them. See: # # http://web.gramlich.net/projects/robobricks/servo4/index.html # # for more details. # # ***************************************************************************** # ***************************************************************************** # # This code is pretty complicated. It has the goals of properly responding # to commands sent 2400 baud and keeping up to 4 servos on position without # any sevro chatter. # # The output waveforms from the chip look basically as follows: # # servo0 __[XX]____________________________[XX]__________________________ # servo1 __________[XX]____________________________[XX]__________________ # servo2 __________________[XX]____________________________[XX]__________ # servo3 __________________________[XX]____________________________[XX]__ # # The basic unit of timing is the "tick", which is definded as 1/3 of # a bit time at 2400 baud, or 1/(3*2400), or .138ms or 138us. The code # uses 32 ticks to service one servo, 32/(3*2400) = 4.44ms. Since there # are 4 servos, the total cycle time is (4*32*)/(3*2400) = 17.7ms. This # is called the "frame rate" and is just a tiny bit shorter than the # target of 20ms. # # Initially, the servo4 code was carefully crafted to generate # pulses that went from 1ms-2ms for each servo. Unfortunately, # most servos only go from +60 to -60 degrees with 1-2ms input # pulse width. In order, to get a full range of motion out of # most servos, the range has been extended from .2ms to 2.3ms. # This means that some values may cause the servo to push # against its mechanical stops. # # During the 32 ticks (numbered from 0 to 31) that are used to # service a particular servo. The ticks are divided up as follows: # # Servo Tick 0: # This tick is used to figure out which servo line to turn # on (if enabled.) # Servo Tick 1: # This tick just leaves the servos alone. # Servo Ticks 2-17 (17-2+1=16 ticks total): # These ticks are used to figure out when when to turn # the servo off. 17/(3*2400) = 2.3ms. # Servo Tick 18: # This is fail safe code that just turns off off the servo # just in case something goes wrong. # Servo Ticks 19-31: # These ticks are used to decode any commands that have # come in from the serial line. # # For Ticks 2-17, the first "half" is spent checking against the # position variable. The second "half" is allocated to serial I/O # service. Bit 3 of the position variable specifies which tick # "half", the servo off needs to occur in. If bit 3 is a 1, # we generate a "long" pulse by turning on the servo at the # beginning of tick 0. If bit 3 is a 0, we generate a "short" # pulse by turning on the servo halfway through tick 0. By # adjusting the start of the pulse on bit 3 in tick 0, it is # ensured that the off pulse will always occur in the first # half of ticks 2-17. Here is some drawings: # # Position=0: ________[XXXXXXXXXXXXXXXXXXXXXXX]________________________________ # Position=7: ________[XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________________________ # Position=8: [XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________________________________ # Position=15: [XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________________________ # Position=16: _________[XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________________ # Position=23: _________[XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________ # Position=24: [XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________________ # Position=31: [XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX]________ # # Tick: |<------0------>|<------1------>|<------2------>|<------3------>| # # Since bit 3 is used to adjust the beginning and end of the # pulse start, it is no longer used in the counting process. # This is done by clearing bit 3 at the end of every servo # service tick. # # The fine grain positioning of the servo off signal is done with # some timing loops. These loops have been carefully hand tweaked # to provide approximately 8 instructions of delay for each position # increment. It was necessary to unroll the loop a little to # smooth out the increments. # # The serial I/O is done using a state machine using the variable # "state" . The state variable can range from 0 through 61. # The states are as follows: # # Serial State 0: # Wait for start bit. # Serial State 1-29: # Receive byte. # Serial State 31-61: # Send byte. # # The state machine stays in state 0 until it receives a start bit. # It then spends the next 29 ticks receiving a byte of data before # returning to state 0. Right before it returns to state 0, it # sets the decode_requested bit. During servo ticks 19-31, the # decode_requested bit is tested to see whether the command decode # procedure needs to be called. # # The decode procedure is separate from everything only because # not everything can be crammed into a single code bank of the # PIC12C509. Instead, the decode routine is in code bank 0 # and the main routine is in code bank 1. The decode takes the # received byte and dispatches appropriately. If it needs to # send a response byte, it crams the byte into the "buffer" # variable and sets state to 31. The next 30 ticks, will cause # buffer to be emptied out to the serial port. # # Since decoding is always deferred to servo ticks 19-31, it # is quite possible that command decoding will not commence # until well after a second byte of data is being received. # That is OK, since a byte takes at least 29 ticks to receive # whereas the longest it takes to process a servo is 20 ticks. # Thus, there will never be a buffer overrun. # # ***************************************************************************** library $pic12f629 library clock4mhz package pdip pin 1 = power_supply pin 2 = gp5_out, name = serial_out, mask = serial_out_mask pin 3 = gp4_out, name = servo3, bit = servo3_bit pin 4 = gp3_in, name = serial_in, mask = serial_in_mask pin 5 = gp2_out, name = servo2, bit = servo2_bit pin 6 = gp1_out, name = servo1, bit = servo1_bit pin 7 = gp0_out, name = servo0, bit = servo0_bit pin 8 = ground configure cp=on, wdte=on, mclre=off, fosc=rc_no_clk # Define serial communication control constants: constant baud_rate = 2400 constant instructions_per_bit = instruction_rate / baud_rate constant delays_per_bit = 3 constant instructions_per_delay = instructions_per_bit / delays_per_bit # Number of servos (changed only during debugging): constant servos_power = 2 constant servos = (1 << servos_power) constant servos_maximum = (servos - 1) constant servos_index_mask = servos_maximum # Register definitions: global counter byte global positions[servos] array[byte] global enable_mask byte global servo byte global glitch byte global id_index byte global state byte global received byte global buffer byte global decode_state byte global position byte global previous byte global amount byte global decode_request bit procedure main arguments_none returns_nothing # This procedure waits for commands and keeps the servo pulses going: local temp byte # Initialize everything: servo := 0 loop_exactly servos positions[servo] := 0x80 servo := servo + 1 buffer := 0 counter := 0 decode_request := 0 decode_state := 0 enable_mask := 0 glitch := 0 id_index := 0 position := 0 serial_out := 1 servo := 0 state := 0 # Wait for commands: loop_forever # Loop_forever has an overhead of 2 instructions per cycle: delay instructions_per_delay - 2 # This is the servo maintenence portion of the code: counter := counter + 1 switch counter & 0x1f case_maximum 0x1f case 0 # Counter = xss0 0000 (Turn on Servo Pulse): servo := (counter >> 5) & servos_index_mask position := positions[servo] if position@3 # Long pulse -- turn on immediately: switch servo case 0 # Servo 0: if enable_mask@0 servo0 := 1 case 1 # Servo 1: if enable_mask@1 servo1 := 1 case 2 # Servo 2: if enable_mask@2 servo2 := 1 case 3 # Servo 3: if enable_mask@3 servo3 := 1 position@3 := 0 else # Short pulse turn on after approximately half a tick: delay 67 # We've got plenty of time to set things up during # this delay, but not much time after that. temp := $gpio & (serial_in_mask | serial_out_mask) switch servo case 0 # Servo 0: if enable_mask@0 temp@servo0_bit := 1 case 1 # Servo 1: if enable_mask@1 temp@servo1_bit := 1 case 2 # Servo 2: if enable_mask@2 temp@servo2_bit := 1 case 3 # Servo 3: if enable_mask@3 temp@servo3_bit := 1 # Now just store the the result into {$gpio}: $gpio := temp case 1 # Do nothing. (Leave pulse on): watch_dog_reset case 18 # Fail safe. Turn all servos off: watch_dog_reset servo0 := 0 servo1 := 0 servo2 := 0 servo3 := 0 case 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17 { # Servo service: position := position + 1 switch servo case 0 delay 65 loop_exactly 4 position := position - 1 if $z servo0 := 0 delay 5 do_nothing position := position - 1 if $z servo0 := 0 delay 2 do_nothing case 1 loop_exactly 4 position := position - 1 if $z servo1 := 0 delay 5 do_nothing position := position - 1 if $z servo1 := 0 delay 2 do_nothing case 2 loop_exactly 4 position := position - 1 if $z servo2 := 0 delay 5 do_nothing position := position - 1 if $z servo2 := 0 delay 2 do_nothing case 3 loop_exactly 4 position := position - 1 if $z servo3 := 0 delay 5 do_nothing position := position - 1 if $z servo3 := 0 delay 2 do_nothing position := position - 1 position@3 := 0 default # Do decode when we are not busy servicing the servos: if decode_request decode_request := 0 call decode() # This is the serial I/O state machine. It is very # carefully coded to take as few cycles as possible. switch state case_maximum 61 case 0 # This is the wait for next byte state: # We test for the start bit: if !serial_in # We have a start bit! Now sequence through the # remaining states: state := state + 1 case 3, 6, 9, 12, 15, 18, 21, 24 # Shift the buffer byte over by one bit: state := state + 1 buffer := buffer >> 1 case 4, 7, 10, 13, 16, 19, 22, 25 # Test to see whether we have a bit or not: state := state + 1 if serial_in buffer@7 := 1 case 29 # We are done receiving. We stop 2/3's of the way # through the stop bit: state := 0 received := buffer decode_request := 1 case 31 # Send start bit: state := state + 1 serial_out := 0 case 34, 37, 40, 43, 46, 49, 52, 55 # Send data bit: state := state + 1 serial_out := buffer@0 case 35, 38, 41, 44, 47, 50, 53 # Shift sending byte: state := state + 1 buffer := buffer >> 1 case 58 # Send stop bit state := state + 1 serial_out := 1 case 60 # Done sending: state := 0 default # All other states do nothing except increment state variable: state := state + 1 procedure decode arguments_none returns_nothing # 75 was found by trail and error: exact_delay 75 local temp byte switch decode_state case 0 # Main decode: switch received >> 6 case 0 # Received = 00hh hhss (Set High): temp := received & servos_index_mask positions[temp] := (received << 2) & 0xf0 case 1 # Received = 01ll llss (Set Low): temp := received & servos_index_mask positions[temp] := positions[temp] & 0xf0 | (received >> 2) & 0xf case 2 # Received = 10xx xxxx: temp := received & servos_index_mask amount := (received >> 2) & 7 if received@5 # Decrement (Received = 101s sddd): amount := 0 - amount # else Increment (Received = 100s siii) do nothing: positions[temp] := positions[temp] + amount case 3 # Received = 11xx xxxx: switch (received >> 3) & 7 case 0 # Set Position/Enable (Received = 1100 0ess): previous := received decode_state := 1 case 1 # Set Enable (Received = 1100 1ess): temp := received & servos_index_mask temp := masks[temp] if received@2 enable_mask := enable_mask | temp else enable_mask := enable_mask & (0xff ^ temp) case 2 { # Received = 1101 0xxx: temp := received & servos_index_mask if received@2 # Read Enable (Received = 1101 01ss): buffer := 0 if enable_mask & masks[temp] != 0 buffer := buffer + 1 else # Read Position (Received = 1101 00ss): buffer := positions[temp] state := 31 case 3 # Received = 1101 1xxx: switch received & 7 case 0 # Read Enables (Received = 1101 1000): buffer := enable_mask state := 31 case 1 # Set Enables (Received = 1101 1001): decode_state := 2 case 2, 3, 4, 5, 6, 7 do_nothing case 4, 5, 6 # Received = 1110 0xxx, 1110 1xxx, or 1111 0xxx: # Do nothing: case 7 # Shared Commands (Received = 1111 1xxx): switch received & 7 case 0 # Clock Decrement (Received = 1111 1000): $osccal := $osccal - $osccal_lsb case 1 # Clock Increment (Received = 1111 1001): $osccal := $osccal + $osccal_lsb case 2 # Clock Read (Received = 1111 1010): buffer := $osccal state := 31 case 3 # Clock Pulse (Received = 1111 1011): buffer := 0 state := 31 case 4 # ID Next (Received = 1111 1100): buffer := 0 if id_index < id.size buffer := id[id_index] id_index := id_index + 1 state := 31 case 5 # ID Reset (Received = 1111 1101): id_index := 0 case 6 # Glitch Read (Received = 1111 1110): buffer := glitch state := 31 glitch := 0 case 7 # Glitch (Received = 1111 1111): if glitch != 0xff glitch := glitch + 1 case 1 # Set Position/Enable (2nd byte = pppp pppp): decode_state := 0 temp := previous & servos_index_mask positions[temp] := received temp := masks[temp] if previous@2 enable_mask := enable_mask | temp else enable_mask := enable_mask & (0xff ^ temp) case 2 # Set Enables (2nd byte = 0000 eeee): decode_state := 0 enable_mask := received & 0xf constant zero8 = "\0,0,0,0,0,0,0,0\" constant module_name = "\7\Servo4E" constant vendor_name = "\15\Gramlich&Benson" string id = "\1,0,15,4,0,0,0,0\" ~ zero8 ~ zero8 ~ module_name ~ vendor_name string masks = "\1,2,4,8\"