ucl 1.0 # Copyright (c) 2000-2004 by Wayne C. Gramlich & William T. Benson. # All rights reserved. library $robobricks_pic16f688 package pdip pin 1 = power_supply pin 2 = ra5_in, name = debug pin 3 = ra4_out, name = motor0b, mask = motor0b_mask pin 4 = ra3_nc pin 5 = rx pin 6 = tx pin 7 = rc3_out, name = enable pin 8 = rc2_out, name = led3 pin 9 = rc1_out, name = led2 pin 10 = rc0_out, name = led1 pin 11 = ra2_out, name = motor1b, mask = motor1b_mask pin 12 = ra1_out, name = motor1a, mask = motor1a_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 global interrupt_pending bit procedure main arguments_none returns_nothing local command byte local temp 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 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 Motors off (Command = 1010 0100): enable := 0 led2 := 0 case 5 # Set Motors on (Command = 1010 0101): enable := 1 led2 := 1 case 6 # Read Motor enable (Command = 1010 0110): temp := 0 if enable temp@0 := 1 call byte_put(temp) case 7 # Unused (Command = 1010 0111): do_nothing 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): $osctune := $osctune - $osccal_lsb case 1 # Clock Increment (Command = 1111 1001): $osctune := $osctune + $osccal_lsb case 2 # Clock Read (Command = 1111 1010): call byte_put($osctune) 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: enable := 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 led1 := 0 led2 := 1 led3 := 0 procedure delay arguments_none returns_nothing #exact_delay delay_instructions local count1 byte local count2 byte # Delay for 1/3 of a bit time. # Kick the dog: watch_dog_reset # Keep count of everything: count1 := count1 + 1 if $z count2 := count2 + 1 # 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 # Light direction LED's: if motor0_direction # Reverse if count2@3 led1 := 1 else led1 := 0 else if actual_speed0 = 0 led1 := 0 else led1 := 1 if motor1_direction # Reverse if count2@3 led3 := 1 else led3 := 0 else if actual_speed1 = 0 led3 := 0 else led3 := 1 constant zero8 = "\0,0,0,0,0,0,0,0\" constant module_name = "\13\DualMotor1Amp" constant vendor_name = "\7\Gramson" string id = "\1,1,14,8,9,0,0,0\" ~ zero8 ~ zero8 ~ module_name ~ vendor_name