ucl 2.0 # Copyright (c) 2006-2009 by Wayne C. Gramlich # All rights reserved. # This code implements the firmware for the MidiMotor1E. # The MidiMotor1E differs from the MidiMotor1 in that the '1E has # two extra connectors N2 and N4. N2 is a 5 pin connector that can # be used to read in two channels of quardarature position (plus an # an index pulse.) Alternatively, N2 can be used to read in an # analog signal between 0 and 5V for an absolute position. N4 is # used as an active low limit switch that can be used to term off # the motor. # # The architecture of this firmware is actually pretty complicated. # The pulse width modulation is controlled by the Timer0 and Timer1 # modules. The quadrature encoding and limit switch is delt with # via some pins on Port A. The UART is responsible for dealing # with RoboBricks2 bus I/O. The A/D converter is responsible # for processing Analog to digital conversions. The code in the # main loop is responsible for running a PID (Proportional, # Integral, Derivative) motor control loop. The PID algorithm is # done using 32-bit floating point arithmetic. Heavy use of interrupts # is used to ensure that everything works in a timely fashion. # # There are three main routines -- interrupt(), main() and uart_manage(). # The interrupt() routine is responsible for processing all interrupt # events. UART processing is sufficiently complicated, that UART input # is quickly stuffed into a temporary buffer, to minimize intterupt # latency. Further UART process is handled by the uart_manage() routine. # Calls to the uart_manage() routine are sprinkled throught the main # loop in main() to ensure that any buffered uart commands are processed # fairly quickly. # We use the PIC16F688 for this module: library $pic16f688 # The module runs at 20MHz: library clock20mhz # The {$eusart} module needs the following 2 constants defined # before inclusion: constant $eusart_clock = clock_rate constant $eusart_factor = 4 library $eusart library rb2_constants # Constants used by firmware and clients # The module uses a 20MHz resonator; thus, the oscillator mode is HS: configure fosc=hs # Besides the bus RX/TX lines, only 4 other lines are used to # control the motor -- RC<0:3>: 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_nc pin 8 = rc2_nc pin 9 = rc1_out, name=m1b, mask=m1b_mask, bit=m1b_bit pin 10 = rc0_out, name=m1a, mask=m1a_mask, bit=m1a_bit pin 11 = an2 pin 12 = ra1_in, name=quad_a, mask=quad_a_mask, bit=quad_a_bit pin 13 = ra0_in, name=quad_b, mask=quad_b_mask, bit=quad_b_bit pin 14 = ground # All of the 24-bit signed file24 in the 24-bit register file are stored # in the {highs}, {middles} and {lows} arrays: constant position_index = 0 # Current position constant target_index = 1 # Target position constant divisor_index = 2 # Denominator for {kp}, {ki}, and {kd} constant kp_index = 3 # Numerator for {kp} constant ki_index = 4 # Numerator for {ki} constant kd_index = 5 # Numerator for {kd} constant debug_index = 6 constant file24_size = 7 # 24-bit register file has 6 registers # We are short of bytes in data bank 0, so we stuff the 24-bit register # files off into data bank 2: data_bank 2 global highs[file24_size] array[byte] # All the highs are grouped together global middles[file24_size] array[byte] # All the middles are grouped together global lows[file24_size] array[byte] # All the lows are grouped together # The quardarture is actually the same as the position: bind quad_high = highs[position_index] bind quad_middle = middles[position_index] bind quad_low = lows[position_index] global quad_state byte # The quadrature state global xstates[32] array[byte] # Every else with the 24-bit register file24 is in data bank 0: data_bank 0 global file24_changed byte # 1 bit for each value to mark change global file24_zero byte # 1 bit for each value set to zero # A number of places need a temporary for high, middle and low. Rather # than chew up scarse memory, we share them all with globals. As of now, # these bytes are used in {file24_float_convert}() and {uart_manage}(): global high byte # Temporary high byte global middle byte # Temporary middle byte global low byte # Temporary low byte # The 8-bit register file is stored in {file8} and indexed using the # indices below: constant configure_index = 0 # Configuration byte index constant status_index = 1 # Status byte index constant speed_index = 2 # Current motor speed index constant errors_index = 3 # Error count constant deadband_index = 4 # Deadband for motor speed constant idle_index = 5 # Current idle count constant limit_index = 6 # Loop index constant file24_index_index = 7 # Index into 24-bit register file constant file8_size = 8 # We have 8 registers in {file8] # Here is the actual array of {file8}: global file8[file8_size] array[byte] # It is a hassle to always index into {file8}, we use binds to make # the code more readable: bind configure = file8[configure_index] bind motor_reverse = configure@0 # 1=>reverse motor direction bind position_reverse = configure@1 # 1=>reverse position direction bind use_potentiometer = configure@2 # 1=>potentiometer; 0=>quadrature bind status = file8[status_index] bind speed = file8[speed_index] bind errors = file8[errors_index] bind deadband = file8[deadband_index] bind idle = file8[idle_index] bind limit = file8[limit_index] bind file24_index = file8[file24_index_index] # These are the variables used to control motor1: global motor1_speed byte global motor1_on_mask byte global motor1_pulse_width byte # UART Globals and constants: global state byte # State machine for command parsing constant state_select_wait = 0 # Waiting for a select address constant state_echo_then_command = 1 # Wait for echo then a command constant state_command = 2 # Wait for a command constant state_file24_full_get1 = 3 constant state_file24_full_get2 = 4 constant state_file24_full_set1 = 5 constant state_file24_full_set2 = 6 constant state_file24_full_set3 = 7 constant state_value_low_set1 = 8 constant state_value_middle_set1 = 9 constant state_value_high_set1 = 10 constant state_file8_set1 = 11 constant state_file8_set2 = 12 constant state_file8_set3 = 13 constant state_address_set1 = 14 constant state_address_set2 = 15 constant state_address_set3 = 16 constant state_address_set4 = 17 constant state_probe_get1 = 18 constant state_probe_get2 = 19 global id_index byte # Index into id string # UART data input buffer: constant uart_input_size = 4 # Buffer size (=power of 2) constant uart_input_mask = uart_input_size - 1 # Mask for index global uart_input[uart_input_size] array[byte] # Buffer for input global uart_input_in_index byte # Next index to insert into global uart_input_out_index byte # Next index to remove from global uart_input_count byte # Bytes in buffer global uart_input_pending bit # 1=>data is pending; 0=>no data global address byte # Address of this module global new_address byte # New address of this module # Load up floating point library in code bank 1 using datab bank 0: library_bank 1 data_bank 1 library $float32 data_bank 0 origin 0 procedure start arguments_none returns_nothing return_suppress assemble databank start,main codebank start,main goto main origin 4 procedure interrupt arguments_none returns_nothing # This is the interrupt processing routine. While it is quite long # it actually does not take very long to execute. This is important # since long interrupt routines can cause problems with over module # latency. # Step1: Save our system registers. There are 4 registers that # need to be saved -- W, STATUS, FSR, and PCLATH. # # W & STATUS: # The compiler takes care of the weird code needed to safely # save W adn STATUS registers. (Read the generated assembly code # comments to see some strange usage of the SWAPF instruction.) # Since the microcontroller can be using any of the 4 register # banks when the interrupt occurs, the W and STATUS register # are stored into a couple of shared memory bank registers. # There are 16 of these registers available, and the interupt # takes 2 of the 16. # # PCLATH: # When the interrupt occurs, the previous progam counter is # pushed onto the stack and then the program counter is set # to 4 to force execution of the interrupt routine. The PCLATH # register is left unchanged. Since the PCLATH register can # pointing to any of the 4 code banks (only 2 implemented for # the PIC16F688), it is vital that we update PCLATH to point # to code bank 0 (where the interrupt routine is) before the # first GOTO, CALL, or computed GOTO. Otherwise, the first # GOTO, CALL, or computed GOTO will jump off into code bank # that was being used prior to the interrupt. # # FSR: # The FSR register needs to be saved since we using code that # indexes into a small buffer to store UART input. # # Both PCLATH and FSR can be saved into regular registers in # register bank 0. No shared registers are needed. Once we have # STATUS stashed away, the register bank can be set to 0. The # compiler takes care of this automagically before the first save # assignment. local fsr_save byte local pclath_save byte fsr_save := $fsr pclath_save := $pclath # Make sure we are on correct page: $pclath := 0 # Now we have to deal with any of the pending interrupts. Timer0 # is set to go off at the overall duty cycle of the PWM (Pulse Width # Modulation) rate. We turn the motor "on" each time Timer0 triggers # an interrupt. We also start Timer1 to specifiy the over width of # the pulse before we turn it off. When Timer1 triggers, it is time # to turn the pulse off. Timer0 is an 8-bit register and it it is # being driven by a the system clock divided by 256. Thus, the Timer0 # triggers every 65536 (=2^16) clock ticks. Timer 1 is a 16-bit # counter that runs at the system clock. We always set the low # order byte to 0, and the high order byte to W, where W=255-PW and # PW is the pulse width as a number between 0 and 255. Thus, we can # have the pulse width vary in increments of 1/256 of the total duty # cycle. # Deal with Timer 0 interrupt: if $t0if # Timer 0 just triggered. It is time to start a new pulse width # duty cycle: # Clear the interrupt flag: $t0if := $false # Make sure Timer1 is off while we muck around with Timer1: $tmr1on := $false # Set the pulse width to W::0, where W=255-PW, where PW={0,...,255}. $tmr1l := 0 $tmr1h := motor1_pulse_width # Make sure that we clear off the Timer1 interrupt flag, just in case: $tmr1if := $false # Turn on Timer1: $tmr1on := $true # Finally, turn on the appropriate leads of the motor to make it move. $portc := motor1_on_mask if use_potentiometer quad_low := $adresl quad_middle := $adresh quad_high := 0 if position_reverse # Perform a 1's complement of the result: quad_low := quad_low ^ 0xff quad_middle := quad_middle ^ 3 quad_low := quad_low + 1 if $z quad_middle := (quad_middle + 1) & 3 # Trigger an A/D conversion to pick up next time: $go := $true # Deal with Timer 1 interrupt: if $tmr1if # Timer 1 just triggered. It is time to turn off the pulse width. # Turn off the motor leads: $portc := 0 # Turn off Timer 1: $tmr1on := $false # Make sure we clear timer 1 interrupt flag: $tmr1if := $false # Deal with UART receive interrupt: if $rcif # We have a byte in the UART receive buffer. We want to be as # quick as possible here. Do the minimum to process the incoming # byte and move on.a # First, look at the ninth bit. If the ninth bit is set, we # process it immediately. if $rx9d # The ninth bit is set, we have an module address select # command: if $rcreg = address # The address that came in matches ours in {address}. # We need to prepare for further incoming data bytes: # Force the UART to start accepting data bytes with the # ninth bit clear: $adden := $false # Clear out the input data buffer: uart_input_in_index := 0 uart_input_out_index := 0 uart_input_count := 0 uart_input_pending := $false # Send an acknowledge byte: call uart_byte_put(rb2_ok) # Set the state machine for {uart_manage} to eat the echo # and start processing commands: state := state_echo_then_command else # The address that came in did not match our address: # Force the UART to only listen to bytes that have the # ninth bit set (i.e. address commands). All data bytes # to the other selected module will be invisible to us: $adden := $true # Set the state machine for {uart_manage} to ignore any # incoming data bytes. This should never happen, be it # it never hurts to be careful. state := state_select_wait else # We have a data byte for us to process. For now we # stuff it into an input buffer: # Stuff the byte into the {uart_input} buffer being careful # not to go out of bounds. The buffer is a power of 2 in # size, so masking {uart_input_in_index} with {uart_input_mask} # will keep us in bounds: uart_input[uart_input_in_index & uart_input_mask] := $rcreg # Bump {uart_input_in_index} so that we next byte the comes # in will follow this current byte: uart_input_in_index := uart_input_in_index + 1 # Keep track of how many bytes are in the input buffer: uart_input_count := uart_input_count + 1 # Let the {uart_manage} routine know that it has work to do: uart_input_pending := $true # The PIC UART can get wedged by errors. We fix that problem # by toggling {$cren} whenever we have an overrun error. The # code below is really tight (5 instructions): if $oerr # We have an over run error, clear {$cren}: $cren := $false if $ferr # We have a framing error, clear {$cren}: $cren := $false # If either of the two previous statements triggered, {$cren} # got turned off. This next statement will turn it on again. # Otherwise, {$cren} is already on, and this statement will do # nothing: $cren := $true # Clear the recieve interrupt condition: $rcif := $false # Deal with quadrature input changes: if $raif # The file24 a the A and B channels of Port A have changed # since the last time we read Port A. Deal with the change # by cycling the {quad_state} state machine. Please carefully # read the documentation in the states.ucl library to understand # the format of the {states} vector. # Cycle the {quad_state} state machine. Use only the lower 3 bits # currently in {quad_state} concatenated with the low order 2 bits # of {$porta} (the A and B quadrature channels). That gives a total # of 5 bits to work with: quad_state := xstates[quad_state & 7 | ($porta << 3) & 0x18] # Disable interrupt condition until the next time: $raif := $false # The rest of this code just increments and decrements the quadrature # counter. The quad counter is signed 24 bit integer that is stored # in {quad_high}, {quad_middle}, and {quad_low}. This code is kind # big and long, but in reality, 99% of the time we will perform either # a single increment or decrement of {quad_low}. So, most of the # time this code very fast: if quad_state@7 # The state machine wants us to increment the quadrature counter # by 1. We work our way from {quad_low} through {quad_high}: quad_low := quad_low + 1 if $z # The zero status flag is set, so {quad_low} just wrapped from # 0xff to 0. Thus, we need to increment {quad_middle}" quad_middle := quad_middle + 1 if $z # The zero status flat is set, so {quad_middle} just # wrapped from 0xff to 0. Thus we need to increment # {quad_high}: quad_high := quad_high + 1 if quad_state@6 # This bit will be set in the state machine if we have an # "illegal" state machine transistion. # Bump the {errors} counter to let somebody know # that we are having problems: errors := errors + 1 # This code is the same as the first increment, so the # comments are not repeated: quad_low := quad_low + 1 if $z quad_middle := quad_middle + 1 if $z quad_high := quad_high + 1 if quad_state@5 # The state machine wants us to decrement the quadrature counter # by 1. We work our way from {quad_low} through {quad_high}: if quad_low = 0 # {quad_low} is 0, so we are going to wrap down to 0xff. # We need to decrement {quad_middle}: if quad_middle = 0 # {quad_middle} is 0, we are going to wrap down to 0xff. # We need to decrement {quad_high}: quad_high := quad_high - 1 # Do the {quad_middle} decrement: quad_middle := quad_middle - 1 # Do the {quad_low} decrement: quad_low := quad_low - 1 if quad_state@4 # This bit will be set in the state machine if we have an # "illegal" state machine transition: # Bump the {errors} counter to let somebody know that # we are having problems: errors := errors + 1 # This code is the same as the first decrement, so the # comments are not repeated: if quad_low = 0 if quad_middle = 0 quad_high := quad_high - 1 quad_middle := quad_middle - 1 quad_low := quad_low - 1 # All done processing interrupt conditions. Let's get out of here by # restoring {$fsr} and {$pclath}: $fsr := fsr_save $pclath := pclath_save # The compiler takes care of restoring {$status} and {$w}. # Split the floats between data banks 1 and 2: data_bank 2 global error float32 global position float32 global target float32 global kp float32 global ki float32 data_bank 1 global kd float32 global pid float32 global divisor float32 global numerator float32 data_bank 0 global target_speed byte # Target motor speed global target_seek bit procedure main arguments_none returns_nothing local pos_high byte local pos_middle byte local pos_low byte local negative bit local small_error byte call reset() loop_forever # Make sure we manage the UART: call uart_manage() # Update target if necesary if file24_changed != 0 # Something has changed: if file24_changed@target_index target_seek := $true target := file24_float_convert(target_index) file24_changed@target_index := $false call uart_manage() if file24_changed@divisor_index divisor := file24_float_convert(divisor_index) if file24_zero@divisor_index # Yikes, the user set the divisor to zero; set to 1 instead: lows[divisor_index] := 1 divisor := 1.0 file24_changed@divisor_index := $false file24_changed@kp_index := $true file24_changed@ki_index := $true file24_changed@kd_index := $true call uart_manage() if file24_changed@kp_index numerator := file24_float_convert(kp_index) call uart_manage() kp := numerator / divisor file24_changed@kp_index := $false call uart_manage() if file24_changed@ki_index numerator := file24_float_convert(ki_index) call uart_manage() ki := numerator / divisor file24_changed@ki_index := $false call uart_manage() if file24_changed@kd_index numerator := file24_float_convert(kd_index) call uart_manage() kd := numerator / divisor file24_changed@kd_index := $false call uart_manage() if target_seek # Fetch the position atomically: $gie := $false pos_high := quad_high pos_middle := quad_middle pos_low := quad_low $gie := $true call uart_manage() # Convert the position bytes into a float: position := xyz(pos_high, pos_middle, pos_low) call uart_manage() # Figure out the proportional error: error := position - target call uart_manage() # Compute the PID equation: pid := kp * error call uart_manage() # Set the new motor speed: negative := $false if pid < 0.0 # Work in positive arithmetic: negative := $true pid := -pid if pid > 127.0 target_speed := 127 idle := 0 else target_speed := $float32_to_byte(pid) call uart_manage() if error >= 0.0 if error >= 255.0 small_error := 255 else small_error := $float32_to_byte(error) else if error < -255.0 small_error := 255 else small_error := $float32_to_byte(-error) call uart_manage() if small_error <= deadband idle := idle + 1 if idle >= limit target_seek := $false target_speed := 0 if negative target_speed := 0 - target_speed call speed_set(target_speed) procedure reset arguments_none returns_nothing # This procedure will return the restore all of the state to start-up state. local index byte # Initialize UART: address := rb2bus_eedata_read() if address = 0 address := 21 call uart_initialize() # Initialize prescaler for use by Timer 0: # $rapu := $true # Disable bit port A pull-ups (bit 7) # $integ := $false # Interrupt edge select (bit 6) # $t0cs := $false # Use clock for tmr0 (bit 5) # $t0se := $false # Source edge select (bit 4) # $psa := $false # Prescaler is assigned to timer0 (bit 3) # $ps2 := $true # $ps1 := $true # $ps0 := $true # Prescaler = 256 # 1000 0111 = 0x87 $option_reg := 0x87 # Initialize Timer 1: # $t1ginv := $false # Timer1 gate (bit 7) # $tmr1ge := $false # Timer1 gate enable (bit 6) # $t1ckps1 := $false # (bit 5) # $t1ckps0 := $false # Timer1 prescale = 1:1 (bit 4) # $t10scen := $false # LP oscillator is off (bit 3) # $t1sync := $false # Timer 1 synchorinize (bit 2) # $tmr1cs := $false # Use internal clock (bit 1) # $tmr1on := $false # Leave timer off for now (bit 0) # 0000 0000 = 0 $t1con := 0 # Initialize A/D module: # ANSEL is already set by the compiler: # # ADCON0 needs to be configured: # $adfm := $true # Right justified result # $vcfg := $false # Use 5V as Vref # $chs2 := $false # $chs1 := $true # $chs0 := $false # Channel = 010 = AN2 # $go := $false # $adon := $true # Turn on A/D converter # 10x0 1001 = 0x89 $adcon0 := 0x89 # ADCON1 needs to be configured: # ADCS<2:0> := 110 = Tad=3.2uS at Fosc=20MHz # $adcs2 := $true # $adcs1 := $true # $adcs0 := $false # x110 xxxx = 0x60 $adcon1 := 0x60 # Initialize motor: call speed_set(0) # Initialize {file8}: index := 0 while index < file8_size file8[index] := 0 index := index + 1 call configure_update() deadband := 4 limit := 20 # Initialize {file24}: index := 0 while index < file24_size highs[index] := 0 middles[index] := 0 lows[index] := 0 index := index + 1 file24_index := 0 file24_changed := 0 file24_zero := 0 # For now force divisor to -1: highs[divisor_index] := 0xff middles[divisor_index] := 0xff lows[divisor_index] := 0xff # Force the an update of {divisor}: file24_changed@divisor_index := $true # For now kp = 20: lows[kp_index] := 1 # No need to manually force a {kp} update, since updating {divisor} does # that already: # We only turn on target seeking when the {target} is changed. # It gets turned off when find the target (or a limit switch is hit): target_seek := $false # It does not really matter, but setting {target} to 0.0 does not hurt: target := 0.0 # Always initialize {quad_state} even if we do not use quadrature: quad_state := 0 # The weak pull-ups can be turned off: $wpua := 0 # We are only interested in changes in the A and B channels: $ioca := quad_a_mask | quad_b_mask # We only want to turn on the interrupt on change if we are using # the quadrature (i.e. !{use_potentiometer}: $raie := !use_potentiometer # Enable the appropriate interrupts: $t0if := $false $t0ie := $true $tmr1if := $false $tmr1ie := $true $peie := $true $gie := $true procedure uart_manage arguments_none returns_nothing local command byte local index byte if uart_input_count != 0 # We have a data/command byte to process: command := uart_input[uart_input_out_index & uart_input_mask] uart_input_out_index := uart_input_out_index + 1 # Manage {uart_input_count} and {uart_input_pending} atomically: $gie := $false uart_input_count := uart_input_count - 1 if uart_input_count = 0 uart_input_pending := $false $gie := $true # We received a "data" byte" switch state case state_select_wait # Technically we should not get here because {$adden} should # be {$true}, but just in case we do, we just ignore the byte: do_nothing case state_echo_then_command # Ignore the echo before processing a command byte: state := state_command case state_command # Process a command byte: switch command >> 6 case 0 # 00xx xxxx: switch (command >> 3) & 7 case 0 # 0000 0xxx: switch command & 7 case 0 # 0000 0000 (Value All Get): call uart_byte_put(highs[file24_index]) state := state_file24_full_get1 case 1 # 0000 0001 (Value Low Get): call uart_byte_put(lows[file24_index]) state := state_echo_then_command case 2 # 0000 0010 (Value Middle Get): call uart_byte_put(middles[file24_index]) state := state_echo_then_command case 3 # 0000 0011 (Value High Get): call uart_byte_put(highs[file24_index]) state := state_echo_then_command case 4 # 0000 0100 (Value All Set): state := state_file24_full_set1 case 5 # 0000 0101 (Value Low Set): state := state_value_low_set1 case 6 # 0000 0110 (Value Middle Set): state := state_value_middle_set1 case 7 # 0000 0111 (Value High Set): state := state_value_high_set1 case 1 # 0000 1sss (Value Index Set): file24_index := command & 7 # Make sure we stay in bounds: if file24_index >= file24_size file24_index := 0 state := state_command case 2 # 0001 0xxx (Byte Get): index := command & 7 if index = 2 # 0001 0010: (Status_Get): call status_update() call uart_byte_put(file8[index]) state := state_echo_then_command case 3 # 0001 1xxx (Byte Set): index := command & 7 state := state_file8_set1 case 4 # 0010 0xxx (Probe Get): switch command & 7 case 0 call zyx(position) case 1 call zyx(target) case 2 call zyx(error) case 3 call zyx(pid) case 4 call zyx(divisor) case 5 call zyx(kp) case_maximum 7 call uart_byte_put(high) state := state_probe_get1 case_maximum 7 case 3 # 11xx xxxx: switch (command >> 3) & 7 case 7 # 1111 1xxx: switch command & 7 case 4 # 1111 1100 (Address Set): call uart_byte_put(address) state := state_address_set1 case 5 # 1111 1101 (Id_Next): call uart_byte_put(id[id_index]) id_index := id_index + 1 if id_index >= id.size id_index := id_index - 1 state := state_echo_then_command case 6 # 1111 1110 (Id_Start): id_index := 0 case 7 # 1111 1111 (Deselect): $adden := $true state := state_select_wait case state_file24_full_get1 # Value All Get 2nd response byte: call uart_byte_put(middles[file24_index]) state := state_file24_full_get2 case state_file24_full_get2 # Value All Get 3rd response byte: call uart_byte_put(lows[file24_index]) state := state_echo_then_command case state_file24_full_set1 # Value All Set 2nd command byte: high := command state := state_file24_full_set2 case state_file24_full_set2 # Value All Set 3rd command byte: middle := command state := state_file24_full_set3 case state_file24_full_set3 # Value All Set 4th command byte: highs[file24_index] := high middles[file24_index] := middle lows[file24_index] := command call file24_updated() state := state_command case state_value_low_set1 # Value Low Set 2nd command byte: lows[file24_index] := command call file24_updated() state := state_command case state_value_middle_set1 # Value Middle Set 2nd command byte: middles[file24_index] := command call file24_updated() state := state_command case state_value_high_set1 # Value High Set 2nd command byte: highs[file24_index] := command call file24_updated() state := state_command case state_file8_set1 # File8 Set 2nd command byte: file8[index] := command switch index case configure_index call configure_update() case speed_index call speed_set(speed) case file24_index_index if command > file24_size command := 0 case_maximum 7 state:= state_command case state_address_set1 # Ignore the echo and wait for the new address: state := state_address_set2 case state_address_set2 # First, copy of new address just arrived: new_address := command # Return it call uart_byte_put(new_address) state := state_address_set3 case state_address_set3 # Ignore the echo state := state_address_set4 case state_address_set4 # We have the 2nd copy of new addres. if command = new_address # They match; write it in: call rb2bus_eedata_write(new_address) $gie := $true address := new_address call uart_byte_put(rb2_ok) else # They do not match, return an error: call uart_byte_put(0) state := state_echo_then_command case state_probe_get1 call uart_byte_put(middle) state := state_probe_get2 case state_probe_get2 call uart_byte_put(low) state := state_echo_then_command procedure file24_updated arguments_none returns_nothing # This routine will cause the {file24_change} mask to be updated # with the value that changed. switch file24_index case position_index do_nothing case target_index file24_changed@target_index := $true case divisor_index file24_changed@divisor_index := $true # Changing the divisor will cause kp, ki, and kd to be updated: case kp_index file24_changed@kp_index := $true case ki_index file24_changed@ki_index := $true case kd_index file24_changed@kd_index := $true procedure file24_float_convert argument index byte returns float32 # This routine will return {file24}[{index}] as a {float32}. local mask byte high := highs[index] middle := middles[index] low := lows[index] mask := index2mask[index] if high = 0 && middle = 0 && low = 0 file24_zero := file24_zero | mask else file24_zero := file24_zero & (mask ^ 0xff) return xyz(high, middle, low) procedure status_update arguments_none returns_nothing # This routine will cause {status} to be updated. do_nothing procedure configure_update arguments_none returns_nothing local index byte local state byte # This routine will read {configure} and process it: index := 0 while index < 32 state := states[index] if position_reverse # Swap the increment and decrement bits in {xstates}: state := state & 0xf | (state << 2) & 0xc0 | (state >> 2) & 0x30 xstates[index] := state index := index + 1 procedure uart_byte_put argument value byte returns_nothing # This routine will send {value} to the bus. # Wait until {$txreg} is ready for a value: while !$txif do_nothing # Ship {value} out to the bus with 9th bit turned off: $adden := $false $tx9d := $false $txreg := value procedure uart_initialize arguments_none returns_nothing # This procedure is responsibile for initializing the UART # connected to the bus. This code is currently specific to # the PIC16F688. #FIXME: Use {rx_bit} and {tx_bit}!!! # Warm up the UART: $trisc@5 := $true $trisc@4 := $true # Initialize the {$txsta} register: $txsta := 0 $tx9 := $true $txen := $true $brgh := $true # Initialize the {$rcsta} register: $rcsta := 0 $spen := $true $rx9 := $true $cren := $true $adden := $true # Set up the baud rate generator: $baudctl := 0 $brg16 := $true $spbrg := $eusart_500000_low $spbrgh := $eusart_500000_high state := state_select_wait id_index := 0 uart_input_in_index := 0 uart_input_out_index := 0 uart_input_count := 0 uart_input_pending := $false # Turn on the interrupts: $txie := $false $rcie := $true procedure speed_set argument speed byte returns_nothing # This procedure will set {motor1_speed}, {motor1_on_mask}, and # {motor1_pulse_width} from {speed}. motor1_speed := speed if motor1_speed@7 # Reverse direction: if motor_reverse motor1_on_mask := 1 else motor1_on_mask := 2 motor1_pulse_width := speed << 1 else if speed = 0 # Stopped motor1_on_mask := 0 else # Forward direction: if motor_reverse motor1_on_mask := 2 else motor1_on_mask := 1 motor1_pulse_width := 0xff - (speed << 1) # FIXME: These two routines are directly copied from rb2_pic16f688.ucl # and should be included via a library command. Alas, the rb2_pic16f688.ucl # library also include rb2bus.ucl, which we do not want!!! constant rb2bus_eedata_address = 0xfe procedure rb2bus_eedata_read arguments_none returns byte # This procedure will return the address stored in EEData. If # there is no data, 0 is returned. local temp1 byte local temp2 byte # Read the first byte (the address): $eecon1 := 0 $eeadr := rb2bus_eedata_address $rd := $true temp1 := $eedat # Read the second byte (the 1'z complement) $eeadr := $eeadr + 1 $rd := $true temp2 := $eedat # If they are 1's complement of one another, they are valid: if temp1 = (0xff ^ temp2) # Return the valid address: return temp1 # They are not 1's complement, so return 0 as an error: return 0 procedure rb2bus_eedata_write argument address byte returns_nothing # This procedure will write {address} into the EEData. The # microcontroller pauses while the EEData is written. # Clear out the {$eecon1} register $eecon1 := 0 $eeadr := rb2bus_eedata_address $eedat := address # Write 2 bytes ({address} followed by {address}^0xff): loop_exactly 2 # Set the data to write: # Set up the for the write: $wren := $true $gie := $false $eecon2 := 0x55 $eecon2 := 0xaa # Start the write: $wr := $true # Wait for write to complete: while $wr do_nothing # Disable writing: $wren := $false # Prepare the second byte of data: $eeadr := $eeadr + 1 $eedat := address ^ 0xff # 24-bit convert to from float code: procedure xyz argument high byte argument middle byte argument low byte returns float32 return_suppress assemble # Grab the high byte: movf xyz__high,w databank xyz,$float32_aargb0 movwf $float32_aargb0 # Grab the middle byte: databank $float32_aargb0,xyz movf xyz__middle,w databank xyz,$float32_aargb1 movwf $float32_aargb1 # Grab the low byte: databank $float32_aargb1,xyz movf xyz__low,w databank xyz,$float32_aargb2 movwf $float32_aargb2 # Do the conversion to float32: databank $float32_aargb2,$float32_from_integer24 codebank xyz,$float32_from_integer24 call $float32_from_integer24 # Result is in the float32 A "register"l not move to 0return: movlw xyz__0return >> 1 databank $float32_from_integer24,$float32_pointer_store codebank $float32_from_integer24,$float32_pointer_store call $float32_pointer_store # Get out code and data banks back home: databank $float32_pointer_store,xyz codebank $float32_pointer_store,xyz return # 24-bit convert to from float code: procedure zyx argument value float32 returns byte return_suppress assemble movlw zyx__value>>1 databank zyx,$float32_pointer_load codebank zyx,$float32_pointer_load call $float32_pointer_load databank $float32_pointer_load,$float32_integer24_convert codebank $float32_pointer_load,$float32_integer24_convert call $float32_integer24_convert codebank $float32_integer24_convert,zyx databank $float32_integer24_convert,$float32_aargb0 movf $float32_aargb0,w databank $float32_aargb0,high movwf high databank high,$float32_aargb1 movf $float32_aargb1,w databank $float32_aargb0,middle movwf middle databank middle,$float32_aargb2 movf $float32_aargb2,w databank $float32_aargb0,low movwf low databank low,zyx return string index2mask = "\1,2,4,8,16,32,64,128\" string id = "\16,0,21,1,3,13\MidiMotor1E-A\7\Gramson" # {state} is defined by another file: library states