# #############################################################################
#
# Copyright (c) 2000-2001 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 Shaft2 RoboBrick.  Basically
# it just waits for commands that come in at 2400 baud and responds
# to them.  See:
#
#   http://web.gramlich.net/projects/robobricks/shaft2/index.html
#
# for more details.
#
# #############################################################################

processor pic16c505 cp=off wdte=on mclre=off fosc=intrc_no_clock

# Define processor constants:
constant clock_rate 4000000						
constant clocks_per_instruction 4
constant instruction_rate clock_rate / clocks_per_instruction
				
# Define serial communication control constants:
constant baud_rate 2400
constant instructions_per_bit instruction_rate / baud_rate
constant delays_per_bit 3
constant instructions_per_delay instructions_per_bit / delays_per_bit
constant extra_instructions_per_bit 9
constant extra_instructions_per_delay extra_instructions_per_bit / delays_per_bit
constant delay_instructions instructions_per_delay - extra_instructions_per_delay

# Register definitions:
register tmr0 1
register status 3
bind c status@0
bind z status@2

register osccal 5
constant osccal_unit 4

# Define port bit assignments:
port portb b bits_and_byte read_write_static
port portc c bits_and_byte read_write_static
pin in0 portb 0 read_only
pin in1 portb 1 read_only
pin pullups portb 2 read_only
pin in2 portb 3 read_only
pin in3 portb 4 read_only
pin led0 portc 0 write_only
pin led1 portc 1 write_only
pin led2 portc 2 write_only
pin led3 portc 3 write_only
pin serial_out portc 4 write_only
pin serial_in portc 5 read_only

string_constants {
    id = 1, 0, 16, 1, 0, 0, 0, 0, 0r'16', 7, 0s'Shaft2B', 15, 0s'Gramlich&Benson'
}

# Shaft state variables:
global shaft0 byte
global shaft1 byte

# Shaft counters:
global shaft0_high byte
global shaft0_low byte
global shaft1_high byte
global shaft1_low byte

# Shaft low threshold counters:
global shaft0_low_high byte
global shaft0_low_low byte
global shaft1_low_high byte
global shaft1_low_low byte

# Shaft high threshold counters:
global shaft0_high_high byte
global shaft0_high_low byte
global shaft1_high_high byte
global shaft1_high_low byte

# Interrupt and shaft direction bits:
global interrupt_pending bit
global interrupt_enable bit
global shaft0_direction bit
global shaft1_direction bit
    
# For now put all the smaller routines first so that they can live
# within the first 256 bytes of main memory.  The PIC12C5xx chips
# can only call routines that are within the first 256 bytes (i.e.
# the first half) of the code page.

bank 1

global receiving bit

procedure get_byte {
    arguments_none

    returns byte

    # Wait for a character and return it.

    # The get_byte() procedure only waits for 9-2/3 bits.  That
    # way the next call to get_byte() will sychronize on the start
    # bit instead of possibly starting a little later.

    variable count byte
    variable char byte

    # Wait for start bit:
    receiving := 1
    while (serial_in) {
	call delay()
    }

    # Clear any interrupt being sent:
    serial_out := 1

    # Skip over 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 (serial_in) {
            char@7 := 1
        }
	call delay()
	call delay()
    }

    # Skip over 2/3's of stop bit:
    call delay()
    call delay()

    return char
}

procedure send_byte {
    argument char byte
    returns_nothing

    # Send {char} to {tx}:

    variable count byte

    # {receiving} will be 1 if the last get/put routine was a get.
    # Before we start transmitting a response back, we want to ensure
    # that there has been enough time to turn the line line around.
    # We delay the first 1/3 of a bit to pad out the 9-2/3 bits from
    # for get_byte to 10 bits.  We delay another 1/3 of a bit just
    # for good measure.  Technically, the second call to delay()
    # is not really needed.
    if (receiving) {
	receiving := 0
	call delay()
	call delay()
    }

    # Send the start bit:
    # 1 cycle;
    serial_out := 0
    call delay()
    call delay()
    call delay()
    # 2 cycles to set up loop:
    # 1+2 + 3:
    nop extra_instructions_per_bit - 3

    # Send the data:
    count_down count 8 {
	# 4 cycles:
	serial_out := char@0
	# 2 cycles:
	char := char >> 1
	call delay()
	call delay()
	call delay() 
	# 3 cycles at end of loop:
	# 4+2+3 = 9 = no NOP's needed:
    }

    # Send the stop bit:
    nop 1
    # 1 cycle:
    serial_out := 1
    # 2 cycles call/return:
    # 2 cycles for argument:
    # 1+2+2 = 5
    call delay()
    call delay()
    call delay()
    nop extra_instructions_per_bit - 5
}

bank 0

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay delay_instructions

    # This procedure will delay for 1/3 of a bit time.

    variable temp byte
    variable alternate bit

    # Kick the dog:
    watch_dog_reset

    # Process shaft 0:
    temp := portb & 3
    # Convert 2-bit grey code into 2-bit binary:
    if (temp@1) {
	temp := temp ^ 1
    }
    switch ((shaft0 - temp) & 3) {
      case 0 {
	# Nothing changed; do nothing:
      }
      case 1 {
	# Increment:
	shaft0_direction := 0
	shaft0_low := shaft0_low + 1
	if (z) {
	    shaft0_high := shaft0_high + 1
	}
      }
      case 2 {
	# Double increment/decrement:
	if (shaft0_direction) {
	    # Double increment:
	    shaft0_low := shaft0_low - 2
	    if (!c) {
		shaft0_high := shaft0_high - 1
	    }
	} else {
	    # Double decrement:
	    shaft0_low := shaft0_low + 2
	    if (c) {
		shaft0_high := shaft0_high + 1
	    }
	}
      }
      case 3 {
	# Decrement:
	shaft0_direction := 1
	if (shaft0_low = 0) {
	    shaft0_high := shaft0_high - 1
	}
	shaft0_low := shaft0_low - 1
      }
    }
    shaft0 := temp

    # Process shaft 1:
    temp := (portb >> 3) & 3
    # Convert 2-bit grey code into 2-bit binary:
    if (temp@1) {
	temp := temp ^ 1
    }
    switch ((shaft1 - temp) & 3) {
      case 0 {
	# Nothing changed; do nothing:
      }
      case 1 {
	# Increment:
	shaft1_direction := 0
	shaft1_low := shaft1_low + 1
	if (z) {
	    shaft1_high := shaft1_high + 1
	}
      }
      case 2 {
	# Double increment/decrement:
	if (shaft1_direction) {
	    # Double increment:
	    shaft1_low := shaft1_low - 2
	    if (!c) {
		shaft1_high := shaft1_high - 1
	    }
	} else {
	    # Double decrement:
	    shaft1_low := shaft1_low + 2
	    if (c) {
		shaft1_high := shaft1_high + 1
	    }
	}
      }
      case 3 {
	# Decrement:
	shaft1_direction := 1
	if (shaft1_low = 0) {
	    shaft1_high := shaft1_high - 1
	}
	shaft1_low := shaft1_low - 1
      }
    }
    shaft1 := temp

    if (alternate) {
	# Now do range checks:
	alternate := 0
	if (shaft0_high < shaft0_low_high || (shaft0_high = shaft0_low_high && shaft0_low < shaft0_low_low)) {
	    interrupt_pending := 1
	}
	if (shaft0_high > shaft0_high_high || (shaft0_high = shaft0_high_high && shaft0_low > shaft0_high_low)) {
	    interrupt_pending := 1
	}
	if (shaft1_high < shaft1_low_high || (shaft1_high = shaft1_low_high && shaft1_low < shaft1_low_low)) {
	    interrupt_pending := 1
	}
	if (shaft1_high > shaft1_high_high || (shaft1_high = shaft1_high_high && shaft1_low > shaft1_high_low)) {
	    interrupt_pending := 1
	}
    } else {
	# Copy the bits over:
	led0 := in0
	led1 := in1
	led2 := in2
	led3 := in3

	# Perform any interrupt:
	alternate := 1
	if (interrupt_pending && interrupt_enable) {
	    serial_out := 0
	    interrupt_enable := 0
	}
    }
}	     

# The main routine can span the 256 byte boundary:

origin 0x200
bank 1

procedure main {
    arguments_none
    returns_nothing

    variable command byte
    variable glitch byte
    variable id_index byte
    variable high byte
    variable index byte
    variable result byte

    # Initialize everything:
    shaft0 := 0
    shaft0_direction := 0
    shaft0_high := 0
    shaft0_low := 0
    shaft1 := 0
    shaft1_high := 0
    shaft1_low := 0
    shaft1_direction := 0
    interrupt_enable := 0
    interrupt_pending := 0
    glitch := 0
    id_index := 0

    # Set up pull-ups:
    command := 0xff
    if (pullups) {
	command@6 := 0
    }
    assemble {
	movf main__command w
	option
    }

    # Loop waiting for commands:
    loop_forever {
	# Get a command byte:
	command := get_byte()

	# Dispatch on command:
	switch (command >> 6) {
	  case 0 {
	    # Command = 00xx xxxx:
	    switch ((command >> 3) & 7) {
	      case 0 {
		# Command = 0000 0xxx:
		switch (command & 7) {
		  case 0 {
		    # Read Shaft 0 (Command = 0000 0000):
		    call send_byte(shaft0_high)
		    call send_byte(shaft0_low)
		  }
		  case 1 {
		    # Read Shaft 1 (Command = 0000 0001):
		    call send_byte(shaft1_high)
		    call send_byte(shaft1_low)
		  }
		  case 2 {
		    # Read Shaft Low 0 (Command = 0000 0010):
		    call send_byte(shaft0_low)
		  }
		  case 3 {
		    # Read Shaft Low 1 (Command = 0000 0011):
		    call send_byte(shaft1_low)
		  }
		  case 4 {
		    # Set Shaft 0 (Command = 0000 0100):
		    high := get_byte()
		    shaft0_low := get_byte()
		    shaft0_high := high
		  }
		  case 5 {
		    # Set Shaft 1 (Command = 0000 0101):
		    high := get_byte()
		    shaft1_low := get_byte()
		    shaft1_high := high
		  }
		  case 6 {
		    # Set Shaft Low 0 (Command = 0000 0110):
		    shaft0_low := get_byte()
		  }
		  case 7 {
		    # Set Shaft Low 1 (Command = 0000 0111):
		    shaft1_low := get_byte()
		  }
		}
	      }
	      case 1 {
		# Command = 0000 1xxx:
		switch (command & 7) {
		  case 0 {
		    # Increment Shaft 0 (Command = 0000 1000):
		    shaft0_low := shaft0_low + 1
		    if (z) {
			shaft0_high := shaft0_high + 1
		    }
		  }
		  case 1 {
		    # Increment Shaft 1 (Command = 0000 1001):
		    shaft1_low := shaft1_low + 1
		    if (z) {
			shaft1_high := shaft1_high + 1
		    }
		  }
		  case 2 {
		    # Decrement Shaft 0 (Command = 0000 1010):
		    if (shaft0_low = 0) {
			shaft0_high := shaft0_high - 1
		    }
		    shaft0_low := shaft0_low - 1
		  }
		  case 3 {
		    # Decrement Shaft 1 (Command = 0000 1011):
		    if (shaft1_low = 0) {
			shaft1_high := shaft1_high - 1
		    }
		    shaft1_low := shaft1_low - 1
		  }
		  case 4 {
		    # Clear Shaft 0 (Command = 0000 1100):
		    shaft0_low := 0
		    shaft0_high := 0
		  }
		  case 5 {
		    # Clear Shaft 1 (Command = 0000 1101):
		    shaft1_low := 0
		    shaft1_high := 0
		  }
		  default 7 {
		    # Command = 0000 111x:
		  }
		}
	      }
	      case 2 {
		# Command = 0001 0xxx:
		switch (command & 7) {
		  case 0 {
		    # Set High Threshold 0 (Command = 0001 0000):
		    high := get_byte()
		    shaft0_high_low := get_byte()
		    shaft0_high_high := high
		  }
		  case 1 {
		    # Set High Threshold 1 (Command = 0001 0001):
		    high := get_byte()
		    shaft1_high_low := get_byte()
		    shaft1_high_high := high
		  }
		  case 2 {
		    # Set Low Threshold 0 (Command = 0001 0010):
		    high := get_byte()
		    shaft0_low_low := get_byte()
		    shaft0_low_high := high
		  }
		  case 3 {
		    # Set Low Threshold 1 (Command = 0001 0011):
		    high := get_byte()
		    shaft1_low_low := get_byte()
		    shaft1_low_high := high
		  }
		  case 4 {
		    # Read High Threshold 0 (Command = 0001 0100):
		    call send_byte(shaft0_high_high)
		    call send_byte(shaft0_high_low)
		  }
		  case 5 {
		    # Read High Threshold 1 (Command = 0001 0101):
		    call send_byte(shaft1_high_high)
		    call send_byte(shaft1_high_low)
		  }
		  case 6 {
		    # Read Low Threshold 0 (Command = 0001 0110):
		    call send_byte(shaft0_low_high)
		    call send_byte(shaft0_low_low)
		  }
		  case 7 {
		    # Read Low Threshold 1 (Command = 0001 0111):
		    call send_byte(shaft1_low_high)
		    call send_byte(shaft1_low_low)
		  }
		}
	      }
	      default 7 {
		# Command = 001x xxxx:
	      }
	    }
	  }
	  case 1, 2 {
	    # Command = 01xx xxxx or10xx xxxx:
	    # Do nothing:
	  }
	  case 3 {
	    # Command = 11xx xxxx:
	    switch ((command >> 3) & 7) {
	      case 5 {
		# Command = 1110 1xxx:
		if ((command & 7) = 7) {
		    # Return Interrupt Bits (Command = 1110 1111):
		    result := 0
		    if (interrupt_enable) {
			result := result | 2
		    }
		    if (interrupt_pending) {
			result := result | 1
		    }
		    call send_byte(result)
		}
	      }
	      case 6 {
		# Shared Interrupt commands.
		switch ((command >> 1) & 3) {
		  case 0, 1 {
		    # Set Interrupt Bits (Command = 1110 00ep):
		    interrupt_enable := command@1
		    interrupt_pending := command@0
		  }
		  case 2 {
		    # Set Interrupt Pending (Command = 1110 010p):
		    interrupt_pending := command@0
		  }
		  case 3 {
		    # Set Interrupt Enable (Command = 1110 011e):
		    interrupt_enable := command@0
		  }
		}
	      }
	      case 7 {
		# Shared commands (Command = 1111 1ccc):
		switch (command & 7) {
		  case 0 {
		    # Clock Decrement (Command = 1111 1000):
		    #if (osccal != 0) {
		    osccal := osccal - osccal_unit 
		    #}
		  }
		  case 1 {
		    # Clock Increment (Command = 1111 1001):
		    #if (osccal + osccal_unit != 0) {
		    osccal := osccal + osccal_unit
		    #}
		  }
		  case 2 {
		    # Clock Read (Command = 1111 1010):
		    call send_byte(osccal)
		  }
		  case 3 {
		    # Clock Pulse (Command = 1111 1011):
		    call send_byte(0)
		  }
		  case 4 {
		    # ID Next (Command = 1111 1100):
		    if (id_index >= id.size) {
			id_index := 0
		    }
		    call send_byte(id[id_index])
		    id_index := id_index + 1
		    if (id_index >= id.size) {
			id_index := 0
		    }
		  }
		  case 5 {
		    # ID Reset (Command = 1111 1101):
		    id_index := 0
		  }
		  case 6 {
		    # Glitch Read (Command = 1111 1110):
		    call send_byte(glitch)
		    glitch := 0
		  }
		  case 7 {
		    # Glitch (Command = 1111 1111):
		    if (glitch != 0xff) {
			glitch := glitch + 1
		    }
		  }
		}
	      }
	    }
	  }
	}
    }
}