ucl 1.0 # ############################################################################# # # Copyright (c) 2000-2004 by Wayne C. Gramlich & William T. Benson. # 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 the code that implements the DualMotor1Amp module. Basically # it just waits for commands that come in at 2400 baud and responds # to them. See: # # http://gramlich.net/projects/robobricks/dualmotor1amp/index.html # # for more details. # # ############################################################################# library $pic16f630 library clock4mhz library bit_bang package pdip pin 1 = power_supply pin 2 = ra5_unused pin 3 = ra4_out, name = motor1a, mask = motor1a_mask pin 4 = ra3_unused pin 5 = rc5_unused, name = debug_out pin 6 = rc4_unused pin 7 = rc3_out, name = motor1e pin 8 = rc2_out, name = motor0e pin 9 = rc1_out, name = serial_out pin 10 = rc0_out, name = serial_in pin 11 = ra2_out, name = motor1b, mask = motor1b_mask pin 12 = ra1_out, name = motor0b, mask = motor0b_mask pin 13 = ra0_out, name = motor0a, mask = motor0a_mask pin 14 = ground # Define duty cycle and motor on/off masks: global actual_speed0 byte global actual_speed1 byte global motor0_off byte global motor0_on byte global motor1_off byte global motor1_on byte # Ramp variables: global desired_speed0 byte global desired_speed1 byte global desired_direction0 bit global desired_direction1 bit global ramp0 byte global ramp1 byte global ramp0_delay byte global ramp1_delay byte global ramp0_offset byte global ramp1_offset byte # Fail safe variables: global fail_safe byte global fail_safe_errors byte global fail_safe_high_counter byte global fail_safe_low_counter byte # Second command stuff for ramped direction change: global second_motor0_command bit global second_motor1_command bit global second_desired_speed0 byte global second_desired_speed1 byte global second_ramp0_offset byte global second_ramp1_offset byte global second_motor0_on byte global second_motor1_on byte global second_motor0_off byte global second_motor1_off byte global motor0 byte global motor1 byte # Mode (pulsed vs. continuous) bits: global motor0_mode bit global motor1_mode bit global motor0_direction bit global motor1_direction bit # Shared command registers and option: global glitch byte global id_index byte global spare byte global command_previous byte global command_last byte global sent_previous byte global sent_last byte procedure main arguments_none returns_nothing local command byte local temp byte call reset() # Loop waiting for commands: loop_forever # Get a command byte: command := byte_get() # Dispatch on command: switch command >> 6 case 0 # Set Quick (Command = 00hh hhdm): temp := ((command << 2) & 0xf0) | (command >> 2) if command@0 # Motor : desired_speed1 := temp desired_direction1 := command@1 else desired_speed0 := temp desired_direction0 := command@1 call set_up() case 1 # Set Low (Command = 01ll lldm): temp := (command >> 2) & 0xf if command@0 # Motor 1: desired_speed1 := desired_speed1 & 0xf0 | temp desired_direction1 := command@1 else # Motor 0: desired_speed0 := desired_speed0 & 0xf0 | temp desired_direction0 := command@1 call set_up() case 2 # Command = 10xx xxxx: switch (command >> 3) & 7 case 0 # Command = 1000 0xxx: switch command & 7 case 0, 1 # Set Ramp (Command = 1000 000m): temp := byte_get() if command@0 ramp1 := temp else ramp0 := temp call set_up() case 2 # Set Failsafe (Command = 1000 0010): fail_safe := byte_get() fail_safe_high_counter := fail_safe fail_safe_low_counter := 0 case 3 # Reset Failsafe (Command = 1000 0011): fail_safe_high_counter := fail_safe fail_safe_low_counter := 0 case 4, 5, 6, 7 # Set Speed (Command = 1000 01dm): temp := byte_get() if command@0 # Motor 1: desired_speed1 := temp desired_direction1 := command@1 else # Motor 0: desired_speed0 := temp desired_direction0 := command@1 call set_up() case 1 # Command = 1000 1xxx: if command@2 # Set direction (Command = 1000 11dm): if command@0 # Motor 1: desired_direction1 := command@1 else # Motor 0: desired_direction0 := command@1 else # Set mode (Command = 1000 10xm): if command@0 # Motor 1: motor1_mode := command@1 else # Motor 0: motor0_mode := command@1 call set_up() case 2 # Set Prescaler (Command = 1001 0ppp): $option_reg := command & 7 $rapu := 1 case 3 # Command = 1001 1xxx: switch command & 7 case 0 # Read Failsafe (Command = 1001 1000): call byte_put(fail_safe) case 1 # Read Prescaler (Command = 1001 1001): call byte_put($option_reg & 7) case 2, 3 # Read Speed (Command = 1001 101m): if command@0 call byte_put(actual_speed1) else call byte_put(actual_speed0) case 4, 5 # Read Mode/Direction (Command = 1001 110m): temp := 0 if command@0 # Motor 1: if motor1_direction temp@1 := 1 if motor1_mode temp@0 := 1 else # Motor 0: if motor0_direction temp@1 := 1 if motor0_mode temp@0 := 1 call byte_put(temp) case 6, 7 # Read Ramp (Command = 1001 101m): if command@0 temp := ramp1 else temp := ramp0 call byte_put(temp) case 4 # Command = 0110 0xxx: switch command & 7 case 0 # Read Failsafe Errors (Command = 1010 0000): call byte_put(fail_safe_errors) fail_safe_errors := 0 case 1 # Read Failsafe Counter (Command = 1010 0001): call byte_put(fail_safe_high_counter) case 2 # Read Actual Speed 0(Command = 1010 0010): call byte_put(actual_speed0) case 3 # Read Actual Speed 1 (Command = 1010 0011): call byte_put(actual_speed1) case 4 # Set Motor 0 off (Command = 1010 0100): motor0e := 0 case 5 # Set Motor 0 on (Command = 1010 0101): motor0e := 1 case 6 # Set Motor 1 off (Command = 1010 0110): motor1e := 0 case 7 # Set Motor 1 on (Command = 1010 0111): motor1e := 1 case 5 if command & 3 = 0 # FIXME: Code generator chokes on single call instruction # in the then clause. Add 'ramp0 := 0' to work around!!! ramp0 := 0 call reset() case 6, 7 do_nothing case 3 # Command = 11xx xxxx: switch (command >> 3) & 7 case 7 # Shared commands (Command = 1111 1ccc): switch command & 7 case 0 # Clock Decrement (Command = 1111 1000): $osccal := $osccal - $osccal_lsb case 1 # Clock Increment (Command = 1111 1001): $osccal := $osccal + $osccal_lsb case 2 # Clock Read (Command = 1111 1010): call byte_put($osccal) case 3 # Clock Pulse (Command = 1111 1011): call byte_put(0) case 4 # ID Next (Command = 1111 1100): temp := 0 if id_index < id.size temp := id[id_index] id_index := id_index + 1 call byte_put(temp) case 5 # ID Reset (Command = 1111 1101): id_index := 0 case 6 # Glitch Read (Command = 1111 1110): call byte_put(glitch) glitch := 0 case 7 # Glitch (Command = 1111 1111): if glitch != 0xff glitch := glitch + 1 procedure set_up arguments_none returns_nothing # This procedure will arrange for the speed and direction of # each motor to be set to desired_speed0/1 and desired_direction0/1. # If ramp0/1 is 0, the speed and direction is changed immediately. # If ramp0/1 is non-zero, the speed is changed gradually. local temporary byte # Reset failsafe: fail_safe_low_counter := 0 fail_safe_high_counter := fail_safe # Mode Dir On Off # ================== # 0 0 A 0 # 0 1 B 0 # 1 0 A B # 1 1 B A # Motor 0: # Figure out all the ramping stuff: if ramp0 = 0 # No ramping: actual_speed0 := desired_speed0 motor0_direction := desired_direction0 ramp0_delay := 0 ramp0_offset := 0 else # We are ramping: # Figure out if we are changing direction: temporary := 0 if motor0_direction temporary@0 := 1 if desired_direction0 temporary := temporary ^ 1 if temporary@0 # We are changing direction: second_ramp0_offset := 1 second_desired_speed0 := desired_speed0 second_motor0_command := 1 desired_speed0 := 0 ramp0_offset := 0xff else # Direction remains unchanged if desired_speed0 < actual_speed0 ramp0_offset := 0xff else ramp0_offset := 1 # Figure out all the direction stuff: motor0_off := 0 second_motor0_off := 0 if motor0_direction # Direction = 1 (Backward): if motor0_mode # Mode = 1 (Continuous): motor0_off := motor0a_mask second_motor0_off := motor0b_mask motor0_on := motor0b_mask second_motor0_on := motor0a_mask else # Direction = 0 (Forward): if motor0_mode # Mode = 1 (Continuous): motor0_off := motor0b_mask second_motor0_off := motor0a_mask motor0_on := motor0a_mask second_motor0_on := motor0b_mask # Motor 1: if ramp1 = 0 # No ramping: actual_speed1 := desired_speed1 motor1_direction := desired_direction1 ramp1_delay := 0 ramp1_offset := 0 else # We are ramping: # Figure out if we are changing direction: temporary := 0 if motor1_direction temporary@0 := 1 if desired_direction1 temporary := temporary ^ 1 if temporary@0 # We are changing direction: second_ramp1_offset := 1 second_desired_speed1 := desired_speed1 second_motor1_command := 1 desired_speed1 := 0 ramp1_offset := 0xff else # We are not changing direction: if desired_speed1 < actual_speed1 ramp1_offset := 0xff else ramp1_offset := 1 motor1_off := 0 second_motor1_off := 0 if motor1_direction # Direction = 1 (Backward): if motor1_mode # Mode = 1 (Continuous): motor1_off := motor1a_mask second_motor1_off := motor1b_mask motor1_on := motor1b_mask second_motor1_on := motor1a_mask else # Direction = 0 (Forward): if motor1_mode # Mode = 1 (Continuous): motor1_off := motor1b_mask second_motor1_off := motor1a_mask motor1_on := motor1a_mask second_motor1_on := motor1b_mask procedure reset arguments_none returns_nothing # Initialize everything else: motor0e := 1 motor1e := 1 $intcon := 0 $option_reg := 0 $wpua := 0 actual_speed0 := 0 actual_speed1 := 0 motor0_off := 0 motor0_on := 0 motor1_off := 0 motor1_on := 0 desired_direction0 := 0 desired_direction1 := 0 desired_speed0 := 0 desired_speed1 := 0 ramp0 := 0 ramp1 := 0 ramp0_delay := 0 ramp1_delay := 0 ramp0_offset := 0 ramp1_offset := 0 motor0_direction := 0 motor1_direction := 0 motor0_mode := 0 motor1_mode := 0 second_motor0_command := 0 second_motor1_command := 0 fail_safe := 0 fail_safe_errors := 0 fail_safe_high_counter := 0 fail_safe_low_counter := 0 glitch := 0 id_index := 0 procedure delay arguments_none returns_nothing exact_delay delay_instructions # Delay for 1/3 of a bit time. # Kick the dog: watch_dog_reset # This is the first probe of TMR0: if $tmr0 < actual_speed0 motor0 := motor0_on else motor0 := motor0_off if $tmr0 < actual_speed1 motor1 := motor1_on else motor1 := motor1_off $porta := motor0 | motor1 # First check out {fail_safe_counter}: fail_safe_low_counter := fail_safe_low_counter - 1 if $z fail_safe_high_counter := fail_safe_high_counter - 1 if $z if fail_safe != 0 # Turn the motors off: motor0_on := 0 motor0_off := 0 motor1_on := 0 motor1_off := 0 desired_speed0 := 0 desired_speed1 := 0 actual_speed0 := 0 actual_speed1 := 0 fail_safe_errors := fail_safe_errors + 1 # This is the second probe of TMR0: if $tmr0 < actual_speed0 motor0 := motor0_on else motor0 := motor0_off if $tmr0 < actual_speed1 motor1 := motor1_on else motor1 := motor1_off $porta := motor0 | motor1 # Do {ramp0} management: ramp0_delay := ramp0_delay - 1 if $z ramp0_delay := ramp0 if actual_speed0 != desired_speed0 actual_speed0 := actual_speed0 + ramp0_offset else_if second_motor0_command second_motor0_command := 0 desired_speed0 := second_desired_speed0 ramp0_offset := second_ramp0_offset motor0_on := second_motor0_on motor0_off := second_motor0_off motor0_direction := desired_direction0 # This is the third probe of TMR0: if $tmr0 < actual_speed0 motor0 := motor0_on else motor0 := motor0_off if $tmr0 < actual_speed1 motor1 := motor1_on else motor1 := motor1_off $porta := motor0 | motor1 # Do {ramp1} management: ramp1_delay := ramp1_delay - 1 if $z ramp1_delay := ramp1 if actual_speed1 != desired_speed1 actual_speed1 := actual_speed1 + ramp1_offset else_if second_motor1_command second_motor1_command := 0 desired_speed1 := second_desired_speed1 ramp1_offset := second_ramp1_offset motor1_on := second_motor1_on motor1_off := second_motor1_off motor1_direction := desired_direction1 else # This is the forth probe of TMR0: if $tmr0 < actual_speed0 motor0 := motor0_on else motor0 := motor0_off if $tmr0 < actual_speed1 motor1 := motor1_on else motor1 := motor1_off $porta := motor0 | motor1 constant zero8 = "\0,0,0,0,0,0,0,0\" constant module_name = "\13\DualMotor1Amp" constant vendor_name = "\13\Mondo-tronics" string id = "\1,0,14,2,0,0,0,0\" ~ zero8 ~ zero8 ~ module_name ~ vendor_name