ucl 2.0

# Copyright (c) 2006 by Wayne C. Gramlich.
# All rights reserved.

# This is a boot loader that resides in high memory for
# loading programs into low memory.

# This module is using a PIC16F767
#library $pic16f767
library $pic16f876

# The resonator is running at 16MHz:
library clock16mhz
constant microsecond = 4

# The libary of bus access routines for use by the PIC16F767:
#library rb2bus_pic16f767
library rb2bus_pic16f876

# Make sure we get the bus access routines:
library rb2bus

# This module uses a 16Mhz resonator; hence mode HS=High Speed:
configure fosc=hs

# Only the RX and TX pins on this package are used:
package pdip
    pin 1 = mclr
    pin 2 = ra0_unused
    pin 3 = ra1_unused
    pin 4 = ra2_unused
    pin 5 = ra3_unused
    pin 6 = ra4_unused
    pin 7 = ra5_unused
    pin 8 = ground
    pin 9 = osc1
    pin 10 = osc2
    pin 11 = rc0_unused
    pin 12 = rc1_unused
    pin 13 = rc2_unused
    pin 14 = rc3_unused
    pin 15 = rc4_unused
    pin 16 = rc5_unused
    pin 17 = tx
    pin 18 = rx
    pin 19 = ground2
    pin 20 = power_supply
    pin 21 = rb0_unused
    pin 22 = rb1_unused
    pin 23 = rb2_unused
    pin 24 = rb3_unused
    pin 25 = rb4_unused
    pin 26 = rb5_unused
    pin 27 = rb6_unused
    pin 28 = rb7_unused

data_bank 0
library_bank 0
library $signed16

data_bank 0

origin 0

procedure start
    arguments_none
    returns_nothing
    return_suppress

    assemble
	codebank start, main
	goto main

origin 4

procedure interrupt
    arguments_none
    returns_nothing

    # An interrupt soaker.

    do_nothing


procedure main
    arguments_none
    returns_nothing

    local command byte
    local id_index byte

    $gie := $false
    call rb2bus_initialize(28)

    id_index := 0

    loop_forever
	# Make sure that we have been selected:
	rb2bus_error := $true
	while rb2bus_error
	    call rb2bus_select_wait()
	    command := rb2bus_byte_get()

	switch command >> 6
	  case 2
	    # 10xx xxxx:
	    switch (command >> 3) & 7
	      case 0
		# 1000 0xxx:
		switch command & 7
		  case 0
		    # 1000 0000 (Run Program)
		    call program2()
		  case_maximum 7
	      case_maximum 7
	  case 3
	    # 11xx xxxx:
	    switch (command >> 3) & 7
	      case 7
		# 1111 1xxx:
		switch command & 7
		  case 5
		    # 1111 1101 (Id_next):
		    if id_index < id.size
			call rb2bus_byte_put(id[id_index])
			id_index := id_index + 1
		  case 6
		    # 1111 1110 (Id_start):
		    id_index := 0
		  case 7
		    # 1111 1111 (Deselect):
		    call rb2bus_deselect()

procedure wait
    arguments_none
    returns_nothing

    # This procedure is repeatably called whenever the software
    # is waiting for a byte to arrive from the bus.

    do_nothing


#procedure program
#    arguments_none
#    returns_nothing
#
#    # This is a quick little program to copy the contents of
#    # A/D channel 0 on the IO8 to the motor 0 speed on the
#    # MiniMotor2.
#
#    # Initialize the LCD32:
#
#    $gie := $false
#
#    call rb2bus_address_put(rb2_lcd32_address)
#    call rb2bus_byte_put(rb2_lcd32_form_feed)
#
#    call rb2bus_address_put(rb2_lcd32_address + 1)
#    call rb2bus_byte_put(rb2_lcd32_form_feed)
#
#    loop_forever
#	call rb2bus_address_put(rb2_shaft2_address)
#	call rb2bus_byte_put(rb2_shaft2_navigation_latch)
#
#	call number_show('X', rb2_shaft2_x_get, 0, 0, 0)
#	call number_show('Y', rb2_shaft2_y_get, 0, 0, 8)
#
#	call number_show('B', rb2_shaft2_bearing16_get, 1, 0, 0)
#
#	call number_show('b', rb2_shaft2_target_bearing16_get, 1, 1, 0)
#	call number_show('d', rb2_shaft2_target_distance_get, 1, 1, 8)
#
#	call delay_ms(200)
#
#	do_nothing


procedure number_show
    argument label byte
    argument command byte
    argument lcd byte
    argument row byte
    argument column byte
    returns_nothing

    # This routine will show the number obtained via {command} from
    # the Shaft5 module starting a {row} and {column} on the LCD32
    # module.  {label} is the label to be displayed.

    local high byte
    local low byte

    call rb2bus_address_put(rb2_shaft2_address)
    call rb2bus_byte_put(command)
    high := rb2bus_byte_get()
    low := rb2bus_byte_get()

    call rb2bus_address_put(rb2_lcd32_address + lcd)
    call rb2bus_byte_put(rb2_lcd32_row_set | row)
    call rb2bus_byte_put(rb2_lcd32_column_set | column)
    call rb2bus_byte_put(label)
    call rb2bus_byte_put(':')
    if high@7
	# Negative number:
	low := low ^ 0xff
	high := high ^ 0xff
	low := low + 1
	if low = 0
	    high := high + 1
	call rb2bus_byte_put('-')
    else
	# Positive number
	call rb2bus_byte_put('+')
    call rb2bus_hex_put(high)
    call rb2bus_hex_put(low)

    
string hex = "0123456789abcdef"

procedure rb2bus_hex_put
    argument data byte
    returns_nothing

    call rb2bus_byte_put(hex[data >> 4])
    call rb2bus_byte_put(hex[data & 0xf])


procedure rb2bus_address_put
    argument address byte
    returns_nothing

    # This procedure will select the module that matches {address}.

    local result byte

    # Wait until {$txreg} is ready for a value:
    while !$txif
	call wait()

    # Ship the address select byte:
    $adden := $false
    $tx9d := $true
    $txreg := address

    loop_exactly 2
	while !$rcif
	    do_nothing
	result := $rcreg
	if $oerr
	    $cren := $false
	if $ferr
	    $cren := $false
	$cren := $true


procedure delay_ms
    argument ms byte
    returns_nothing

    # This procedure will delay {ms} milliseconds.

    loop_exactly ms
	loop_exactly 100
	    delay 10*microsecond
		do_nothing


string id = "\16,0,28,3,3,14\Controller28-D\7\Gramson"

data_bank 1

procedure program2
    arguments_none
    returns_nothing

    # This is a quick little program to copy the contents of
    # A/D channel 0 on the IO8 to the motor 0 speed on the
    # MiniMotor2.

    # Initialize the LCD32:

    #local actual_left signed16
    #local actual_right signed16
    local bearing_absolute signed16
    local bearing_gain signed16
    local bearing signed16
    local count_current byte
    local count_previous byte
    local computed_left signed16
    local computed_right signed16
    local distance signed16
    local distance_gain signed16
    local distance_slow_down signed16
    local distance_stop signed16
    local forward signed16
    local forward_fast signed16
    local limit_positive signed16
    local limit_negative signed16
    local motor_left byte
    local motor_right byte
    local motor_adjust signed16
    local turn_limit signed16
    local zero signed16 
    local x signed16
    local y signed16
    local five signed16
    local corner byte
    local corner_previous byte
    local x_high byte
    local y_high byte
    local degree_360 signed16
    local degree_180 signed16
    local minus_degree_180 signed16

    $gie := $false

    call rb2bus_address_put(rb2_lcd32_address)
    call rb2bus_byte_put(rb2_lcd32_form_feed)

    limit_positive := $signed16_from_byte(127)
    limit_negative := -limit_positive
    bearing_gain := $signed16_from_byte(0x10)
    zero := $signed16_from_byte(0)
    turn_limit := $signed16_from_byte(20)
    motor_adjust := $signed16_from_byte(10)

    #actual_left := zero
    #actual_right := zero

    degree_360 := $signed16_from_byte2(1, 104)	# 360 = 256 + 104
    degree_180 := $signed16_from_byte2(0, 180)	# 180
    minus_degree_180 := -degree_180

    distance_slow_down := $signed16_from_byte(50)
    five := $signed16_from_byte(2)
    distance_stop := distance_slow_down / five
    forward_fast := $signed16_from_byte(100)
    distance_gain := forward_fast / distance_slow_down

    corner_previous := 0xff
    corner := 0
    call rb2bus_address_put(rb2_shaft2_address)
    call rb2bus_byte3_put(rb2_shaft2_target_x_set, 0, 0)
    call rb2bus_byte3_put(rb2_shaft2_target_y_set, 1, 0)

    #count_previous := 0
    loop_forever
	call rb2bus_address_put(rb2_shaft2_address)

	if corner_previous != corner
	    switch corner & 3
	      case 0
		x_high := 1
		y_high := 0
	      case 1
		x_high := 1
		y_high := 1
	      case 2
		x_high := 0
		y_high := 1
	      case 3
		x_high := 0
		y_high := 0
	    call rb2bus_byte3_put(rb2_shaft2_target_x_set, x_high, 0)
	    call rb2bus_byte3_put(rb2_shaft2_target_y_set, y_high, 0)

	#count_current := count_previous
	#while count_current = count_previous
	#    call rb2bus_byte_put(rb2_shaft2_navigation_latch)
	#    count_current :=
	#      rb2bus_byte_put_get(rb2_shaft2_count_iteration_get)
	#count_previous := count_current

	call number_show('X', rb2_shaft2_x_get, 0, 0, 0)
	call number_show('Y', rb2_shaft2_y_get, 0, 0, 8)
	call number_show('b', rb2_shaft2_target_bearing16_get, 0, 1, 0)
	call number_show('d', rb2_shaft2_target_distance_get, 0, 1, 8)

	call rb2bus_address_put(rb2_shaft2_address)
	x := rb2bus_signed16_put_get(rb2_shaft2_x_get)
	y := rb2bus_signed16_put_get(rb2_shaft2_y_get)
	bearing := rb2bus_signed16_put_get(rb2_shaft2_target_bearing16_get)
	distance := rb2bus_signed16_put_get(rb2_shaft2_target_distance_get)

	#if bearing < minus_degree_180
	#    bearing := bearing + degree_360
	#else_if bearing > degree_180
	#    bearing := bearing - degree_360

	bearing_absolute := bearing
	if bearing < zero
	    bearing_absolute := -bearing
	if bearing_absolute < turn_limit
	    #Let's move forward:
	    if distance < distance_slow_down
		if distance < distance_stop
		    forward := zero
		    bearing := zero
		    corner := corner + 1
		else
		    forward := distance * distance_gain
	    else
	    	forward := forward_fast
	else
	    forward := zero
	computed_right := bearing * bearing_gain
	computed_left := -computed_right
	computed_left := computed_left + forward
	computed_right := computed_right + forward

	#call signed16_show('L', computed_left, 0, 0, 0)
	#call signed16_show('R', computed_right, 0, 0, 8)
	#call signed16_show('F', forward, 0, 1, 8)

	# Limit the number:
	if computed_left > limit_positive
	    computed_left := limit_positive
	else_if computed_left < limit_negative
	    computed_left := limit_negative
	if computed_right > limit_positive
	    computed_right := limit_positive
	else_if computed_right < limit_negative
	    computed_right := limit_negative
	
	#ctual_left := computed_left
	#actual_right := computed_right
	#if computed_left > actual_left
	#    actual_left := actual_left - motor_adjust
	#else_if computed_left < actual_left
	#    actual_left := actual_left + motor_adjust
	#if computed_right > actual_right
	#    actual_right := actual_right - motor_adjust
	#else_if computed_right < actual_right
	#    actual_right := actual_right + motor_adjust

	#motor_left := $signed16_to_byte(actual_left)
	#motor_right := $signed16_to_byte(actual_right)

	motor_left := $signed16_to_byte(computed_left)
	motor_right := $signed16_to_byte(computed_right)

	#call byte_show('l', motor_left, 0, 1, 0)
	#call byte_show('r', motor_right, 0, 1, 8)

	call rb2bus_address_put(rb2_midimotor2_address)
	call rb2bus_byte2_put(rb2_motor0_speed_set, motor_left)
	call rb2bus_byte2_put(rb2_motor1_speed_set, motor_right)


procedure rb2bus_byte2_put
    argument command byte
    argument data byte
    returns_nothing

    # This routine will send {command} followed by {data} to the currently
    # selected module.

    call rb2bus_byte_put(command)
    call rb2bus_byte_put(data)


procedure rb2bus_byte3_put
    argument command byte
    argument high byte
    argument low byte
    returns_nothing

    # This routine will send {command} followed by {data} to the currently
    # selected module.

    call rb2bus_byte_put(command)
    call rb2bus_byte_put(high)
    call rb2bus_byte_put(low)


procedure byte_show
    argument label byte
    argument value byte
    argument lcd byte
    argument row byte
    argument column byte
    returns_nothing

    # This routine will ...

    call rb2bus_address_put(rb2_lcd32_address + lcd)
    call rb2bus_byte_put(rb2_lcd32_row_set | row)
    call rb2bus_byte_put(rb2_lcd32_column_set | column)
    call rb2bus_byte_put(label)
    call rb2bus_byte_put(':')
    if value@7
	call rb2bus_byte_put('-')
	value := 0 - value
    else
	call rb2bus_byte_put('+')
    call rb2bus_hex_put(value)


procedure signed16_show
    argument label byte
    argument value signed16
    argument lcd byte
    argument row byte
    argument column byte
    returns_nothing

    # This routine will ...

    local zero signed16

    call rb2bus_address_put(rb2_lcd32_address + lcd)
    call rb2bus_byte_put(rb2_lcd32_row_set | row)
    call rb2bus_byte_put(rb2_lcd32_column_set | column)
    call rb2bus_byte_put(label)
    call rb2bus_byte_put(':')

    zero := $signed16_from_byte(0)
    if value < zero
	value := zero - value
	call rb2bus_byte_put('-')
    else
	call rb2bus_byte_put('+')
    call rb2bus_hex_put($signed16_byte_high(value))
    call rb2bus_hex_put($signed16_byte_low(value))


procedure rb2bus_byte_put_get
    argument command byte
    returns byte

    # This routine will send {command} to the currently selected module
    # and return the one byte response.

    call rb2bus_byte_put(command)
    return rb2bus_byte_get()


procedure rb2bus_signed16_put_get
    argument command byte
    returns signed16

    # This routine will send {command} to the currently selected module.
    # and retreive a two byte result as a signed 16 number and return it.

    local high byte
    local low byte

    call rb2bus_byte_put(command)
    high := rb2bus_byte_get()
    low := rb2bus_byte_get()
    return $signed16_from_byte2(high, low)