# ############################################################################# # # Copyright (c) 1999-2001 by Wayne C. Gramlich. # All rights reserved. # # Permission to use, copy, modify, distribute, and sell this software # for any purpose is hereby granted without fee provided that the above # copyright notice and this permission are retained. The author makes # no representations about the suitability of this software for any purpose. # It is provided "as is" without express or implied warranty. # # This is a test harness for testing the Motor2 RoboBrick. See: # # http://web.gramlich.net/projects/robobricks/motor2/index.html # # for more details. # # ############################################################################# processor pic16f628 cp=off cpd=off lvp=off bowden=off mclre=on pwrte=off wdte=off fosc=xt constant clock_rate 10000000 constant clock_ticks_per_instruction 4 constant instructions_per_second clock_rate / clock_ticks_per_instruction constant baud_rate 2400 constant instructions_per_bit clock_rate / (clock_ticks_per_instruction * baud_rate) constant delays_per_bit 3 constant instructions_per_delay instructions_per_bit / delays_per_bit constant extra_instructions_per_bit 12 constant extra_instructions_per_delay extra_instructions_per_bit / delays_per_bit constant delay_instructions instructions_per_delay - extra_instructions_per_delay # The null pulse that comes back from a clock pulse command is supposed to be # exactly 9 bits long. 9 bits at 2400 baud is 9/2400 = 3.75mS. The number # iterations through the loop is 3.75mS / (number of instructions per iteration.) constant nine_bits_instructions (clock_rate * 9) / (clock_ticks_per_instruction * baud_rate) constant instructions_per_iteration 7 constant iterations_for_nine_bits nine_bits_instructions / instructions_per_iteration constant iterations_high iterations_for_nine_bits / 256 constant iterations_low iterations_for_nine_bits - (iterations_high * 256) # Some character constants: constant sp 32 constant cr 13 constant lf 10 # Some bit definitions: constant rx_slave_bit 5 constant tx_slave_bit 4 constant rx_master_bit 1 constant tx_master_bit 2 constant rx_slave_mask 1 << rx_slave_bit constant tx_slave_mask 1 << tx_slave_bit constant rx_master_mask 1 << rx_master_bit constant tx_master_mask 1 << tx_master_bit # Somre register and pin defintions: register status 3 bind c status@0 bind z status@2 # Some port and pin definitions: port porta a unused none port portb b bits_only read_write_static pin tx_master_pin portb tx_master_bit write_only pin rx_master_pin portb rx_master_bit read_only pin tx_slave_pin portb tx_slave_bit write_only pin rx_slave_pin portb rx_slave_bit read_only pin heart porta 4 write_only # Miscellaneous definitions: constant space 0xff constant buffer_size 5 string_constants { clock_string = 0s'Clk' common_string = 0s'Common' direction_string = 0s'Dir' done_string = 0s'Done' fail_string = 0s'Fail' failsafe_string = 0s'F/S' hello_string = 0s'Motor2_Test' mode_string = 0s'Mode' mode_dir_string = 0s'M/D' motor_string = 0s'Motor' ramps = 0, 1, 0x10, 0x80, 0xff ramp_string = 0s'Ramp' scaler_string = 0s'Scaler' speeds = 0x10, 0x20, 0x40, 0x80, 0xff, 0 speed_string = 0s'Speed' } procedure main { arguments_none returns_nothing # Read a byte. variable buffer[buffer_size] byte variable char byte variable count byte variable index byte variable number byte # Print out a welcome message: call master_crlf() call master_string(hello_string) call master_crlf() # Main loop number := 0 loop_forever { # Get a character: tx_slave_pin := 1 char := master_get() # Delay 2/3's of bit make sure that get_byte is done. call delay() call delay() if (0c'0' <= char && char < (0c'9' + 1)) { # Do a multiply by 8 then add in digit: call master_send(char) number := (number << 3) + char - 0c'0' } else_if (char = 0c's') { # Send byte to brick, no wait: # Echo command and send CRLF: call master_send(char) call master_crlf() # Ship the byte down to the brick: call slave_send(number) number := 0 } else_if (char = 0c'w') { # Send byte to brick, wait for results: # Echo command and send CRLF: call master_send(char) call master_crlf() # Ship the byte down to the brick ... call slave_send(number) number := 0 #call delay() # ...and wait for a response: index := 0 while (index < buffer_size) { buffer[index] := slave_get() index := index + 1 } index := 0 while (index < buffer_size) { call master_octal(buffer[index]) index := index + 1 } # Terminate the output list call master_crlf() } else_if (char = 0c'i') { # Interrogate the slave RoboBrick: call master_send(char) call master_crlf() # Initialize the id index: call slave_send(0xfd) # Get the first 8 bytes: call slave_id8() # Get the next 8 bytes of random numbers: call slave_id8() # Get the next 8 bytes of random numbers: call slave_id8() # Get the slave brick name: call slave_id_string() # Get the vendor name: call slave_id_string() } else_if (char = 0c't') { # Do testing: call master_send(char) call master_crlf() call test(number) number := 0 } else_if (char = 0c'c') { call master_send(char) call master_crlf() call clock_adjust(number) number := 0 } else_if (char != 0xfc) { # Just echo back the current number: call master_send(0c'<') call master_octal(char) call master_send(0c'>') # Send a carriage-return line-feed: call master_crlf() # Output the character in octal: call master_octal(number) # Send a carriage-return line-feed: call master_crlf() # Reset number number := 0 } } } procedure clock_adjust { argument adjust byte returns_nothing # This procedure will adjust the clock to the slave. variable command byte variable count byte variable error byte variable error_minimum byte variable high byte variable low byte variable target byte target := iterations_low - adjust count := 2 error := 0xff error_minimum := 0xf0 # Print out the target: call master_send(0c'T') call master_octal(iterations_high) call master_octal(target) call master_crlf() loop_forever { # Print out the clock value: call master_send(0c'C') call slave_send(0xfa) call master_octal(slave_get()) # Ask for a timing byte: call slave_send(0xfb) low := 0 high := 0 while (rx_slave_pin) { # Do nothing: } while (!rx_slave_pin) { low := low + 1 if (z) { high := high + 1 } } # Print out high and low: call master_send(0c'H') call master_octal(high) call master_send(0c'L') call master_octal(low) # Now think about adjusting clock. if (high > iterations_high) { # Clock pulse is too long; slave clock is too slow: command := 0xf9 error := 0xff - target } else_if (high < iterations_high) { # Clock pulse is too short; slave clock is too fast: command := 0xf8 error := target } else { # The high 8-bits are equal: if (low > target) { # Clock pulse is too long; slave clock is too slow: command := 0xf9 error := low - target } else_if (low < target) { # Clock pulse is too short; slave clock is too fast: command := 0xf8 error := target - low } else { # Exact match; we are done: command := 0 error := 0 error_minimum := 0 } } # Print out the error and error minimum: call master_send(0c'E') call master_octal(error) call master_send(0c'M') call master_octal(error_minimum) if (error = error_minimum) { call master_crlf() return } else_if (error < error_minimum) { error_minimum := error error := error + 1 } else { count := count - 1 if (z) { error_minimum := error_minimum + 1 count := 2 } } # Now adjust the clock: call slave_send(command) call master_crlf() } } procedure test { argument test_mask byte returns_nothing # This procedure will test the Motor2 RoboBrick: variable counter1 byte variable counter2 byte variable direction byte variable failsafe byte variable mode byte variable motor byte variable prescaler byte variable ramp byte variable ramp_index byte variable speed_index byte variable speed byte variable temp byte if (test_mask = 0) { test_mask := 0xff } # Verify that the common commands are working: if (test_mask & 1 != 0) { call common_test() } # Reset everything: call slave_send(0xa8) # Test with prescaler: if (test_mask & 2 != 0) { call master_string(scaler_string) call master_crlf() prescaler := 0 while (prescaler <= 7) { # Set Prescaler: call slave_send(0x90 | prescaler) # Read prescaler: call slave_send(0x99) temp := slave_get() if (temp != prescaler) { call master_octal(temp) call master_fail(scaler_string, 0x30) } prescaler := prescaler + 1 } } # Reset the prescaler to 5: call slave_send(0x95) # Test failsafe: if (test_mask & 4 != 0) { call master_string(failsafe_string) call master_crlf() # Set failsafe: call slave_send(0x82) call slave_send(0xff) # Set speed: call slave_send(0x3c) # Wait for failsafe to trigger: failsafe := 0xff while (failsafe != 0) { call slave_send(0xa1) failsafe := slave_get() } # Read failsafe error counter: call slave_send(0xa0) temp := slave_get() if (temp != 1) { call master_octal(temp) call master_fail(failsafe_string, 0x37) } # Verify that failsafe error counter reset: call slave_send(0xa0) temp := slave_get() if (temp != 0) { call master_octal(temp) call master_fail(failsafe_string, 0x38) } } # Reset everything: call slave_send(0xa8) # Do some ramp testing: if (test_mask & 8 != 0) { ramp_index := 0 while (ramp_index < ramps.size) { #call master_send(0c'R') ramp := ramps[ramp_index] mode := 0 while (mode < 2) { #call master_send(0c'M') direction := 0 while (direction < 2) { #call master_send(0c'D') motor := 0 while (motor < 2) { #call master_send(0c'S') call master_string(ramp_string) call master_octal(ramp) call master_string(mode_string) call master_octal(mode) call master_string(direction_string) call master_octal(direction) call master_string(motor_string) call master_octal(motor) call master_crlf() # Set the mode: call slave_send(0x88 | (mode << 1) | motor) # Set the ramp: call slave_send(0x80 | motor) call slave_send(ramp) # Verify the ramp: call slave_send(0x9e | motor) temp := slave_get() if (temp != ramp) { call master_octal(temp) call master_fail(ramp_string, 0x31) } # Do a complete speed check when the ramp is zero: if (ramp = 0) { # Ramp motor up through speeds: speed := 0 while (speed < 255) { # Set speed: temp := (direction << 1) | motor if (speed >> 4 = speed & 0xf) { # Set quick (only): call slave_send(((speed >> 2) & 0x3c) | temp) } else_if (speed & 0xf = 0) { # Set high (set quick followed by set low): call slave_send((speed >> 2) | temp) call slave_send(0x40 | temp) } else { # Set low: call slave_send(0x40 | (speed << 2) & 0x3c | temp) } # Read and verify speed: call slave_send(0x9a | motor) temp := slave_get() if (speed != temp) { call master_octal(speed) call master_octal(temp) call master_fail(speed_string, 0x32) } speed := speed + 1 } # Ramp motor down through speeds: while (speed != 0) { # Set speed: call slave_send(0x84 | (direction << 1) | motor) call slave_send(speed) # Read and verify speed: call slave_send(0x9a | motor) temp := slave_get() if (speed != temp) { call master_octal(speed) call master_octal(temp) call master_fail(speed_string, 0x33) } speed := speed - 1 } } else { # Do ramp testing: speed_index := 0 while (speed_index < speeds.size) { #call master_send(0c'S') # Set the speed: speed := speeds[speed_index] call slave_send(0x84 | (direction << 1) | motor) call slave_send(speed) # Now wait for the speed to ramp up: temp := speed - 1 count_down counter1 8 { count_down counter2 255 { # Read actual speed: if (temp != speed) { call slave_send(0xa2 | motor) temp := slave_get() } } } # See whether we timed out: if (temp != speed) { call master_octal(temp) call master_octal(speed) call master_fail(ramp_string, 0x34) } speed_index := speed_index + 1 } } motor := motor + 1 } direction := direction + 1 } mode := mode + 1 } ramp_index := ramp_index + 1 } } # Reset everything: call slave_send(0xa8) # Announce end of test run: call master_string(done_string) call master_crlf() } # The procedures below are used to test the common shared commands: procedure common_test { arguments_none returns_nothing # This procedure will verify that the common shared commands work. variable actual byte # Print the ID information: # ID reset: call slave_send(0xfd) # Read the fixed bytes: call common_test_id_match(1, 0xc0) call common_test_id_match(0, 0xc1) call common_test_id_match(14, 0xc2) call common_test_id_match(2, 0xc3) call common_test_id_match(0, 0xc4) call common_test_id_match(0, 0xc5) call common_test_id_match(0, 0xc6) call common_test_id_match(0, 0xc7) call master_crlf() call common_test_id_bytes8() call common_test_id_bytes8() # Read the brick name: call common_test_id_string() # Read the vendor name: call common_test_id_string() # Verfify that we don't read off the end of the id and crash: call common_test_id_next() # Reset id: call slave_send(0xfd) # Verify that we are still alive: call common_test_id_match(1, 0xc8) # Read glitch: # Clear glitch register: call slave_send(0xfe) actual := slave_get() # Send a couple of glitches: call slave_send(0xff) call slave_send(0xff) # Read the glitch register: call slave_send(0xfe) if (slave_get() != 2) { call master_fail(common_string, 0xc9) } # Do a clock pulse: call slave_send(0xfb) if (slave_get() != 0) { call master_fail(common_string, 0xca) } # Read clock: call slave_send(0xfa) actual := slave_get() # Increment: call slave_send(0xf9) # Decrement: call slave_send(0xf8) # Read clock again: call slave_send(0xfa) if (actual != slave_get()) { call master_fail(common_string, 0xcb) } } procedure common_test_id_match { argument desired byte argument test_number byte returns_nothing # This procedure will verify that the next byte in the id is {desired}. if (desired != common_test_id_next()) { call master_fail(common_string, test_number) } } procedure common_test_id_bytes8 { arguments_none returns_nothing # This procedure will print out the next 8 bytes of the id in octal. variable count byte count_down count 8 { call master_octal(common_test_id_next()) } call master_crlf() } procedure common_test_id_string { arguments_none returns_nothing # This procedure will print out the next id string. variable count byte count_down count (common_test_id_next()) { call master_send(common_test_id_next()) } call master_crlf() } procedure common_test_id_next { arguments_none returns byte # This procedure returns the next byte from the identification string. call slave_send(0xfc) return slave_get() } # The following procedures are used to communicate with the master: procedure master_crlf { arguments_none returns_nothing # This procedure will output a carriage-return line-feed # to the master. call master_send(cr) call master_send(lf) } procedure master_fail { argument test_name string argument test_number byte returns_nothing # This procedure will output `fail' followed by a carriage return # and line feed. call master_string(fail_string) call master_string(test_name) call master_octal(test_number) call master_crlf() } procedure master_get { arguments_none returns byte # This procedure will get the next byte or return 0xfc if # no byte is forthcoming. return get_byte(rx_master_mask) } procedure master_octal { argument number byte returns_nothing # This procedure will output {number} in octal to the tx port. # Output the character in octal: call master_send((number>>6) + 0c'0') call master_send(((number>>3) & 7) + 0c'0') call master_send((number & 7) + 0c'0') call master_send(sp) } procedure slave_get { arguments_none returns byte # This procedure will get a byte from the slave or return 0xfc # if no byte is forthcoming. return get_byte(rx_slave_mask) } procedure slave_id8 { arguments_none returns_nothing # This procedure will print out the next 8 bytes of data from # the slave RoboBrick id bytes. variable counter byte count_down counter 8 { call master_octal(slave_id_next()) } call master_crlf() } procedure slave_id_next { arguments_none returns byte # This procedure will return the next byte from the id bytes. call slave_send(0xfc) return slave_get() } procedure slave_id_string { arguments_none returns_nothing # This procedure will return the string for the id bytes. variable size byte size := slave_id_next() call master_octal(size) count_down size size { call master_send(slave_id_next()) } call master_crlf() } procedure slave_send { argument data byte returns_nothing # This procedure will send one byte of {data} to the slave. call send_byte(data, tx_slave_mask) } # The last procedures do character sending and receiving: procedure get_byte { argument mask byte returns byte # Get an 8-bit byte from {mask} bit of {portb} and return it. # If no character shows up in a while 0xfc is returned. variable count byte variable char byte # Wait until a start bit arrives: count := 0 while (portb & mask != 0) { count := count - 1 if (count = 0) { return 0xfc } call delay() } # Skip over the start bit: call delay() call delay() call delay() # Sample in the middle third of each data bit: char := 0 count_down count 8 { call delay() char := char >> 1 if (portb & mask != 0) { char := char | 0x80 } call delay() call delay() } # Skip over 1/3 of the stop bit: call delay() return char } procedure send_byte { argument char byte argument mask byte returns_nothing # Send {char} to {mask} bit of {portb}. variable count byte variable mark byte # Send the start bit: mark := mask ^ space portb := mark call delay() call delay() call delay() # Send the data: count_down count 8 { if (char@0) { portb := space } else { portb := mark } char := char >> 1 call delay() call delay() call delay() } # Send the stop bit: portb := space call delay() call delay() call delay() } procedure master_send { argument character byte returns_nothing # This procedure will output {character} to the master. call send_byte(character, tx_master_mask) } procedure master_string { argument message string returns_nothing # This procedure will output {message} to the master. variable size byte variable index byte index := 0 while (index < message.size) { call master_send(message[index]) index := index + 1 } call master_send(sp) } procedure delay { arguments_none returns_nothing uniform_delay instructions_per_delay # Delay 1 third of a bit: }