# #############################################################################
#
# Copyright (c) 2000-2003 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://gramlich.net/projects/robobricks/motor2/index.html
#
# for more details.
#
# #############################################################################

processor pic16f676 bg=bg00 cpd=off cp=off boden=off mclre=off pwrte=off wdte=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
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:
# TMR0 register:
register tmr0 1

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

# OSCCAL register:
register osccal 0x90
constant osccal_unit 4

# On the 630/676, the OPTION register has the following bits:
register option 0x81
constant rapu_bit 7
constant intedg_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 rapu_mask (1 << rapu_bit)
constant intedg_mask (1 << intedg_bit)
constant t0cs_mask (1 << t0cs_bit)
constant t0se_mask (1 << t0se_bit)
constant psa_mask (1 << psa_bit)
# Disable Wake-up and pull-ups; set timer to internal; edge_source to raising:
constant option_mask rapu_mask


# INTCON:
register intcon 0xb
constant gie_bit 0
constant peie_bit 0
constant t0ie_bit 0
constant inte_bit 0
constant raie_bit 0
constant t0if_bit 0
constant intf_bit 0
constant raif_bit 0

# WPUA:
register wpua 0x95

# Define port A bit assignments:
constant motor0a_bit 0
constant motor0b_bit 1
constant motor1b_bit 2
constant unused1_bit 3
constant motor1a_bit 4
constant unused0_bit 5
port porta a bits_and_byte read_write_static
pin motor0a porta motor0a_bit write_only
pin motor0b porta motor0b_bit write_only
pin motor1b porta motor1b_bit write_only
pin unused0 porta unused0_bit read_only
pin motor1a porta motor1a_bit write_only
pin unsued1 porta unused1_bit read_only

# Define port C bit assignments:
constant serial_in_bit 0
constant serial_out_bit 1
constant motor0e_bit 2
constant motor1e_bit 3
constant unused2_bit 4
constant unused3_bit 5
port portc c bits_and_byte read_write_static
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
pin unused2 portc unused2_bit read_only
pin unsued3 portc unused2_bit read_only

# Define some masks:
constant motor0a_mask (1 << motor0a_bit)
constant motor0e_mask (1 << motor0e_bit)
constant motor0b_mask (1 << motor0b_bit)
constant motor1a_mask (1 << motor1a_bit)
constant motor1e_mask (1 << motor1e_bit)
constant motor1b_mask (1 << motor1b_bit)
constant serial_in_mask (1 << serial_in_bit)
constant serial_out_mask (1 << serial_out_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

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

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

# 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()
    }

    # 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()
	# 2 cycles:
        char := char >> 1
	# 2 cycles:
        if (serial_in) {
            char@7 := 1
        }
	call delay()
	call delay()
	# 3 cycles at end of loop for test and branch:
	# 2+2+3 = 7
	nop extra_instructions_per_bit - 7
    }

    # 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:
    serial_out := 0
    call delay()
    call delay()
    call delay()

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

    # Send the stop bit:
    serial_out := 1
    call delay()
    call delay()
    call delay()
}

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

procedure reset {
    arguments_none
    returns_nothing

    # Initialize everything else:
    motor0e := 1
    motor1e := 1
    intcon := 0
    option := option_mask
    wpua := 0
    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
}

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay delay_instructions

    # Delay for 1/3 of a bit time.

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

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

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

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) | (command >> 2)
	    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_high_counter := fail_safe
		    fail_safe_low_counter := 0
		  }
		  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)
	      }
	      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 1 (Command = 1010 0011):
		    call send_byte(actual_speed1)
		  }
		  case 4 {
		    # Set Motor 0 off (Command = 1010 0100):
		    motor0e := 0
		  }
		  case 5 {
		    # Set Motor 0 on (Command = 1010 0101):
		    motor0e := 1
		  }
		  case 6 {
		    # Set Motor 1 off (Command = 1010 0110):
		    motor1e := 0
		  }
		  case 7 {
		    # Set Motor 1 on (Command = 1010 0111):
		    motor1e := 1
		  }
		}
	      }
	      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()
		}
	      }
	      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
		    }
		  }
		}
	      }
	    }
	  }
	}
    }
}