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

processor pic12c509 cp=off wdte=off mclre=off fosc=intrc

# 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

# Stepper bit numbers:
constant stepper0_bit 0
constant stepper1_bit 1
constant stepper2_bit 2
# Note: on the 8-pin PIC's, GPIO3 is input only:
constant serial_in_bit 3
constant stepper3_bit 4
constant serial_out_bit 5

# Servo bit masks:
constant stepper0_mask (1 << stepper0_bit)
constant stepper1_mask (1 << stepper1_bit)
constant stepper2_mask (1 << stepper2_bit)
constant stepper3_mask (1 << stepper3_bit)
constant serial_in_mask (1 << serial_in_bit)
constant serial_out_mask (1 << serial_out_bit)

# Register definitions:

# Status register:
register status 3
bind c status@0
bind z status@2

# OSCCAL register:
register osccal 5

# The 509 has 4 bits of OSCCAL and the 509A has 6 bits.
constant osccal_lsb 0x10
#constant osccal_lsb 0x4

# Define port bit assignments
port porta a bits_and_byte read_write_static
pin stepper0 porta stepper0_bit write_only
pin stepper1 porta stepper1_bit write_only
pin stepper2 porta stepper2_bit write_only
pin stepper3 porta stepper3_bit write_only
pin serial_in porta serial_in_bit read_only
pin serial_out porta serial_out_bit write_only

string_constants {
    id = 1, 0, 15, 0, 0, 0, 0, 0, 0r'16', 9, 0s'Stepper1A', 15, 0s'Gramlich&Benson'
    waves = 0x10, 4, 2, 1, 0x14, 6, 3, 0x11, 0x10, 0x14, 4, 6, 2, 3, 1, 0x11
}

# Note that the 12-bit PIC's only have a 2-level deep stack.
# The code starts in the main procedure (located at the end of this code)
# The next level of procedure call is either get_byte or send_byte.
# Lastly, the lowest level of procedure called is delay.  It all fits,
# but just barely.

bank 0

# Define global values that are read and written by main procedure
# and read by delay procedure.  See comment in front of delay
# procedure that talks about global register accessibility.
# Note that servos=4, so we are defining 9 registers below.

global adjust_low byte
global adjust_high byte
global current_low byte
global current_high byte
global desired_low byte
global desired_high byte
global middle_high byte
global middle_low byte
global slow_rate byte
global fast_rate byte
global ramp_rate byte
global ramp_amount byte
global complement byte
global power_down bit
global wave_table byte
global wave_mask byte
global wave_offset byte
global interrupt_pending bit
global interrupt_enable bit

# We are really tight on data registers.  Push data registers
# into register bank 1 for every procedure execept delay to
# spread the register bank load.

bank 0

procedure get_byte {
    arguments_none
    returns byte

    # This procedure will wait for a byte to be received from
    # serial_in_bit.  It calls the delay procedure for all delays.

    variable count byte
    variable char byte

    # Why does the delay procedure wait for a third of bit?  Well, it
    # has to do with the loop immediately below.  If we catch the
    # start bit at the beginning of a 1/3 bit time, we will be
    # sampling data at approximately 1/3 of the way into each bit.
    # Conversely, if we catch the start near the end of a 1/3 bit
    # bit time, we will be sampling data at approximately 2/3 of the
    # way into each bit.  So, what this means is that our bit sample
    # times will be somewhere between 1/3 and 2/3 of bit (i.e. in
    # the middle of the bit.

    # It would be nice to tweak the code to shorter delay times
    # (1/4 bit, 1/5 bite, etc.) but then it gets too hard to get
    # the bookeeping done in the delay routine.  A 12-bit PIC
    # running at 4MHz (=1MIPS), only has 138 instructions available
    # for the delay routine when at 1/3 of bit.

    # Wait for a start bit:
    while (serial_in) {
	call delay()
    }

    # 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 := char | 0x80
	}
        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

    variable count byte

    # This procedure will send byte to serial_out_bit.

    # send the start bit:
    serial_out := 0
    call delay()
    call delay()
    call delay()

    # send the data:
    count_down count 8 {
	serial_out := char@0
	char := char >> 1
	call delay()
	call delay()
	call delay()
    }
    
    # send stop bit
    serial_out := 1
    call delay()
    call delay()
    call delay()
}

# In order for the uniform delay stuff to work, the delay routine must have
# all of its local variables in register bank 0 *AND* all of the global
# variables it accesses in register bank 0.  That way, the uCL peep-hole
# optimizer never attempts to stuff in any instructions to patch up
# register bank accessibility.  Any tweaking by the peep-hole optimizer
# messes up the uniform delay instruction counting.

bank 0

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay instructions_per_delay

    # This procedure will delay for a third of bit at 2400 baud.
    # It is responsible for keeping the servo pulses going on
    # a regular basis.

    variable counter byte

    porta := porta & serial_out_mask | (waves[current_low & wave_mask + wave_offset] ^ complement)
    
    counter := counter - 1
    if (z) {
	# Adjust the position:
	counter := ramp_rate
	current_high := current_high + adjust_high
	current_low := current_low + adjust_low
	if (c) {
	    current_high := current_high + 1
	}
    }
}

bank 1

procedure end_point_update {
    arguments_none
    returns_nothing

    # This procedure will figure out all of magic constants needed
    # when the end-points are adjusted.

    variable direction bit

    if (desired_high > current_high) {
	direction := 1
    } else_if (desired_high < current_high) {
	direction := 0
    } else_if (desired_low > current_low) {
	direction := 1
    } else_if (desired_low > current_low) {
	direction := 1
    } else {
	# They are equal; think about turning off the coils:
    }

    if (direction) {
	adjust_high := 0
	adjust_low := 1
	middle_high := desired_high - current_high
	middle_low := desired_low - current_low
    } else {
	adjust_high := 0xff
	adjust_low := adjust_high
	middle_high := current_high - desired_high
	middle_low := current_low - desired_low
    }
    if (!c) {
	middle_high := middle_high + 1
    }
    # Divide middle by 2:
    assemble {
	bcf c___byte c___bit
	rrf middle_high f
	rrf middle_low f
    }
}

procedure reset {
    arguments_none
    returns_nothing

    current_low := 0
    current_high := 0
    desired_low := 0
    desired_high := 0
    slow_rate := 255
    fast_rate := 255
    ramp_rate := 1
    ramp_amount := 1
    complement := 0
    wave_table := 0
    interrupt_pending := 0
    interrupt_enable := 0
}

# The main procedure is loaded with switch statements.  On the 12-bit
# PIC's, switch statements have to live in the first 256 bytes of
# each code bank.  For this reason, we shove main into code bank 1.
# If we, try to put main in code bank 0, it pushes the first bytes
# of several other routines out of the first 256 bytes, which is also
# a no-no of the 12-bit PIC's.

origin 0x200
bank 1

procedure main {
    arguments_none
    returns_nothing

    variable command byte
    variable temp byte
    variable glitch byte
    variable index byte

    call reset()
    glitch := 0
    index := 0

    # Loop around waiting for a command:
    loop_forever {
	# Wait for a command byte:
        command := get_byte()

	# Dispatch on the command:
	switch (command >> 6) {
	  case 0 {
	    # Command = 01xx xxxx:
	    if (command@5) {
		# Decrement Desired (Command = 001d dddd):
		desired_low := desired_low - 1
		if (desired_low = 0xff) {
		    desired_high := desired_high - 1
		}
	    } else {
		# Increment Desired (Command = 000i iiii):
		desired_low := desired_low + 1
		if (z) {
		    desired_high := desired_high + 1
		}
	    }
	    call end_point_update()
	  }
	  case 1 {
	    # Command = 01xx xxxx:
	    switch ((command >> 3) & 7) {
	      case 0 {
		# Command = 0100 0xxx:
		temp := get_byte()
		switch (command & 7) {
		  case 0 {
		    # Set Desired High (Command = 0100 0000):
		    desired_high := temp
		    call end_point_update()
		  }
		  case 1 {
		    # Set Desired Low (Command = 0100 0001):
		    desired_low := temp
		    call end_point_update()
		  }
		  case 2 {
		    # Set Current High (Command = 0100 0010):
		    current_high := temp
		    call end_point_update()
		  }
		  case 3 {
		    # Set Current Low (Command = 0100 0011):
		    current_low := temp
		    call end_point_update()
		  }
		  case 4 {
		    # Set Slow Rate (Command = 0100 0100):
		    slow_rate := temp
		  }
		  case 5 {
		    # Set Fast Rate (Command = 0100 0101):
		    fast_rate := temp
		  }
		  case 6 {
		    # Set Ramp Rate (Command = 0100 0110):
		    ramp_rate := temp
		  }
		  case 7 {
		    # Set Ramp Amount (Command = 0100 0111):
		    ramp_amount := temp
		  }
		}
	      }
	      case 1 {
		# Command = 0100 1xxx:
		switch (command & 7) {
		  case 0 {
		    # Read Desired High (Command = 0100 1000):
		    temp := desired_high
		  }
		  case 1 {
		    # Read Desired Low (Command = 0100 1001):
		    temp := desired_low
		  }
		  case 2 {
		    # Read Current High (Command = 0100 1010):
		    temp := current_high
		  }
		  case 3 {
		    # Read Current Low (Command = 0100 1011):
		    temp := current_low
		  }
		  case 4 {
		    # Read Slow Rate (Command = 0100 1100):
		    temp := slow_rate
		  }
		  case 5 {
		    # Read Fast Rate (Command = 0100 1101):
		    temp := fast_rate
		  }
		  case 6 {
		    # Read Ramp Rate (Command = 0100 1110):
		    temp := ramp_rate
		  }
		  case 7 {
		    # Read Ramp Amount (Command = 0100 1111):
		    temp := ramp_amount
		  }
		}
		call send_byte(temp)
	      }
	      case 2, 3 {
		# Set Complement Mask (Command = 0101 cccc):
		complement := (command << 1) & 0x10 | command & 7
	      }
	      case 4 {
		# Command = 0110 0xxx:
		switch (command & 7) {
		  case 0, 1, 2, 3 {
		    # Set Wave Table (Command = 0110 00ww):
		    wave_table := command & 3
		    switch (wave_table) {
		      case 0 {
			# Wave Drive:
			wave_mask := 3
			wave_offset := 0
		      }
		      case 1 {
			# Two Phase:
			wave_mask := 3
			wave_offset := 4
		      }
		      case 2, 3 {
			# Two Phase:
			wave_mask := 7
			wave_offset := 8
		      }
		    }
		  }
		  case 4, 5 {
		    # Set Power Down Bit (Command = 0110 010p):
		    power_down := command@0
		  }
		  case 6 {
		    # Read Power Down Bit (Command = 0110 0110):
		    temp := 0
		    if (power_down) {
			temp := temp + 1
		    }
		    call send_byte(temp)
		  }
		  case 7 {
		    # Read Wave Table (Command = 0110 0111):
		    call send_byte(wave_table)
		  }
		}
	      }
	      case 5 {
		# Command = 0110 1xxx:
		switch (command & 7) {
		  case 0 {
		    # Set Desired (Command = 0110 1000):
		    temp := get_byte()
		    desired_low := get_byte()
		    desired_high := temp
		    call end_point_update()
		  }
		  case 1 {
		    # Set Current (Command = 0110 1001):
		    temp := get_byte()
		    current_low := get_byte()
		    current_high := temp
		    call end_point_update()
		  }
		  case 2 {
		    # Read Desired (Command = 0110 1010):
		    call send_byte(desired_low)
		    call send_byte(desired_high)
		  }
		  case 3 {
		    # Read Current (Command = 0110 1011):
		    call send_byte(current_low)
		    call send_byte(current_high)
		  }
		  case 4 {
		    # Read Complement Mask (Command = 0110 1100):
		    call send_byte((complement >> 1) & 8 | complement & 7)
		  }
		  case 5 {
		    # Reset (Command = 0110 1101):
		    call reset()
		  }
		  case 6 {
		    # Stop (Command = 0110 1110):
		    # Code to quickly stop goes here:
		  }
		  case 7 {
		    # Clear (Command = 0110 1111):
		    current_high := 0
		    current_low := 0
		    desired_high := 0
		    desired_low := 0
		    call end_point_update()
		  }
		}
	      }
	      default 7 {
		# Do nothing:
	      }
	    }
	  }
	  case 3 {
	    # Command = 11xx xxxx:
	    switch ((command >> 3) & 7) {
	      case 5 {
		# Command = 1110 1xxx:
		switch (command & 7) {
		  case 0, 1, 2, 3, 4, 5, 6 {
		    # Command = 1110 10xx, 1110 110x, or 1110 1110:
		    # Do nothing:
		  }
		  case 7 {
		    # Read Interrupt Bits (Command = 1110 1111):
		    if (interrupt_enable) {
			temp := 2
		    } else {
			temp := 0
		    }
		    if (interrupt_pending) {
			temp := temp + 1
		    }
		    call send_byte(temp)
		  }
		}
	      }
	      case 6 {
		# Set Interrupt Bits (Command = 1111 0xxx):
		switch ((command >> 1) & 3) {
		  case 0, 1 {
		    # Set Interrupt Bits (Command = 1111 00ep):
		    interrupt_enable := command@1
		    interrupt_pending := command@0
		  }
		  case 2 {
		    # Set Interrupt Pending (Command = 1111 010p):
		    interrupt_pending := command@0
		  }
		  case 3 {
		    # Set Interrupt Enable (Command = 1111 010e):
		    interrupt_enable := command@0
		  }
		}
	      }
	      case 7 {
		# Shared commands (Command = 1111 1xxx):
		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 send_byte(osccal)
		  }
		  case 3 {
		    # Clock Pulse (Command = 1111 1011):
		    call send_byte(0)
		  }
 		  case 4 {
		    # ID Next (Command = 1111 1100):
		    call send_byte(id[index])
		    index := index + 1
		    if (index >= id.size) {
			index := 0
		    }
		  }
		  case 5 {
		    # ID Reset (Command = 1111 1101):
		    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
		    }
		  }
		}
	      }
	    }
	  }
	} 
    }
}