# #############################################################################
#
# Copyright (c) 1999-2001 by Wayne C. Gramlich.
# 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 software needed to run Laser1 RoboBrick.  All communication
# occurs at 2400 baud.  See:
#
#   http://web.gramlich.net/projects/robobricks/laser1/index.html
#
# for more details.
#
# #############################################################################

processor pic16f628 cp=off cpd=off lvp=off bowden=off mclre=on pwrte=off wdte=off fosc=xt
#processor pic16f84 cp=off dp=off pwrte=disabled wdte=disabled fosc=xt

# Some timing constants:
constant clock_rate 10000000
constant clock_ticks_per_instruction 4
constant instructions_per_second clock_rate / clock_ticks_per_instruction
constant baud_rate 2400
constant instructions_per_bit clock_rate / (clock_ticks_per_instruction * baud_rate)
constant delays_per_bit 3
constant instructions_per_delay instructions_per_bit / delays_per_bit

# Some bit definitions (see schematic):
constant laser_enable_bit 0
constant serial_in_bit 1
constant serial_out_bit 2
constant laser_in_bit 3
constant servo_out_bit 5
constant low_in_bit 6
constant high_in_bit 7

# Some port and pin definitions:
port porta a unused none
port portb b bits_only read_write_static
pin laser_enable_pin portb laser_enable_bit write_only
pin serial_out_pin portb serial_out_bit write_only
pin serial_in_pin portb serial_in_bit read_only
pin laser_in_pin portb laser_in_bit read_only
pin servo_out_pin portb servo_out_bit write_only
pin low_in_pin portb low_in_bit read_only
pin high_in_pin portb high_in_bit read_only

# Some register and bit declarations:

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

# INTCON: Interrupt Control
register intcon 0xb
bind gie intcon@7
bind peie intcon@6

# PIR1:
register pir1 0xc
bind eeif pir1@7
bind cmif pir1@6
bind rcif pir1@5
bind txif pir1@4
bind ccp1if pir1@2
bind tmr2if pir1@1
bind tmr1if pir1@0

# TMR1L: Timer 1 Low:
register tmr1l 0xe

# TMR1H: Timer1 High:
register tmr1h 0xf

# T1CON: Timer 1 Contro:
register t1con 0x10
bind t1ckps1 t1con@5
bind t1ckps0 t1con@4
bind t1oscen t1con@3
bind t1sync t1con@2
bind tmr1cs t1con@1
bind tmr1on t1con@0

# CCPR1L: Capture/Compare/PWM Low:
register ccpr1l 0x15

# CCPR1H: Capture/Compare/PWM High:
register ccpr1h 0x16

# CCP1CON: Capture/Compare/PWM Control:
register ccp1con 0x17
bind ccp1x ccp1con@5
bind ccp1y ccp1con@4
bind ccp1m3 ccp1con@3
bind ccp1m2 ccp1con@2
bind ccp1m1 ccp1con@1
bind ccp1m0 ccp1con@0

# PIR2:
register pie1 0x8c
bind eeie pie1@7
bind cmie pie1@6
bind rcie pie1@5
bind txie pie1@4
bind ccp1ie pie1@2
bind tmr2ie pie1@1
bind tmr1ie pie1@0

# Some globals:
constant maximum_captures 7
global interrupt_enable bit
global interrupt_pending bit
global servo_enable bit
global sweeping bit
global servo byte
global servo_low byte
global servo_high byte
global state byte
global middle byte
global high byte
global capture_index byte
global capture_count byte
global capture_low[maximum_captures] byte
global capture_middle[maximum_captures] byte
global capture_high[maximum_captures] byte

# The state variable states:
constant state_idle 0
constant state_forward_up1 1
constant state_forward_down1 2
constant state_forward_up2 3
constant state_forward_down2 4
constant state_backward_up1 5
constant state_backward_down1 6
constant state_backward_up2 7
constant state_backward_down2 8

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

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay instructions_per_delay

    # Delay 1 third of a bit:

    variable counter byte
    variable middle byte

    # Deal with servo pulse:
    counter := counter + 1
    if (counter = 144) {
	counter := 0
    }
    if (servo_enable) {
	if (counter = 0) {
	    servo_out_pin := 1
	} else_if (counter = servo) {
	    servo_out_pin := 0
	}
    }

    # Process any capture event:
    if (ccp1if) {
	ccp1if := 0
	capture_low[capture_index] := ccpr1l
	middle := ccpr1h
	capture_middle[capture_index] := middle
	if (tmr1if && !(middle@7)) {
	    # Capture occured after counter overflowed:
	    tmr1if := 0
	    high := high + 1
	}
	capture_high[capture_index] := high
	if (capture_index != maximum_captures) {
	    capture_index := capture_index + 1
	}
    }

    # Process state variable:
    switch state {
      case state_idle {
	# Do nothing:
	servo_enable := 0
	sweeping := 0
      }
      case state_forward_up1 {
	if (low_in_pin) {
	    state := state_forward_down1
	    laser_enable_pin := 1
	    tmr1on := 1
	}
      }
      case state_forward_down1 {
	# Force the servo to the high location:
	if (!low_in_pin) {
	    state := state_forward_up2
	}
      }
      case state_forward_up2 {
	# Force servo to low location:
	if (low_in_pin) {
	    # Turn off the timer and laser:
	    tmr1on := 0
	    laser_enable_pin := 0
	    state := state_forward_down2

	    # Deal with any counter overflow:
	    if (tmr1if) {
		tmr1if := 0
		high := high + 1
	    }

	    # Record the 24-bit timer:
	    capture_low[capture_index] := ccpr1l
	    capture_middle[capture_index] := ccpr1h
	    capture_high[capture_index] := high
	    if (capture_index = maximum_captures) {
		# We have an error:
		capture_count := 0xff
	    } else {
		capture_count := capture_index + 1
	    }
	    capture_index := 0
	}
      }
      case state_forward_down2 {
	# Force the servo to the high location:
	if (!low_in_pin) {
	    state := state_backward_up1
	    servo := servo_high
	}
      }
      case state_backward_up1 {
	# Force the servo to the high location:
	if (low_in_pin) {
	    state := state_backward_down1
	}
      }
      case state_backward_down1 {
	# Force the servo to the high location:
	if (!low_in_pin) {
	    state := state_backward_up2
	}
      }
      case state_backward_up2 {
	# Force the servo to the high location:
	if (low_in_pin) {
	    state := state_backward_down2
	}
      }
      case state_backward_down2 {
	# Force the servo to the high location:
	if (!low_in_pin) {
	    state := state_idle
	}
      }
    }

    # Process any counter overflow:
    if (tmr1if) {
	tmr1if := 0
	high := high + 1
    }

}

procedure get_byte {
    arguments_none
    returns byte

    # Wait for an 8-bit byte from {serial_in_pin}, read it, and return it.

    variable result byte
    variable count byte

    # Wait until a start bit arrives:
    while (serial_in_pin) {
	call delay()
    }
    interrupt_enable := 0

    # Skip over the start bit:
    call delay()
    call delay()
    call delay()

    # Sample in the middle third of each data bit:
    result := 0
    count_down count 8 {
	call delay()
	result := result >> 1
	if (serial_in_pin) {
	    result@7 := 1
	}
	call delay()
	call delay()
    }

    # Skip over 1/3 of the stop bit:
    call delay()

    return result
}

procedure send_byte {
    argument data byte
    returns_nothing

    # Send {data} to {serial_out_pin}.

    variable count byte

    # Send the start bit:
    serial_out_pin := 0
    call delay()
    call delay()
    call delay()

    # Send the data:
    count_down count 8 {
	serial_out_pin := data@0
	data := data >> 1
	call delay()
	call delay()
	call delay()
    }

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


procedure reset {
    arguments_none
    returns_nothing

    # Reset everything:

    capture_index := 0
    while (capture_index < maximum_captures) {
	capture_high[capture_index] := 0
	capture_middle[capture_index] := 0
	capture_low[capture_index] := 0
	capture_index := capture_index + 1
    }
    capture_index := 0
}

# This is used to get the switch statements to work out:
origin 0x200

procedure main {
    arguments_none
    returns_nothing

    # Main program.

    variable command byte
    variable glitch byte
    variable id_index byte
    variable temporary byte
    
    glitch := 0
    id_index := 0
    serial_out_pin := 1
    servo := 14
    servo_low := 5
    servo_high := 17
    servo_enable := 0
    sweeping := 0
    laser_enable_pin := 0
    state := state_idle
    high := 0
    call reset()

    # Initialize timer:
    t1ckps0 := 0
    t1ckps1 := 0
    t1oscen := 0
    # t1sync := 1
    tmr1cs := 0
    tmr1on := 0

    # Make sure all interrupts are off:
    intcon := 0

    # Turn off CCP:
    ccp1con := 0

    loop_forever {
	# Wait for command:
	command := get_byte()

	# Dispatch on command:
	switch (command >> 6) {
	  case 0 {
	    # Command = 00xx xxxx:
	    switch (command >> 3) {
	      case 0 {
		# Command = 0000 0xxx:
		switch (command & 7) {
		  case 0, 1 {
		    # Set Laser Enable (Command = 0000 000z):
		    laser_enable_pin := command@0
		  }
		  case 2 {
		    # Read Laser (Command = 0000 0010):
		    temporary := capture_count & 0xf
		    if (!laser_in_pin) {
			temporary@4 := 1
		    }
		    if (low_in_pin) {
			temporary@5 := 1
		    }
		    if (high_in_pin) {
			temporary@6 := 1
		    }
		    if (sweeping) {
			temporary@7 := 1
		    }
		    call send_byte(temporary)
		  }
		  case 3 {
		    # Read last caputure (Command = 0000 0011):
		    call send_byte(capture_high[capture_index])
		    call send_byte(capture_middle[capture_index])
		    call send_byte(capture_low[capture_index])
		    capture_index := capture_index + 1
		  }
		  case 4 {
		    # Disable Servo (Command = 0000 0100):
		    servo_enable := 0
	          }
		  case 5 {
		    # Enable Servo Low (Command = 0000 0101):
		    servo_enable := 1
		    servo := servo_low
	          }
		  case 6 {
		    # Enable Servo High (Command = 0000 0101):
		    servo_enable := 1
		    servo := servo_high
	          }
		  case 7 {
		    # Sweep (Command = 0000 0111):
		    call reset()
		    sweeping := 1
		    servo_enable := 1
		    servo := servo_low
		    state := state_forward_up1

		    # Turn on CCP:
		    ccp1con := 4
		    ccp1if := 0

		    # Clear timer 1:
		    tmr1l := 0
		    tmr1h := 0
		    high := 0
		  }
		}
	      }
	      case 1 {
		# Undefinded command; do nothing:
		switch (command & 7) {
		  case 0 {
		    # Set Servo Low (Command = 0000 1000)
		    servo_low := get_byte()
		  }
		  case 1 {
		    # Set Servo High (Command = 0000 1001)
		    servo_high := get_byte()
		  }
		  case 2 {
		    # Read Servo Low (Command = 0000 1010)
		    call send_byte(servo_low)
		  }
		  case 3 {
		    # Read Servo Low (Command = 0000 1011)
		    call send_byte(servo_high)
		  }
		  default 7 {
		    # (Command = 0000 11xx)
		  }
		}
	      }
	      default 7 {
		# Do Nothing:
	      }
	    }
	  }
	  case 1, 2 {
	    # Do nothing (Command = 01xx xxxx or 10xx xxxx):
	  }
	  case 3 {
	    # Command = 11xx xxxx:
	    switch ((command >> 3) & 7) {
	      case 5 {
		# Command = 1110 1xxx:
		if ((command & 7) = 7) {
		    # Return Interrupt Bits (Command = 1110 1111):
		    temporary := 0
		    if (interrupt_enable) {
			temporary@1 := 1
		    }
		    if (interrupt_pending) {
			temporary@0 := 1
		    }
		    call send_byte(temporary)
		}
	      }
	      case 6 {
		# Shared Interrupt commands (Command = 1111 0xxx):
		switch (command & 7) {
		  case 0, 1, 2, 3 {
		    # Set interrupt enable and pending (Command = 1111 00ep):
		    interrupt_enable := command@1
		    interrupt_pending := command@0
		  }
		  case 4, 5 {
		    # Set Interrupt pending bit only (Command = 1111 010p):
		    interrupt_pending := command@0
		  }
		  case 6, 7 {
		    # Set Interrupt pending bit only (Command = 1111 011e):
		    interrupt_enable := command@0
		  }
		}
	      }
	      case 7 {
		# Shared commands.
		switch (command & 7) {
		  case 0 {
		    # Clock Decrement (Command = 1111 1000):
		    # Do nothing:
		  }
		  case 1 {
		    # Clock Increment (Command = 1111 1001):
		    # Do nothing:
		  }
		  case 2 {
		    # Clock Read (Command = 1111 1010):
		    call send_byte(0)
		  }
		  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
		    }
		  }
		}
	      }
	    }
	  }
	}
    }
}