# #############################################################################
#
# 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 Motor2 RoboBrick.  Basically
# it just waits for commands that come in at 2400 baud and responds
# to them.  See:
#
#   http://web.gramlich.net/projects/robobricks/motor2/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
# Most RoboBricks use 9 cycles per loop, but this one needs 12:
constant extra_instructions_per_bit 12
constant extra_instructions_per_delay extra_instructions_per_bit / delays_per_bit
constant delay_instructions instructions_per_delay - extra_instructions_per_delay

# Register definitions:
# TMR0 register:
register tmr0 1

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

# OSCCAL register:
register osccal 5
constant osccal_unit 4

# On the 505, the OPTION register is only setable via the option instrucition.
constant rbwu_bit 7
constant rbpu_bit 6
constant t0cs_bit 5
constant t0se_bit 4
constant psa_bit 3
constant ps2_bit 2
constant ps1_bit 1
constant ps0_bit 0
constant rbuw_mask (1 << rbwu_bit)
constant rbpu_mask (1 << rbpu_bit)
constant t0cs_mask (1 << t0cs_bit)
constant t0se_mask (1 << t0se_bit)
# Disable Wake-up and pull-ups; set timer to internal; edge_source to raising:
constant option_mask rbuw_mask | rbpu_mask

# Define port bit assignments
constant motor0a_bit 0
constant motor0b_bit 1
constant motor1a_bit 4
constant motor1b_bit 5
constant serial_in_bit 0
constant serial_out_bit 1
constant motor0e_bit 2
constant motor1e_bit 3
port portb b bits_and_byte read_write_static
port portc c bits_and_byte read_write_static
pin motor0a portb motor0a_bit write_only
pin motor0b portb motor0b_bit write_only
pin motor1a portb motor1a_bit write_only
pin motor1b portb motor1b_bit write_only
pin serial_in portc serial_in_bit read_only
pin serial_out portc serial_out_bit write_only
pin motor0e portc motor0e_bit write_only
pin motor1e portc motor1e_bit write_only
constant motor0a_mask (1 << motor0a_bit)
constant motor0b_mask (1 << motor0b_bit)
constant motor1a_mask (1 << motor1a_bit)
constant motor1b_mask (1 << motor1b_bit)
constant serial_in_mask (1 << serial_in_bit)
constant serial_out_mask (1 << serial_out_bit)
constant motor0e_mask (1 << motor0e_bit)
constant motor1e_mask (1 << motor1e_bit)

# 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 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

# Mode (pulsed vs. continuous) bits:
global motor0_mode bit
global motor1_mode bit
global motor0_direction bit
global motor1_direction bit

bank 1

# Shared command registers and option:
global glitch byte
global id_index byte
global option byte

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

# 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

# Globals:
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()
    }
    # 3 cycles to escape loop:

    # Skip over start bit:
    # 1 cycle to code bank switch:
    call delay()
    call delay()
    call delay()
    # 1 cycle to code bank switch back:

    # Sample in the middle third of each data bit:
    # 1 cycle:
    char := 0
    # 2 cycles to set up loop:
    # 3+1+1+1+2 = 8
    count_down count 8 {
	# 1 cycle to code bank switch:
	call delay()
	# 1 cycle to code bank switch back:
	# 2 cycles:
        char := char >> 1
	# 2 cycles:
        if (serial_in) {
            char@7 := 1
        }
	# 1 cycle to code bank switch:
	call delay()
	call delay()
	# 1 cycle to code bank switch back:
	# 3 cycles at end of loop for test and branch:
	# 1+1+2+2+1+1+3 = 11
	nop extra_instructions_per_bit - 11
    }

    # 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
    # 1 cycle for code bank switch:
    call delay()
    call delay()
    call delay()
    # 1 cycle for code bank switch:
    # 2 cycles for loop setup:
    # 1+1+1+2=5
    nop extra_instructions_per_bit - 5

    # Send the data:
    count_down count 8 {
	# 4 cycles:
	serial_out := char@0
	# 2 cycles:
	char := char >> 1
	# 1 cycle for code bank switch:
	call delay()
	call delay()
	call delay() 
	# 1 cycle for code bank switch:
	# Test and jump at end of loop takes 3 cycles:
	# 4+2+1+1+3 = 11
	nop extra_instructions_per_bit - 11
    }

    # Send the stop bit:
    # 1 cycle extra needed to balance loop:
    nop 1
    # 1 cycle:
    serial_out := 1
    # 1 cycle for code bank switch:
    call delay()
    call delay()
    call delay()
    # 1 cycle for code bank switch:
    # 2 cycles for call/return:
    # 2 cycles for argument
    # 1+1+1+2+2 = 7
    nop extra_instructions_per_bit - 7
}

bank 0

procedure set_up {
    arguments_none
    returns_nothing

    # This procedure will set the speed of motor to speed.

    # 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:
    if (ramp0 = 0) {
	# No ramping:
	actual_speed0 := desired_speed0
	ramp0_delay := 0
	ramp0_offset := 0
    } else {
	# Ramping:
	if (desired_speed0 < actual_speed0) {
	    ramp0_offset := 0xff
	} else {
	    ramp0_offset := 1
	}
    }
    #FIXME: do a motor0_off := 0 and delete the appropriate statements below;
    if (motor0_direction) {
	# Direction = 1 (Backward):
	if (motor0_mode) {
	    # Mode = 1 (Continuous):
	    motor0_off := motor0a_mask
	} else {
	    # Mode = 0 (Pulsed)
	    motor0_off := 0
	}
	motor0_on := motor0b_mask
    } else {
	# Direction = 0 (Forward):
	if (motor0_mode) {
	    # Mode = 1 (Continuous):
	    motor0_off := motor0b_mask
	} else {
	    # Mode = 0 (Pulsed):
	    motor0_off := 0
	}
	motor0_on := motor0a_mask
    }

    # Motor1:
    if (ramp1 = 0) {
	# No ramping:
	actual_speed1 := desired_speed1
	ramp1_delay := 0
	ramp1_offset := 0
    } else {
	# Ramping:
	if (desired_speed1 < actual_speed1) {
	    ramp1_offset := 0xff
	} else {
	    ramp1_offset := 1
	}
    }
    # FIXME: do a motor1 := 0 here and delete the appropriate statements below:
    if (motor1_direction) {
	# Direction = 1 (Backward):
	if (motor1_mode) {
	    # Mode = 1 (Continuous):
	    motor1_off := motor1a_mask
	} else {
	    # Mode = 1 (Pulsed)
	    motor1_off := 0
	}
	motor1_on := motor1b_mask
    } else {
	# Direction = 0 (Forward):
	if (motor1_mode) {
	    # Mode = 1 (Continuous):
	    motor1_off := motor1b_mask
	} else {
	    # Mode = 0 (Pulsed):
	    motor1_off := 0
	}
	motor1_on := motor1a_mask
    }
}

bank 1

# This is kind of ugly.  The main routine pretty much fills up all
# of code bank 1.  We need a little extra space.  So we push global
# variable initialization off to this little chunk of code in code
# bank 0.  Not pretty.

procedure reset {
    arguments_none
    returns_nothing


    # FIXME: By a little careful global rearrangement, it should be possible
    # to use a loop to initialize everything.  It is quite likely that it is
    # OK to just zero out all veriables (except option.)

    # Totally blast bank 0:

    # Initialize everything else:
    motor0e := 1
    motor1e := 1
    actual_speed0 := 0
    actual_speed1 := 0
    motor0_off := 0
    motor0_on := 0
    motor1_off := 0
    motor1_on := 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
    fail_safe := 0
    fail_safe_errors := 0
    fail_safe_high_counter := 0
    fail_safe_low_counter := 0
    glitch := 0
    id_index := 0

    # Initialize the OPTION register:
    option := option_mask
    # Now option_mask is in W:
    assemble {
	option
    }
}

bank 0

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay delay_instructions

    # Delay for 1/3 of a bit time.

    variable motor0 byte
    variable motor1 byte

    # 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
    }
    portb := motor1 | motor0

    # 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_speed0 := 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
    }
    portb := motor1 | motor0

    # Do {ramp0} management:
    ramp0_delay := ramp0_delay - 1
    if (z) {
	ramp0_delay := ramp0
	if (actual_speed0 != desired_speed0) {
	    actual_speed0 := actual_speed0 + ramp0_offset
	}
    }

    # 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
    }
    portb := motor1 | motor0

    # Do {ramp1} management:
    ramp1_delay := ramp1_delay - 1
    if (z) {
	ramp1_delay := ramp1
	if (actual_speed1 != desired_speed1) {
	    actual_speed1 := actual_speed1 + ramp1_offset
	}
    }

    # 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
    }
    portb := motor1 | motor0
}

origin 0x200
bank 1

procedure main {
    arguments_none
    returns_nothing

    variable command byte
    variable temp byte

    call reset()

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

	# Dispatch on command:
	switch (command >> 6) {
	  case 0 {
	    # Set Quick (Command = 00hh hhdm):
	    temp := (command << 2) & 0xf0
	    if (command@0) {
		# Motor :
		desired_speed1 := temp
		motor1_direction := command@1
	    } else {
		desired_speed0 := temp
		motor0_direction := 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
		motor1_direction := command@1
	    } else {
		# Motor 0:
		desired_speed0 := desired_speed0 & 0xf0 | temp
		motor0_direction := 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 := get_byte()
		    if (command@0) {
			ramp1 := temp
		    } else {
			ramp0 := temp
		    }
		    call set_up()
		  }
		  case 2 {
		    # Set Failsafe (Command = 1000 0010):
		    fail_safe := get_byte()
		    fail_safe_high_counter := fail_safe
		    fail_safe_low_counter := 0
		  }
		  case 3 {
		    # Reset Failsafe (Command = 1000 0011):
		    fail_safe_low_counter := 0
		    fail_safe_high_counter := fail_safe
		  }
		  case 4, 5, 6, 7 {
		    # Set Speed (Command = 1000 01dm):
		    temp := get_byte()
		    if (command@0) {
			# Motor 1:
			desired_speed1 := temp
			motor1_direction := command@1
		    } else {
			# Motor 0:
			desired_speed0 := temp
			motor0_direction := command@1
		    }
		    call set_up()
		  }
		}
	      }
	      case 1 {
		# Command = 1000 1xxx:
		if (command@2) {
		    # Set direction (Command = 1000 11dm):
		    if (command@0) {
			# Motor 1:
			motor1_direction := command@1
		    } else {
			# Motor 0:
			motor0_direction := 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 := option_mask | (command & 7)
		# Option is in W register:
		assemble {
		    option
		}
	      }
	      case 3 {
		# Command = 1001 1xxx:
		switch (command & 7) {
		  case 0 {
		    # Read Failsafe (Command = 1001 1000):
		    call send_byte(fail_safe)
		  }
		  case 1 {
		    # Read Prescaler (Command = 1001 1001):
		    call send_byte(option & 7)
		  }
		  case 2, 3 {
		    # Read Speed (Command = 1001 101m):
		    if (command@0) {
			call send_byte(actual_speed1)
		    } else {
			call send_byte(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 send_byte(temp)
		  }
		  case 6, 7 {
		    # Read Ramp (Command = 1001 101m):
		    if (command@0) {
			temp := ramp1
		    } else {
			temp := ramp0
		    }
		    call send_byte(temp)
		  }
		}
	      }
	      case 4 {
		# Command = 0110 0xxx:
		switch (command & 7) {
		  case 0 {
		    # Read Failsafe Errors (Command = 1010 0000):
		    call send_byte(fail_safe_errors)
		    fail_safe_errors := 0
		  }
		  case 1 {
		    # Read Failsafe Counter (Command = 1010 0001):
		    call send_byte(fail_safe_high_counter)
		  }
		  case 2 {
		    # Read Actual Speed 0(Command = 1010 0010):
		    call send_byte(actual_speed0)
		  }
		  case 3 {
		    # Read Actual Speed (Command = 1010 001m):
		    call send_byte(actual_speed1)
		  }
		  case 4, 5, 6, 7 {
		    # Reset everything (Command =  1010 01om):
		    if (command@0) {
			# Motor 1:
			motor1e := command@1
		    } else {
			# Motor 0:
			motor0e := command@1
		    }
		  }
		}
	      }
	      case 5 {
		# Command = 1010 1xxx:
		switch (command & 3) {
		  case 0 {
		    # Reset (Command = 1010 1000):
		    call reset()
		  }
		  case 1 {
		    # Read On/Off (Command = 1010 1001):
		    temp := 0
		    if (motor0e) {
			temp@0 := 1
		    }
		    if (motor1e) {
			temp@1 := 1
		    }
		    call send_byte(temp)
		  }
		  default 7 {
		    # Do nothing:
		  }
		}
	      }
	      default 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_unit
		  }
		  case 1 {
		    # Clock Increment (Command = 1111 1001):
		    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
		    }
		  }
		}
	      }
	    }
	  }
	}
    }
}