# #############################################################################
#
# 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/rev_c/index.html
#
# Some pin assignments:
#
#	No	Name		Kind		Description
#	========================================================
#	1	RA2/AN2/VREF	Digital Out	N2:3 TRIG
#	2	RA3/AN3/CMP1	Digital Out	D5
#	3	RA4/TOCKI/CMP2	Digital Out	D4
#	4	RA5/MCLR*/THV	No Connection
#	5	VSS		Ground		Ground
#	6	RB0/INT		Digital Out	D8
#	7	RB1/RX/DT	Digital	In	N1:4 SIN
#	8	RB2/TX/CK	Digital Out	N1:5 SOUT
#	9	RB3/CCP1	Digital In	N2:4 ECHO
#	10	RB4/PGM		Digital Out	N4:1 SRV
#	11	RB5		Digital Out	D1
#	12	RB6/T1OSO/TICK1	Digital Out	D2
#	13	RB7/T1OSI	Digital Out	D3
#	14	VDD		Power		+5 Volts
#	15	RA6/OSC2/CLKOUT Xtal		Resonator
#	16	RA7/OSC1/CLKIN	Digital In	Oscillator In
#	17	RA0/AN0		Digital Out	D6
#	18	RA1/AN1		Digital Out	D7
#
# #############################################################################

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

# 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: D6
# RA1: D7
# RA2: TRIG
# RA3: D5
# RA4: D4
# RA5: No Connection 
# RA6: Resonator In
# RA7: Oscillator In

constant led6_bit 0
constant led7_bit 1
constant trigger_bit 2
constant led5_bit 3
constant led4_bit 4
constant nc1_bit 5
constant res_in_bit 6
constant osc_in_bit 7

port porta a bits_only read_write_static
pin led6 porta led6_bit write_only
pin led7 porta led7_bit write_only
pin trigger porta trigger_bit write_only
pin led5 porta led5_bit write_only
pin led4 porta led4_bit write_only
pin nc1 porta nc1_bit read_only
pin res_in porta res_in_bit read_only
pin osc_in porta osc_in_bit read_only

# Port B pin assignments:

# RB0: D8
# RB1: SIN (RX)
# RB2: SOUT (TX)
# RB3: ECHO
# RB4: SRV
# RB5: D1
# RB6: D2
# RB7: D3

constant led8_bit 0
constant rx_bit 1
constant tx_bit 2
constant echo_return_bit 3
constant servo_out_bit 4
constant led1_bit 5
constant led2_bit 6
constant led3_bit 7

constant sweep_maximum 16

port portb b bits_only read_write_static
pin led8 portb led8_bit write_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 servo_out portb servo_out_bit write_only
pin led1 portb led1_bit write_only
pin led2 portb led2_bit write_only
pin led3 portb led3_bit write_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, 2, 0, 0, 0, 0, 0r'16', 9, 0s'SonarDT1C', 29, 0s'Gramlich&Benson'
}

procedure main {
    arguments_none
    returns_nothing

    variable command byte
    variable command1 byte
    variable command2 byte
    variable command_need byte
    variable command_have byte
    variable continuous bit
    variable counter byte
    variable distance_high byte
    variable distance_low byte
    variable leds byte
    variable temp byte
    variable phase bit
    variable result byte
    variable servo byte
    variable servo_enable bit

    variable sweep_highs[sweep_maximum] byte
    variable sweep_lows[sweep_maximum] byte
    variable sweep_initial byte
    variable sweep_increment byte
    variable sweep_count byte
    variable sweep_counter byte
    variable sweep_delay byte
    variable sweep_sleep byte
    variable sweep_index byte

    variable sweep_enable bit
    variable sweep_direction bit

    call initialize()

    # Initialize ring buffer:
    sweep_enable := 0
    send_in_index := 0
    send_out_index := 0
    command_need := 1
    command_have := 0

    continuous := 1
    servo := 0x80
    servo_enable := 0
    glitch := 0
    id_index := 0
    sweep_sleep := 0
    sweep_direction := 1
    sweep_index := 1

    # 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

		    # Now do any sweep processing:
		    if (sweep_enable) {
			sweep_sleep := sweep_sleep - 1
			if (z) {
			    sweep_sleep := sweep_delay
			    sweep_highs[sweep_index] := distance_high
			    sweep_lows[sweep_index] := distance_low

			    # See if we need to change sweep direction:
			    if (sweep_direction) {
				# Going up:
				if (sweep_index + 1 >= sweep_count) {
				    sweep_direction := 0
				    sweep_counter := sweep_counter + 1
				}			
			    } else {
				# Going down:
				if (sweep_index = 0) {
				    sweep_direction := 1
				    servo := sweep_initial
				    sweep_counter := sweep_counter + 1
				}
			    }

			    # Update the sweep index and corresponding
			    # servo position:
			    if (sweep_direction) {
				sweep_index := sweep_index + 1
				servo := servo + sweep_increment
			    } else {
				sweep_index := sweep_index - 1
				servo := servo - sweep_increment
			    }
			}
		    }
		}
	    }
	}

	# 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) {
	    # Read the command:
	    switch (command_have) {
	      case 0 {
		command := rcreg
	      }
	      case 1 {
		command1 := rcreg
	      }
	      case 2 {
		command2 := rcreg
	      }
	    }
	    command_have := command_have + 1

	    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 {
		# Command = 01xx xxxx:
		switch ((command >> 3) & 7) {
		  case 0 {
		    # Command = 0100 0xxx:
		    switch (command & 7) {
		      case 0 {
			# Set Sweep Initial Position (command 0100 0000):
			command_need := 2
			if (command_have = command_need) {
			    sweep_initial := command1
			}
		      }
		      case 1 {
			# Set Sweep Increment (command 0100 0001):
			command_need := 2
			if (command_have = command_need) {
			    sweep_increment := command1
			}
		      }
		      case 2 {
			# Set Sweep Count (command 0100 0010):
			command_need := 2
			if (command_have = command_need) {
			    sweep_count := command1
			    if (sweep_count > 16) {
				sweep_count := 16
			    }
			}
		      }
		      case 3 {
			# Set Sweep Delay (command 0100 0011):
			command_need := 2
			if (command_have = command_need) {
			    sweep_delay := command1
			}
		      }
		      case 4 {
			# Set Sweep Counter (command 0100 0100):
			command_need := 2
			if (command_have = command_need) {
			    sweep_counter := command1
			}
		      }
		      case 5 {
			# Do_nothing (command 0100 0101):
		      }
		      case 6, 7 {
			# Set Sweep Enable (command 0100 011s):
			sweep_enable := 0
			if (command@0) {
			    sweep_sleep := 15
			    sweep_enable := 1
			    sweep_index := 0
			    servo_enable := 1
			    servo := sweep_initial
			} else {
			    sweep_enable := 0
			    servo_enable := 0
			    servo := sweep_initial
			}
		      }
		    }
		  }
		  case 1 {
		    # Command = 0100 1xxx:
		    switch (command & 7) {
		      case 0 {
			# Read Sweep Initial Position (command 0100 1000):
			call send_byte(sweep_initial)
		      }
		      case 1 {
			# Read Sweep Increment (command 0100 1001):
			call send_byte(sweep_increment)
		      }
		      case 2 {
			# Read Sweep Count (command 0100 1010):
			call send_byte(sweep_count)
		      }
		      case 3 {
			# Read Sweep Delay (command 0100 1011):
			call send_byte(sweep_delay)
		      }
		      case 4 {
			# Read Sweep Counter (command 0100 0100):
			call send_byte(sweep_counter)
		      }
		      case 5 {
			# Read Sweep Enable (command 0100 0101):
			result := 0
			if (sweep_enable) {
			    result@0 := 1
			}
			call send_byte(result)
		      }
		      default 7 {
			# Do nothing.
		      }
		    }
		  }
		  case 2, 3 {
		    # Read Sweep Index High and Low (command 0101 iiii):
		    result := command & 0xf
		    call send_byte(sweep_highs[result])
		    call send_byte(sweep_lows[result])
		  }
		  case 4, 5 {
		    # Read Sweep Index High Only (command 0101 iiii):
		    result := command & 0xf
		    call send_byte(sweep_highs[result])
		  }
		  case 6 {
		    # Write into memory:
		    command_need := 3
		    if (command_need = command_have) {
			sweep_highs[command1] := command2
		    }
		  }
		  default 7 {
		    # Do nothing:
		    command_need := 2
		    if (command_need = command_have) {
			call send_byte(sweep_highs[command1])
		    }
		  }
		}
	      }
	      case 2 {
		# Do Nothing:
	      }
	      case 3 {
		# Do shared commands:
		if (command >> 3 & 7 = 7) {
		    call do_shared(command)
		}
	      }
	    }
	}

	# Reset command reader:
	if (command_have = command_need) {
	    command_need := 1
	    command_have := 0
	}

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

	    # Make distance visible in LED's:
	    
	    leds := 0xff ^ ccpr1h
	    led1 := leds@0
	    led2 := leds@1
	    led3 := leds@2
	    led4 := leds@3
	    led5 := leds@4
	    led6 := leds@5
	    led7 := leds@6
	    led8 := leds@7

	    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

    # Initialize the comparator module:
    c2out := 0
    c1out := 0
    c2inv := 0
    c1inv := 0
    cis := 0
    cm2 := 1
    cm1 := 1
    cm0 := 1

}

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