# #############################################################################
#
# Copyright (c) 2002 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 code for the SonarDT1 RoboBrick at:
#
#   http://web.gramlich.net/projects/robobricks/sonardt1/index.html
#
# Some pin assignments:
#
#	No	Name		Kind		Description
#	========================================================
#	1	RA2/AN2/VREF	Digital Out	LED D4
#	2	RA3/AN3/CMP1	Digital Out	LED D3
#	3	RA4/TOCKI/CMP2	Digital Out	LED D2
#	4	RA5/MCLR*/THV	Digital Out	LED D1
#	5	VSS		Ground		Ground
#	6	RB0/INT		No Connection
#	7	RB1/RX/DT	Digital	In	Serial In
#	8	RB2/TX/CK	Digital Out	Serial Out
#	9	RB3/CCP1	Digital In	Echo Return
#	10	RB4/PGM		Digital Out	Trigger
#	11	RB5		Digital Out	Servo Out
#	12	RB6/T1OSO/TICK1	No Connection
#	13	RB7/T1OSI	No Connection
#	14	VDD		Power		+5 Volts
#	15	RA6/OSC2/CLKOUT	No Connection
#	16	RA7/OSC1/CLKIN	Digital In	Oscillator In
#	17	RA0/AN0		Digital Out	LED D6
#	18	RA1/AN1 	Digital Out	LED D5
#
# #############################################################################

processor pic16f628 cp=off cpd=off lvp=off bowden=off mclre=off pwrte=off wdte=off fosc=ec
constant clock_rate 20000000

# Some character constants:
constant sp 32
constant cr 13
constant lf 10

# Some register definitions:
register tmr0 1

register status 3
bind c status@0
bind z status@2

register intcon 0xb
bind gie intcon@7
bind t0ie intcon@5
bind inte intcon@4
bind rbie intcon@3
bind t0if intcon@2
bind intf intcon@1
bind rbif intcon@0

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

register tmr1l 0xe

register tmr1h 0xf

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

register tmr2 0x11

register t2con 0x12
bind toutps3 t2con@6
bind toutps2 t2con@5
bind toutps1 t2con@4
bind toutps0 t2con@3
bind tmr2on t2con@2
bind t2ckps1 t2con@1
bind t2ckps0 t2con@0

register ccpr1l 0x15

register ccpr1h 0x16

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

register rcsta 0x18
bind spen rcsta@7
bind rx9 rcsta@6
bind sren rcsta@5
bind cren rcsta@4
bind aden rcsta@3
bind ferr rcsta@2
bind oerr rcsta@1
bind rx9d rcsta@0

register txreg 0x19

register rcreg 0x1a

# Comparator module control:
register cmcon 0x1f
bind c2out cmcon@7
bind c1out cmcon@6
bind c2inv cmcon@5
bind c1inv cmcon@4
bind cis cmcon@3
bind cm2 cmcon@2
bind cm1 cmcon@1
bind cm0 cmcon@0

register option 0x81
bind t0cs option@5
bind t0se option@4
bind psa option@3
bind ps2 option@2
bind ps1 option@1
bind ps0 option@0

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

register pr2 0x92

register txsta 0x98
bind tx9 txsta@6
bind txen txsta@5
bind sync txsta@4
bind brgh txsta@2
bind trmt txsta@1
bind tx9d txsta@0

register spbrg 0x99

# Some port, bit, and pin definitions:

# Port A pin assignments:

# RA0: LED D6
# RA1: LED D5
# RA2: LED D4
# RA3: LED D3
# RA4: LED D2
# RA5: LED D1
# RA6: No/Connection
# RA7: Oscillator In

constant led6_bit 0
constant led5_bit 1
constant led4_bit 2
constant led3_bit 3
constant led2_bit 4
constant led1_bit 5
constant nc1_bit 6
constant osc_in_bit 7

port porta a bits_only read_write_static
pin led6 porta led6_bit write_only
pin led5 porta led5_bit write_only
pin led4 porta led4_bit write_only
pin led3 porta led3_bit write_only
pin led2 porta led2_bit write_only
pin led1 porta led1_bit write_only
pin nc1 porta nc1_bit read_only
pin osc_in porta osc_in_bit read_only

# Port B pin assignments:

# RB0: No Connection
# RB1: Serial Input (RX)
# RB2: Serial Output (TX)
# RB3: Echo Return
# RB4: Trigger
# RB5: Servo Out
# RB6: No_Connection
# RB7: No_Connection

constant nc2_bit 0
constant rx_bit 1
constant tx_bit 2
constant echo_return_bit 3
constant trigger_bit 4
constant servo_out_bit 5
constant nc3_bit 6
constant nc4_bit 7

port portb b bits_only read_write_static
pin nc2 portb nc2_bit read_only
# When using the USART, both the TX and RX pins must be set to input:
pin tx_pin portb tx_bit read_only
pin rx_pin portb rx_bit read_only
pin echo_return portb echo_return_bit read_only
pin trigger portb trigger_bit write_only
pin servo_out portb servo_out_bit write_only
pin nc3 portb nc3_bit read_only
pin nc4 portb nc4_bit read_only

global send_in_index byte
global send_out_index byte
constant send_buffer_size 10
global send_buffer[send_buffer_size] byte

# This code basically has to do 3 things:
#
# 1) It has to keep listening for commands from the serial input.
# 2) It has to be able to keep a servo pulse of length .5ms to 2.0ms
#    coming out every 20ms or so.
# 3) It has to trigger an sonar pulse, and time the resultant period
#    until the echo is heard (100uSec to 36mSec).
#
# Timer 0 is hooked up to the clock/4 and then run through the 256
# prescaler.  With the clock running at 20MHz, divided by 4 and again
# by 256 leaves a clock frequency of 19531Hz.  20ms is a refresh rate
# of 50Hz.  So, 19531/50 is 390.62, or 390.  390 does not quite fit
# into 8-bit, so we divide it by 2 to get 195.  So, we wait for the
# Timer 0 flag to count through 195 twice, before triggering servo
# pulse.

global glitch byte
global id_index byte

string_constants {
    id = 1, 0, 29, 0, 0, 0, 0, 0, 0r'16', 8, 0s'SonarDT1', 15, 0s'Gramlich&Benson'
}

procedure main {
    arguments_none
    returns_nothing

    variable command byte
    variable continuous bit
    variable counter byte
    variable distance_high byte
    variable distance_low byte
    variable phase bit
    variable result byte
    variable servo byte
    variable servo_enable bit

    call initialize()

    # Initialize ring buffer:
    send_in_index := 0
    send_out_index := 0

    continuous := 1
    servo := 0x80
    servo_enable := 1
    glitch := 0
    id_index := 0

    # Main loop
    loop_forever {
	# Deal with timer 0:
	if (t0if && servo_enable) {
	    t0if := 0
	    counter := counter - 1
	    if (c) {
		# See discussion above to see where 195 comes from:
		counter := 195
		if (phase) {
		    phase := 0
		} else {
		    phase := 1

		    # Now it is time to squirt out a servo pulse:
		    servo_out := 1  

		    # Now use Timer2 to control pulse width:
		    pr2 := servo
		    tmr2 := 0
		    tmr2on := 1
		}
	    }
	}

	# Deal with turning off servo pulse:
	if (tmr2if) {
	    tmr2if := 0
	    servo_out := 0
	    tmr2on := 0
	}

	# See whether we can transmit a character:
	if (send_in_index != send_out_index && txif) {
	    txreg := send_buffer[send_out_index]
	    send_out_index := send_out_index + 1
	    if (send_out_index >= send_buffer_size) {
		send_out_index := 0
	    }
	}

	# See whether we've got a character:
	if (rcif) {
	    command := rcreg
	    switch (command >> 6) {
	      case 0 {
		# Do Nothing:
		switch ((command >> 3) & 7) {
		  case 0 {
		    # Command = 0000 0xxx
		    switch (command & 7) {
		      case 0 {
			# Read Distance Low (Command = 0000 0000):
			call send_byte(distance_low)
		      }
		      case 1 {
			# Read Distance High (Command = 0000 0001):
			call send_byte(distance_high)
		      }
		      case 2 {
			# Read Distance Low and High (Command = 0000 0010):
			call send_byte(distance_low)
			call send_byte(distance_high)
		      }
		      case 3 {
			# Trigger Distance Measurement (Command 0000 0011):
			tmr1on := 1
		      }
		      case 4 {
			# Disable Servo (Command 0000 0100):
			servo_enable := 0
		      }
		      case 5 {
			# Enable Servo (Command 0000 0101):
			servo_enable := 1
		      }
		      case 6 {
			# Disable Continuous Measurement (Command 0000 0110):
			continuous := 0
			tmr1on := 0
		      }
		      case 7 {
			# Enable Continuous Measurement (Command 0000 0111):
			continuous := 1
			tmr1on := 1
		      }
		    }
		  }
		  case 1 {
		    switch (command & 7) {
		      case 0 {
			# Increment Servo (Command 0000 1000):
			servo := servo + 1
		      }
		      case 1 {
			# Decrement Servo (Command 0000 1001):
			servo := servo - 1
		      }
		      case 2 {
			# Increment Servo (Command 0000 1010):
			call send_byte(servo)
		      }
		      case 3 {
			# Decrement Servo (Command 0000 1011):
			result := 0
			if (servo_enable) {
			    result@0 := 1
			}
			if (continuous) {
			    result@1 := 1
			}
			call send_byte(result)
		      }
		      default 7 {
			# Do nothing
		      }
		    }
		  }
		  case 2, 3 {
		    # Set Servo Low (Command 0001 llll):
		    servo := servo & 0xf0 | command & 0xf
		  }
		  case 4, 5 {
		    # Set Servo High (Command 0010 hhhh):
		    servo := servo & 0xf | command << 4
		  }
		  default 7 {
		    # Do Nothing:
		  }
		}
	      }
	      case 1 {
		# Do Nothing:
	      }
	      case 2 {
		# Do Nothing:
	      }
	      case 3 {
		# Do shared commands:
		if (command >> 3 & 7 = 7) {
		    call do_shared(command)
		}
	      }
	    }
	}

	# Read the captured distance:
	if (ccp1if) {
	    ccp1if := 0
	    distance_high := ccpr1h
	    distance_low := ccpr1l

	    # Make distance visible in LED's:
	    porta := 0xff ^ ccpr1h

	    if (!continuous) {
		tmr1on := 0
	    }
	}

	# See whether we need to generate another trigger pulse:
	if (tmr1if) {

	    # Make sure trigger is low for at least 10uSec:
	    tmr1if := 0
	    delay 25 {
		tmr1on := 0
		tmr1l := 0
		tmr1h := 0
	    }

	    # Fire trigger for 10uSec:
	    trigger := 1
	    delay 25 {
		ccp1if := 0
	    }

	    # Turn the timer on when trigger goes low:
	    trigger := 0
	    tmr1on := 1
	}

    }
}

procedure do_shared {
    argument command byte
    returns_nothing

    # Command 1111 1xxx:
    switch (command & 7) {
      case 0 {
        # Clock Decrement (Command 1111 1xxx):
      }
      case 1 {
	# Clock Increment (Command 1111 1xxx):
      }
      case 2 {
	# Clock Read (Command 1111 1xxx):
	call send_byte(0)
      }
      case 3 {
	# Clock Pulse (Command 1111 1xxx):
	call send_byte(0)
      }
      case 4 {
	# Id Next (Command 1111 1xxx):
	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 1xxx):
	id_index := 0
      }
      case 6 {
	# Glitch Read (Command 1111 1110):
	call send_byte(glitch)
	glitch := 0
      }
      case 7 {
	# Glitch (Command 1111 1111):
	glitch := glitch + 1
      }
    }
}


procedure initialize {
    arguments_none
    returns_nothing

    # Get all interrupts turned off:
    intcon := 0
    pie1 := 0
    pir1 := 0

    # Initialize UART:
    # Prescaler = low:
    brgh := 0
    # Baud rate = 2400 baud:
    spbrg := 129

    # Asynchronous mode:
    sync := 0
    # 8-bit mode:
    tx9 := 0
    # Serial Port Enable:
    spen := 1

    # Keep interrupts off:
    txie := 0
    # Clear out an previous character.
    txif := 0
    # Enable the transmitter:
    txen := 1

    # Enable the receiver:
    # Keep inerrupts off:
    rcie := 0
    # Enable continuous reception:
    cren := 1
    # Enable single receptions:
    sren := 1
    # Disable address
    aden := 0

    # Initialize the timer 0 module:
    t0cs := 0
    psa := 0
    ps2 := 1
    ps1 := 1
    ps0 := 1

    # Initialize the timer 1 module:
    # Prescale = 1:2
    t1ckps1 := 0
    t1ckps0 := 1
    # Disable oscillator:
    t1oscen := 0
    # T1SYNC not used when TMR1CS = 0:
    #t1sync := 0
    # Internal clock:
    tmr1cs := 0
    # Turn timer off:
    tmr1on := 1
    # Clear interrupt flags:
    tmr1ie := 0
    tmr1if := 0
	
    # Initialize the timer 2 module:
    tmr2ie := 0
    tmr2on := 0
    # Postscale is 1:14
    toutps3 := 1
    toutps2 := 1
    toutps1 := 0
    toutps0 := 1
    # Prescale is 1:4
    t2ckps1 := 0
    t2ckps0 := 1

    # Initialize the Capture/Compare/PWM (CCP) module:
    # CCP1X and CCP1Y are unused:
    # Capture mode every falling edge:
    ccp1m3 := 0
    ccp1m2 := 1
    ccp1m1 := 0
    ccp1m0 := 0
    # Turn off CCP module:
    ccp1ie := 0
    ccp1if := 0
}

# The following procedures are used to send data back to the master:

procedure send_byte {
    argument character byte
    returns_nothing

    # This procedure will cause {character} to placed into
    # a ring buffer for transmission.

    send_buffer[send_in_index] := character
    send_in_index := send_in_index + 1
    if (send_in_index >= send_buffer_size) {
	send_in_index := 0
    }
}