Schematic

    Well, I tried to get file upload/download working between my laptop and my Motorola Razr.  Unfortunately, my phone isn't supported by the Linux tools.  I'll be dipped before I pay Motorola for tools to upload software.  Anyway, spent a bit of time trying to make a schematic with Eagle from Cadsoft.  Great product, a little wonky to get used to the interface.  With luck, my schematic will be viewable here.  Click on the image for a size you can actually see.

    As you can see, there is not a whole lot to the thing.  The beast was in the code and the board layout.

    Success!!! Now I can post code!. This time I'm replacing the less thans with ampersand pound 60;. Here goes:

;******************************************************************************
;
; Filename: head.asm

; Date: 2010.06.28

; File Version: 1.0.0

;

; Author: Tom Hunt
;

;******************************************************************************

; NOTES: Pic 16f88, running @ 4MHz internal clock.
; Integrate ping ultrasonic sensor and servo "neck"
; Test look ahead, stop on block ( under threshold ), look left and
; take reading, look right and take reading. Then find most open way.
; If no open way ( all under threshold ), do 180 degree turn.
; Motor operation will be simulated with leds.
;
; Eyes on RA0
; Neck on RA6
; Motor1 on RB2-3
; Motor2 on RB4-5
; Speaker on RB7
;
; TODO - Need a cap for maximum range on sensor. If TMR1H > 8, set
; EyesLH to max value, indicating wide open space.
; I'm not sure this will work, but I need something to tell
; if we've gone beyond maximum range. Google time....
;
; Stupid note to self: CARRY SET MEANS POSITIVE YOU IDIOT!!!!
;
;******************************************************************************
; CHANGE LOG:
; 2010.07.04 * Fixed decision logic. Got backwards on the Carry
; flag after subtraction again.
; * Added sound. Before decision, plays Zelda's lullaby.
;
; 2010.07.05 * Added motors. Tweaking values for motors. Went from
; 6V to 3V motors. May need pulsing.
;
;******************************************************************************



list R=DEC, p=16f88 ; list directive to define processor

#include <p16f88.inc> ; processor specific variable definitions

; _INTRC_IO _HS_OSC

;Program Configuration Register 1
__CONFIG _CONFIG1, _CP_OFF & _CCP1_RB0 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_OFF & _PWRTE_ON & _WDT_OFF & _INTRC_IO

;Program Configuration Register 2
__CONFIG _CONFIG2, _IESO_OFF & _FCMEN_OFF



; '__CONFIG' directive is used to embed configuration data within .asm file.

; The lables following the directive are located in the respective .inc file.

; See respective data sheet for additional information on configuration word.


;******************************************************************************
; CONSTANT DEFINITIONS
;******************************************************************************

#define TOOCLOSE FLAGS,0
#define UTURNLEFT FLAGS,1
#define TIMEOUT FLAGS,2
#define EYES PORTA,0
#define EYESTRIS TRISA^0x080,0
#define MOTORPORT PORTB
#define MOTORRIGHTONE 5
#define MOTORRIGHTTWO 4
#define MOTORLEFTONE 2
#define MOTORLEFTTWO 3
#define SERVO PORTA,6
#define PULSECOUNT 40
#define PULSEDELAY 0x040
#define UTURN 0
#define CANGOLEFT 1
#define CANGORIGHT 2
#define LEFTORRIGHT 3
#define MAXRANGEH 8
#define MAXRANGEL 143
#define THRESHOLDH 3
#define THRESHOLDL 143 ; threshold equates to ~1 foot

; sound constants
#define SPEAKERTRIS TRISB ^0x080
#define SPEAKERPORT PORTB
#define SPEAKER PORTB,7
#define NOTEE7HIGH 0xFF
#define NOTEE7LOW 0x42
#define NOTED7BHIGH 0xFF
#define NOTED7BLOW 0x13
#define NOTEB6HIGH 0xFF
#define NOTEB6LOW 0x02


;******************************************************************************
; VARIABLE DEFINITIONS
;******************************************************************************

CBLOCK 0x20
EYESLH:2 ; holds the time of a pulse back from eyes
LEFTLH:2 ; holds the time of the pulse back from the "eyes" for left look
RIGHTLH:2 ; holds the time of the pulse back from the "eyes" for right look
DIRECTION ; next direction to take
DELAY:3
PULSE:2
PDELAY:2
NOTEHIGH
NOTELOW
COUNTER
SONGCOUNTER
ENDC

CBLOCK 0x70 ; interupt context-saving vars here see datasheet chp 15
FLAGS
w_temp
status_temp
pclath_temp
ENDC



;******************************************************************************
; MACRO DEFINITIONS
;******************************************************************************

bank0 MACRO
bcf STATUS, RP1
bcf STATUS, RP0
ENDM

bank1 MACRO
bcf STATUS, RP1
bsf STATUS, RP0
ENDM


;******************************************************************************
; RESET VECTOR
;******************************************************************************

ORG 0x000 ; processor reset vector
goto main ; go to beginning of program



;******************************************************************************
; INTERRUPT VECTOR
;******************************************************************************

ORG 0x004 ; interrupt vector location
MOVWF w_temp ;Copy W to TEMP register
SWAPF STATUS, W ;Swap status to be saved into W
CLRF STATUS ;bank 0, regardless of current bank, Clears IRP,RP1,RP0
MOVWF status_temp ;Save status to bank zero STATUS_TEMP register
MOVF PCLATH, W ;Only required if using page 1
MOVWF pclath_temp ;Save PCLATH into W
CLRF PCLATH ;Page zero, regardless of current page

; **********************************************
; TMR1 Interrupt
; **********************************************
btfss PIR1,TMR1IF ; see if we timed out
goto finishInt ; no timer 1 overflow, finish interrupt
; handle timeout
bsf TOOCLOSE
bsf TIMEOUT
finishInt
bcf PIR1,TMR1IF ; clear the interrupt flag

MOVF pclath_temp, W ;Restore PCLATH
MOVWF PCLATH ;Move W into PCLATH
SWAPF status_temp, W ;Swap STATUS_TEMP register into W
;(sets bank to original state)
MOVWF STATUS ;Move W into STATUS register
SWAPF w_temp, F ;Swap W_TEMP
SWAPF w_temp, W ;Swap W_TEMP into W

retfie ; return from interrupt


;******************************************************************************
; Initialization Code
;******************************************************************************

init
bank0
clrf TMR1L
clrf TMR1H
movlw 0x00 ^ ( (1<<GIE) + (1<<PEIE) )
movwf INTCON ; enable global & peripheral interrupts
call initEyes
clrf ADCON0 ; all digital
bank1
movlw 0x00
movwf ANSEL ; go all digital
movlw b'01100010' ; bits for OSCCON internal clock at 4MHz 0110 0000
movwf OSCCON^0x080
movlw 0x00 ^ ( (1<<TMR1IE) ) ; enable TMR1 interrupts
movwf PIE1^0x080
bank0
return

initEyes
movlw 0x00 ^ ( (1<<T1CKPS0) + (1<<TMR1ON) ) ; (1<<T1CKPS0) + try at 1:2 prescale
movwf T1CON ; prescale 1:8 and enable tmr1
return

initSong
movlw 0x00 ^ ((1<<TMR1ON) ) ; prescaler 1:1
movwf T1CON
return

;******************************************************************************
; Sound Code
;******************************************************************************

playNote
movlw 250
movwf COUNTER
playItAgain
bsf SPEAKER
call playOnOff
bcf SPEAKER
call playOnOff
decfsz COUNTER,F
goto playItAgain
return

playOnOff
bcf PIR1,TMR1IF
bcf TIMEOUT
bsf T1CON,TMR1ON
movf NOTEHIGH,W
movwf TMR1H
movf NOTELOW,W
movwf TMR1L
waitForTimeout
btfss TIMEOUT
goto waitForTimeout

return

playE7
; play E7
movlw NOTEE7HIGH
movwf NOTEHIGH
movlw NOTEE7LOW
movwf NOTELOW
call playNote
call songDelay
return

playD7B
; play D7b
movlw NOTED7BHIGH
movwf NOTEHIGH
movlw NOTED7BLOW
movwf NOTELOW
call playNote
call songDelay
return

playB6
; play B6
movlw NOTEB6HIGH
movwf NOTEHIGH
movlw NOTEB6LOW
movwf NOTELOW
call playNote
call songDelay
call songDelay
return

; Zelda's Song ... kinda
playSong
bcf SPEAKER
call initSong ; tmr1 prescaler set to 1:1

movlw 3
movwf SONGCOUNTER
fromTop
call playE7
call playD7B
call playB6

decfsz SONGCOUNTER,F
goto fromTop

call playD7B
call playB6

call initEyes ; change prescaler back to 1:8
return


;******************************************************************************
; Ultrasonic Ranging Routines
;******************************************************************************
; This code takes a reading and stores the result in EyesLH. It also sets
; flag tooclose if there is a bad reading/under threshold

; after cueing the pulse, we have a total wait of 750us

takeRange
; clear TOOCLOSE flag
movlw 0x00 ; init EYESLH to zero
movwf EYESLH + 1
movwf EYESLH
bcf TOOCLOSE
; first, send init pulse to EYES
bank1
bcf EYESTRIS ; make EYES output enabled
bank0
bsf EYES
goto $ + 1 ; five ops at 4MHz gives 5us
goto $ + 1 ; we use 11us
goto $ + 1
goto $ + 1
goto $ + 1
nop
bcf EYES
bank1
bsf EYESTRIS ; make EYES input enabled
bank0
; now set everything up and wait for total of 750us
call delay750us

clrf TMR1L ; reset tmr1
clrf TMR1H
bcf PIR1,TMR1IF ; clear the overflow flag for tmr1
bcf TOOCLOSE ; clear tooclose flag
bsf T1CON,TMR1ON ; start the timer1

waitForRange
; get range and set TOOCLOSE flag, or return EYESLH.
; If max range, set EYESLH to max range value.
btfsc TOOCLOSE ; flag set by interrupt if we time out
goto gotTooClose
; TRIAL CODE - No idea if this works. Want to set allclear
; if TMR1H goes above 8
btfss TMR1H,7
goto keepChecking
btfsc TMR1H,0
goto setAllClear

keepChecking
btfsc EYES ; wait for send to be done
goto waitForRange
; if we get here, we have a range
; if not TOOCLOSE, set EYESLH
bcf T1CON,TMR1ON ; turn off timer1
movf TMR1L,W ; capture timer1 values in EyesLH
movwf EYESLH
movf TMR1H,W
movwf EYESLH + 1

; Test for reading less than THRESHOLD
; if high greater than THRESHOLDH(3), we're good, return
; if eyes greater than threshhold, result is negative, carry is clear, return value
movf EYESLH + 1,W
sublw THRESHOLDH ; test Eyes > Thresh by Thresh - Eyes
btfss STATUS,C ; if carry is clear, EYESLH is greater than THRESHOLDH
return ; return the good reading

; if high less than THRESHOLDH(3), gotTooClose
; if eyes less than threshold, set flag and return
movlw THRESHOLDH
subwf EYESLH + 1,W ; test Eyes < Thresh by Eyes - Thresh
btfss STATUS,C ; if carry set, EYESLH is less than THRESHOLDH
goto gotTooClose

; if eyes high equals THRESHOLDH(3), check THRESHOLDL
; if eyes low is less than threshold low, set flag and return
movlw THRESHOLDL
subwf EYESLH,W ; test EYESL < THRESHL by EYESL - THRESHL
btfss STATUS,C ; if carry is set, EYESL is less than THRESHL
goto gotTooClose
return

setAllClear
; we hit max range. set EyesLH to max value
movlw MAXRANGEH
movwf EYESLH + 1
movlw MAXRANGEL
movwf EYESLH
return

gotTooClose
;redo this for straight, left, and right reading
; set EYESLH to zero and set TOOCLOSE flag
bsf TOOCLOSE
movlw 0x00
movwf EYESLH + 1
movwf EYESLH
return

;******************************************************************************
; Servo Neck Code
;******************************************************************************
; Generic code to move the servo "Neck"

moveServo
movlw PULSEDELAY
movwf DELAY
movf PULSE,W
movwf PULSE + 1
call pulseOut
decfsz DELAY,F
goto $ - 4
return

; pulses the servo. 20ms total cycle, duty cycle determines position.
; value in pulse will determine time pulse is high
; value of one = .5 ms, 2 = 1ms, etc
; servo needs 6 delay values:
; 10 o'clock needs 2.0ms on and 18.0ms off pulse = 4 2000 and 18000 instructions
; 12 o'clock needs 1.5ms on and 18.5ms off pulse = 3 1500 and 18500 instructions
; 2 o'clock needs 1.0ms on and 19.0ms off pulse = 2 1000 and 19000 instructions
pulseOut
movlw PULSECOUNT
movwf PDELAY + 1
; cheezy hack til I redo this: add one to pulse
incf PULSE + 1,F
bsf SERVO ; start sending high to servo

pulseLoop
decf PULSE + 1,F ; every 500us, decrement pulse
btfsc STATUS,Z
bcf SERVO ; if zero send low to servo

movlw 99
movwf PDELAY
halfMs ; 500us delay inner loop
decf PDELAY,F
nop
btfss STATUS,Z
goto $ - 3
decfsz PDELAY + 1,F
goto pulseLoop
return

;******************************************************************************
; Look Code
;******************************************************************************
; This code looks around, moving the neck and taking readings

lookAhead
; move to 12 o'clock
movlw 3
movwf PULSE
call moveServo
; take reading
call takeRange ; puts reading in EYESLH
return

lookLeft
; move to 10 o'clock
movlw 4
movwf PULSE
call moveServo
; take reading and store in LEFTLH
call takeRange
movf EYESLH + 1,W
movwf LEFTLH + 1
movf EYESLH,W
movwf LEFTLH
return

lookRight
; move to 2 o'clock
movlw 2
movwf PULSE
call moveServo
; take reading, store in RightLH
call takeRange
movf EYESLH + 1,W
movwf RIGHTLH + 1
movf EYESLH,W
movwf RIGHTLH
return

;******************************************************************************
; Decision Code
;******************************************************************************
; This code compares readings to determine next direction

determineNewDirection
; figure out which way to turn: left, right, or uturn
call allStop ; stop moving till we know where to go
call playSong
movlw UTURN
movwf DIRECTION
call lookLeft ; if too close, dir stays zero
btfss TOOCLOSE ; else dir is cangoleft
incf DIRECTION,F

call lookRight
btfsc TOOCLOSE
goto pickATurn
movlw CANGORIGHT ; add 2 to direction
addwf DIRECTION,F ; it will be 2 if right only, 3 if both
pickATurn
movf DIRECTION,W
sublw LEFTORRIGHT
btfsc STATUS,Z ; dir = leftorright ? goto compareLeftRight
goto compareLeftRight
movf DIRECTION,W
sublw CANGOLEFT
btfsc STATUS,Z ; dir = left ? turn left
goto turnLeft
btfss STATUS,C ; dir = 0 ? uturn
goto turnRight ; result was 1-2=-1, negative, carry clear
goto uturn ; result was 1-0=0, positive, carry set
return ; safety return

compareLeftRight
movf LEFTLH + 1,W ; right - left, test if negative
subwf RIGHTLH + 1,W ; right >= left high? turn right : turn left
btfss STATUS,C ; if carry set, right is higher
goto turnLeft
goto turnRight
return ; safety return

;******************************************************************************
; Move Code
;******************************************************************************
; This code controls the motors: straight, left turn, right turn, and turnaround

allStop
; stop all motors and figure things out
; debug code: clear all motor leds
movlw 0x00
movwf MOTORPORT
return

moveAhead
; all clear ahead. Move forward
movlw 0x00 ^ ( ( 1 << MOTORLEFTONE ) + ( 1 << MOTORRIGHTONE ) )
movwf MOTORPORT
return

turnLeft
; turn left ~45 degrees?
movlw 0x00 ^ ( ( 1 << MOTORLEFTTWO ) + ( 1 << MOTORRIGHTONE ) )
movwf MOTORPORT
call motorDelay
clrf PORTB
return

turnRight
; turn right ~45 degrees?
movlw 0x00 ^ ( ( 1 << MOTORLEFTONE ) + ( 1 << MOTORRIGHTTWO ) )
movwf MOTORPORT
call motorDelay
clrf PORTB
return

uturn
; turn left or right ~180 degrees. direction alternates on each call
btfss UTURNLEFT
goto uRight

uLeft
bcf UTURNLEFT ; togggle for different uturn next time
call turnLeft
call turnLeft
return

uRight
bsf UTURNLEFT ; togggle for different uturn next time
call turnRight
call turnRight
return

;******************************************************************************
; Delay Code
;******************************************************************************

delay750us
movlw 148 ; 750us less the 5us for set up, above
movwf DELAY
decf DELAY,F
nop
btfss STATUS,Z
goto $ - 3
nop
return

oneSecDelay
movlw 0x3 ; 0x3 is about 1 sec at 4 MHz
movwf DELAY + 2
outerLoop ; (254*5+8) * 255 + 2 = 325892 instructions
movlw 0xff ; at 4MHz is about 1/3 of a second, .065 sec at 20MHz
movwf DELAY
movlw 0xff
movwf DELAY + 1
loop
decf DELAY, F
btfsc STATUS, Z
decfsz DELAY + 1, F
goto loop
decfsz DELAY + 2, F
goto outerLoop
return

songDelay
movlw 0xff ; at 4MHz is about 1/3 of a second, .065 sec at 20MHz
movwf DELAY
movlw 0xB0
movwf DELAY + 1
thirdLoop
decf DELAY, F
btfsc STATUS, Z
decfsz DELAY + 1, F
goto thirdLoop
nop
nop
return

motorDelay ; roughly 1/10 of a sec at 4 MHz
movlw 0xC8
movwf DELAY
movlw 0x0A
movwf DELAY + 1
tenthLoop
decf DELAY, F
btfsc STATUS, Z
decfsz DELAY + 1, F
goto tenthLoop
return

;******************************************************************************
; Mainline Code
;******************************************************************************

main
call init
bank1
; PORTB is for the motors: all output
movlw 0x00
movwf TRISB ^ 0x080 ; enable all PORTB for output
; eyes and neck. All intially output
movlw 0x00
movwf TRISA ^ 0x080 ; enable all PORTA for output
bank0
bcf EYES ; assert low on EYES so no accidental pulse goes out
clrf MOTORPORT ; turn off motors
explore
call lookAhead
btfsc TOOCLOSE
call determineNewDirection ; we got too close, turn to new direction
call lookAhead
call moveAhead ; move forward
goto explore

END

Robot Version 2.2 - Working

    Kids and wife are away this week.  I had a lot of quiet time to play with the robot.  I've decided to name it KiBHuJu ( Kit Bashed Hunk of Junk ).  I added the ( duh ) line of code to stop the motor as soon as the turn delay was finished.  That helped the doughnuts a bit.  Then, I pulled out one of the AA batteries and replaced it with a piece of antenna as a bridge.  Now, with the 3V motors, it moves a lot slower. 
    I went to my sister's for dinner tonight.  My nephew just finished robot camp.  They create robots with the Lego system ( Mindstorm ? Something like that ).  I decided to bring my robot along and show him.  My niece loved it.  She played with it for a good part of the evening.  And then. . . the same stupid contact to the motor sheared off.  I now have one working 6V motor and one 3V. 
    When I got home, I decided to rip the broken 3V motor apart.  The brush for the motor is just a flat piece of copper that nests in a piece of plastic.  I pulled both out.  I stripped and flattened some solid 22 gauge wire and wedged it in the plastic.  Now, my brushes are one with the lead wires.  I did the same with the 6V motor.  Now, all my motors are working again.  I just need to decide if I want to proactively redo the others.  I will probably wait till they break.  At least I now know how to fix them.
    My new board came out great.  Now, instead of a jumble of soldered, one use mess under the board, I have a plugged in mess on top.  But I can use the board for lots of other projects.  I can also put in a header that will allow me to connect other boards to it. 
    My next goal is to put in a header for ICSP.  I'm going back to the bootloader.  I think I will essentially use the tinyBld program I found.  The actual PC code is for Windoze, though.  I'll write my own Python version.  I'll just change the bootloader code enough to work with the Python program.  The problem is that the bootloader code wants to use port B pins 2 and five for the built in USART module.  I may rewrite it ( making it longer ) to use the port A pins.  I'll just use bit banging routines instead of the built in USART.  I don't care if it is longer than 100 lines.  I have yet to come near the max program RAM. 
    Still working on the CAD.  Eagle is a great tool.  I just haven't put the time in.  Tried to get my phone to connect to my laptop.  No dice.  I've got a Razr.  For Linux, moto4lin is the way to go.  My version, V3re, is not supported.  I'd like to be able to take some pictures and post them.  Maybe when my wife gets back, I'll use the camera.  All good. 

Robot Working... Sort of...

    Well, I blew a lot of time soldering my proto board this weekend.  I learned a lot.  First of all, I really stink at soldering!  I spent some time reviewing my board.  I now have a design for a proto board that will reduce the amount of jumpers I need to solder.  I'm going to buy one of the Radio Shack boards that have some holes connected.  I ordered a slew of female header pins, some chip sockets, etc from Sparkfun.  The result should be a board with two 18 pin sockets, a piezo speaker, a 5 V regulator, and some additional headers.  I'll have 2 2-pin header with 0.1 uF caps for the motor drivers to cancel noise.  I'll have 2 3-pin headers for signal positive and ground.  This board should be able to be reused for multiple projects.  The idea is to have everything connected to female jumpers.  That way, I can just stick wires in where I need them.  Any project specific parts can be either soldered in or I'll add jumpers.  The other nice thing about female jumpers is that I reduce the risk of frying parts.
    I ran into quite a few challenges this weekend.  Other than soldering wires to the wrong spots, I also had a few mechanical failures.  I ordered 2 geared motors from Solarbotics.  Right after I soldered 22 gauge solid wires to them, one of the leads snapped.  It sheared so close to the join that there was no hope of a fix.  Aaaagh!  I dropped back and punted.  I pulled the motors out and replaced them with the only spares I had: two 3 Volt motors with gear teeth from Radio Shack.  When I tested them on my breadboard, they wreaked havoc.  They put a ton of noise on the line.  I put 0.1 uF caps across their positive and negative leads to cancel the noise. 
    Once I got the rest of the problems worked out, I hooked it all up and went for a test ride.  The code worked great.  The only problem was the switch from 6 Volt motors to 3 Volt.  When the robot decided to turn, it peeled out, doing doughnuts!  I need to gear the turn delay time Waaaaaaaaay down. 
    I see that I can upload images here.  I'll take images of the final code when I get it and upload that.  Once I build my good proto board, I'll upload images of that, too.  I downloaded a freeware version of Eagle CAD.  I think I should be able to use that to generate a good schematic.  I am just getting familiar with it.  TAFN!

Warning - Paste Error!

Arrrrgghh!  Paste is not pasting my full code in.  Do not try to compile my code.  For some reason, part of my code are not appearing.  I'll try and fix this as soon as possible.  Sorry!

Sidetracked Again - Robots!!!!

    Got side tracked again...and again.  Work, vacation, and now... robots!  I found a great website on robots, including a beginners project.  The site is Let's Make Robots, the beginner's tutorial is: Let's Make Robots: Start Here.  Since I program in high level languages all day, every day, I wanted to mod this to do it in assembly with a Pic microcontroller rather than a PicAxe in PicBasic.  I ordered some parts ( motors, wheels, and motor driver ) from SolarBotics.  I couldn't wait for the parts, so I went to Radio Shack and bought a Ping Ultrasonic sensor for the eyes.  I used a Pic 16f88 for its internal 4 MHz clock and Timer1 16 bit timer functionality.  I was able to get readings on the sensor from 2cm to 1 meter in 500mm increments!.  Below, I will attach the code I stepped through to get the parts working.
    First, I got the "neck" working.  This is a standard, Parallax servo.  It only needs to look straight ahead, left ( about 10 o'clock ) and right ( about 2 o'clock ).  Then, I got the eyes to take ranges, so I could figure out the values and how sensitive it was.  I settled on one foot as the "threshold" of danger.  I may change this once the robot has "feet".  I then coded what I call the "head".  This has the eyes and neck working together.
    The head program looks straight ahead and takes a bearing.  If all is clear, it moves forward.  Loop.  If the range is under the threshold, it determines a new direction.  It looks left and takes a bearing.  It looks right and takes a bearing.  It then figures out the most open route and turns in that direction.  If both left and right are under the threshold, it makes a uturn.  The code below is debug code.  It just turns on leds where the motor signals would be.  Everything is working as expected.
    I am still waiting on the motors and driver.  While I wait, I think I will add a piezo-electric speaker to the project.  I would like to play a tune when I reach a decison point ( maybe the original Super Mario Brothers ? My kids will love that ).  I'll turn.  Then, when I decide left or right, I'll continue the theme song for a bit before resuming.  If I have to uturn, I'd like to play the classic big truck backing up sound ( beep, beep, beep ).

Note:  I played around alot with this code.  Pay attention to the changes in the ports and pin assignmentes.  They are in the define statements.  Enjoy!  I am like a kid at Christmas.  I can't believe they came together so quickly and easily.  Please, please, please, point out any flaws in this code.  I'm used to code reviews and appreciate any helpful feedback!

Note2:  For the ranger code, I hooked up to serial port ttyS0 and used some simple python code to read the messages.  The connection is direct to the serial port, and it does not need an inverter like a Max232.  I set the transmit speed nice and low at 2400 baud.  The python program uses the uspp code ( thanks to author - need to provide reference ).  Here is my code:

#! /usr/bin/python
from uspp import *

# COM1 is initialized at 9600 baud. The
# default data format is 8N1

s = SerialPort("/dev/ttyS0", None, 2400)
s.flush() # discard unread bytes

#print ord(s.read()) # s.read() returns a one-character
                    # string. We convert it into its ascii
                    # value
strg = ''
num = False

while 1:
        ch = s.read()
        if '\n' == ch:
                print strg
                strg=''
        elif '"' == ch:
                if False == num:
                        num = True
                else:
                        num = False    
        else:
                if True == num:
                        strg = strg + str( ord(ch) )
                else:
                        strg = strg + ch
        # ch=''
        #print 'char: ', ch, '   ordinal: ', ord(ch), ' binary: ', bin(ord(ch))
        #str = str + ch
        #print str

    Here's the servo "neck" code:

;******************************************************************************
;                                                                             *
;      Filename:  servo.asm                                                   *                           
;          Date:  2010.06.12                                                  *
;  File Version:  1.0.0                                                       *
;                                                                             *
;        Author:  Tom Hunt                                                    *
;                                                                             *
;******************************************************************************
;  NOTES: Rotate a Parallax Standard Servo through its paces.              *
;         First attempt will not use interrupts, tmr0 or a chip with CCP/PWM. *
;          If I'm feeling lucky, I'll try that later.              *
;                                                                             *
;******************************************************************************
;  CHANGE LOG:                                                                *
;                                                                             *
;                                                                             *
;******************************************************************************

   list      R=DEC, p=16f88           ; list directive to define processor
   #include         ; processor specific variable definitions
    ; _INTRC_IO  _HS_OSC
        ;Program Configuration Register 1
        __CONFIG    _CONFIG1, _CP_OFF & _CCP1_RB0 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_ON & _PWRTE_ON & _WDT_OFF & _INTRC_IO

        ;Program Configuration Register 2
        __CONFIG    _CONFIG2, _IESO_OFF & _FCMEN_OFF

;******************************************************************************
;  CONSTANT DEFINITIONS                                                       *
;******************************************************************************

#define        SERVO        PORTB,2
#define        PULSECOUNT    40

;******************************************************************************
;  VARIABLE DEFINITIONS                                                       *
;******************************************************************************
    CBLOCK 0x20
pulse:2
pdelay:2
dlay:3
    ENDC


    CBLOCK    0x70     ; interupt context-saving vars here see datasheet chp 15
w_temp
status_temp
pclath_temp
    ENDC

;******************************************************************************
;  MACRO DEFINITIONS                                                       *
;******************************************************************************
bank0    MACRO
    bcf    STATUS, RP1
    bcf    STATUS, RP0
    ENDM

bank1    MACRO
    bcf    STATUS, RP1
    bsf    STATUS, RP0
    ENDM

;******************************************************************************
;  RESET VECTOR                                                               *
;******************************************************************************
      ORG     0x000             ; processor reset vector
      goto    main              ; go to beginning of program

;******************************************************************************
;  INTERRUPT VECTOR                                                           *
;******************************************************************************
      ORG     0x004             ; interrupt vector location
    MOVWF  w_temp                 ;Copy W to TEMP register
    SWAPF  STATUS, W              ;Swap status to be saved into W
    CLRF   STATUS                 ;bank 0, regardless of current bank, Clears IRP,RP1,RP0
    MOVWF  status_temp            ;Save status to bank zero STATUS_TEMP register
    MOVF   PCLATH, W              ;Only required if using page 1
    MOVWF  pclath_temp            ;Save PCLATH into W
    CLRF   PCLATH                 ;Page zero, regardless of current page
    ;
    ;(ISR)                ;(Insert user code here)
    ;
    MOVF   pclath_temp, W         ;Restore PCLATH
    MOVWF  PCLATH                 ;Move W into PCLATH
    SWAPF  status_temp, W         ;Swap STATUS_TEMP register into W
                                  ;(sets bank to original state)
    MOVWF  STATUS                 ;Move W into STATUS register
    SWAPF  w_temp, F              ;Swap W_TEMP
    SWAPF  w_temp, W              ;Swap W_TEMP into W

    retfie                    ; return from interrupt

;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
;           Intitialization
;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

init
    clrf    INTCON   
    bank1
    movlw    0x00
    movwf    ANSEL                ; go all digital
    movlw    b'01100010'            ; bits for OSCCON internal clock at 4MHz 0110 0000
    movwf    OSCCON^0x80
    movlw    0x000
    movwf    TRISB ^ 0x80        ; port B all output
    bank0
    return

;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
;            Code Section
;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
;            MoveServo Method
;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

moveservo
    movlw    0x040
    movwf    dlay
    movf    pulse,W
    movwf    pulse+1
    call    pulseout
    decfsz    dlay,F
        goto    $ - 4
    return

;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
;            Pulseout Method
;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

; pulses the servo.  20ms total cycle, duty cycle determines position.
; value in pulse will determine time pulse is high
; value of one = .5 ms, 2 = 1ms, etc
; servo needs 6 delay values:
; 10 o'clock needs 2.0ms on and 18.0ms off  pulse = 4  2000  and 18000 instructions
; 12 o'clock needs 1.5ms on and 18.5ms off  pulse = 3  1500  and 18500 instructions
;  2 o'clock needs 1.0ms on and 19.0ms off  pulse = 2  1000  and 19000 instructions
pulseout
    movlw    PULSECOUNT
    movwf    pdelay + 1
    ; cheezy hack til I redo this: add one to pulse
    incf    pulse+1,F
    bsf        SERVO            ; start sending high to servo

pulseloop
    decf    pulse+1,F            ; every 500us, decrement pulse
    btfsc    STATUS,Z
        bcf    SERVO            ; if zero send low to servo
   
    movlw    99
    movwf    pdelay
halfms                        ; 500us delay inner loop
    decf    pdelay,F
    nop
    btfss    STATUS,Z
        goto     $ - 3
    decfsz    pdelay + 1,F
        goto    pulseloop
    return

;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
;            Main Method
;&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&

main
    call    init

    ; move to 12 o'clock
    movlw    3
    movwf    pulse
    call    moveservo ;pulseout
    ; pause one second
    call    onesec

    ; move to 10 o'clock
    movlw    2
    movwf    pulse
    call    moveservo ;pulseout
    ; pause one second
    call    onesec
   
    ; move to 2 o'clock
    movlw    4
    movwf    pulse
    call    moveservo ;pulseout
    ; pause one second
    call    onesec

    ; move to 12 o'clock
    movlw    3
    movwf    pulse
    call    moveservo ;pulseout
    goto    $


onesec
    movlw    0x3          ; 0xF is about 1 sec at 20 MHz ; 0x8 is .5 sec
    movwf    dlay + 3
outerLoop                ; (254*5+8) * 255 + 2 = 325892 instructions
    movlw     0xff        ; at 4MHz is about 1/3 of a second, .065 sec at 20MHz
    movwf     dlay
    movlw    0xff       
    movwf   dlay + 1
loop
    decf    dlay, F       
    btfsc    STATUS, Z
       decfsz    dlay + 1, F
               goto    loop
       decfsz    dlay + 3, F
               goto    outerLoop
       return

END                          ; directive 'end of program'



    Here's the range taking code:

;******************************************************************************
;                                                                            
;      Filename:  ranger.asm                                                                                
;          Date:  2010.06.13                                                  
;  File Version:  1.0.0                                                       
;                                                                             
;        Author:  Tom Hunt                                                   
;                                                                             
;******************************************************************************
;  NOTES: Pic 16f88, running @ 4MHz internal clock.                           
;          Test of the Ping)) Ultrasonic Range Senor.
;         The goal is to send out a pulse and send the value to PC via RS232                                 
;                                                                            
;******************************************************************************
;  CHANGE LOG:                                                               
;                                                                            
;                                                                            
;******************************************************************************

   list      R=DEC, p=16f88           ; list directive to define processor
   #include         ; processor specific variable definitions
    ; _INTRC_IO  _HS_OSC
        ;Program Configuration Register 1
        __CONFIG    _CONFIG1, _CP_OFF & _CCP1_RB0 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_ON & _PWRTE_ON & _WDT_OFF & _INTRC_IO

        ;Program Configuration Register 2
        __CONFIG    _CONFIG2, _IESO_OFF & _FCMEN_OFF

; '__CONFIG' directive is used to embed configuration data within .asm file.
; The lables following the directive are located in the respective .inc file.
; See respective data sheet for additional information on configuration word.

;******************************************************************************
;  CONSTANT DEFINITIONS                                                      
;******************************************************************************

#define                timedout    flags,0
#define    TX            PORTB,2
#define BUTTONBIT    3
#define BUTTON        PORTB,BUTTONBIT
#define    EYES        PORTA,3
#define EYESTRIS    TRISA^0x080,3

;******************************************************************************
;  VARIABLE DEFINITIONS                                                      
;******************************************************************************

    CBLOCK 0x20
Byte        ; holds the byte to send
Count        ; holds the counter for start, stop and data bits ( 10 )
EyesLH:2    ; holds the time of a pulse back from eyes
LeftLH:2    ; holds the time of the pulse back from the "eyes" for left look
RightLH:2    ; holds the time of the pulse back from the "eyes" for right look
delay:2
    ENDC

    CBLOCK    0x70     ; interupt context-saving vars here see datasheet chp 15
flags
w_temp
status_temp
pclath_temp
    ENDC



;******************************************************************************
;  MACRO DEFINITIONS                                                         
;******************************************************************************
bank0    MACRO
    bcf    STATUS, RP1
    bcf    STATUS, RP0
    ENDM

bank1    MACRO
    bcf    STATUS, RP1
    bsf    STATUS, RP0
    ENDM

;******************************************************************************
;  RESET VECTOR                                                              
;******************************************************************************
    ORG     0x000             ; processor reset vector
    goto    main              ; go to beginning of program

;******************************************************************************
;  INTERRUPT VECTOR                                                          
;******************************************************************************
    ORG     0x004             ; interrupt vector location
    MOVWF  w_temp                 ;Copy W to TEMP register
    SWAPF  STATUS, W              ;Swap status to be saved into W
    CLRF   STATUS                 ;bank 0, regardless of current bank, Clears IRP,RP1,RP0
    MOVWF  status_temp            ;Save status to bank zero STATUS_TEMP register
    MOVF   PCLATH, W              ;Only required if using page 1
    MOVWF  pclath_temp            ;Save PCLATH into W
    CLRF   PCLATH                 ;Page zero, regardless of current page

;    **********************************************
;                TMR1 Interrupt
;    **********************************************
    btfss    PIR1,TMR1IF            ; see if we timed out
        goto    finishInt        ; no timer 1 overflow, finish interrupt
    ; handle timeout
    bsf        timedout
       

finishInt
    bcf        PIR1,TMR1IF            ; clear the interrupt flag

    MOVF   pclath_temp, W         ;Restore PCLATH
    MOVWF  PCLATH                 ;Move W into PCLATH
    SWAPF  status_temp, W         ;Swap STATUS_TEMP register into W
                                  ;(sets bank to original state)
    MOVWF  STATUS                 ;Move W into STATUS register
    SWAPF  w_temp, F              ;Swap W_TEMP
    SWAPF  w_temp, W              ;Swap W_TEMP into W
    retfie                    ; return from interrupt

;******************************************************************************
;  Initialization CODE                                                       
;******************************************************************************

init
    bank0
    clrf    TMR1L
    clrf    TMR1H
    movlw    0x00 ^ ( (1<<
    movwf    INTCON                                ; enable global & peripheral interrupts
    movlw    0x00 ^ ( (1<<<
    movwf    T1CON                                ; prescale 1:8 and enable tmr1
    clrf    ADCON0                                ;  all digital
    bank1
    movlw    0x00
    movwf    ANSEL                                ; go all digital
    movlw    b'01100010'                            ; bits for OSCCON internal clock at 4MHz 0110 0000
    movwf    OSCCON^0x080               
    movlw    0x00 ^ ( (1<
    movwf    PIE1^0x080
    bank0
    return

;******************************************************************************
;  Ultrasonic Ranging Routines                                                       
;******************************************************************************

; after cueing the pulse, we have a total wait of 750us

takeRange
    ; first, send init pulse to EYES
    bank1
    bcf        EYESTRIS     ; make EYES output enabled
    bank0
    bsf        EYES
    goto    $ + 1        ; five ops at 4MHz gives 5us
    goto    $ + 1   
    goto    $ + 1
    goto    $ + 1
    goto    $ + 1
    nop
    bcf        EYES
    bank1
    bsf        EYESTRIS     ; make EYES input enabled
    bank0
    ; now set everything up and wait for total of 750us
    call    delay750us
   
    clrf    TMR1L            ; reset tmr1
    clrf    TMR1H
    bcf        PIR1,TMR1IF        ; clear the overflow flag for tmr1
    bcf        timedout        ; clear timedout flag
    bsf        T1CON,TMR1ON    ; start the timer1
   
waitForRange
    btfsc    timedout
        goto    badReading

    btfsc    EYES            ; wait for send to be done
        goto $ - 3
    ; if we get here, we have a range
    ; if not timedout, set EyesLH
    bcf        T1CON,TMR1ON      ; turn off timer1
    movf    TMR1L,W            ; capture timer1 values in EyesLH
    movwf    EyesLH
    movf    TMR1H,W
    movwf    EyesLH + 1
   
    ; Test for reading less than 15
    movlw    0                    ; Eyes High - zero
    subwf    EyesLH + 1, W
    btfss    STATUS,Z            ; if zero flag, then it is zero and test for 15 in low
        goto    writeRange        ; if zero not set, we can skip to write the range
    ; test for Eyes low less than 15
    movlw    15
    subwf    EyesLH,W            ; if carry is clear, eyes value is too low.
    btfss    STATUS,C            ; if carry is set, fall into writeRange
        goto    lessThanMin

writeRange
    ; write value to RS232
    movlw    'H'
    call    transmitSerial
    movlw    ':'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    0x22                ; quote
    call    transmitSerial
    movf    EyesLH + 1,W
    call    transmitSerial
    movlw    0x22                ;quote
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    'L'
    call    transmitSerial
    movlw    ':'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    0x22                ; should be a quote
    call    transmitSerial
    movf    EyesLH,W
    call    transmitSerial
    movlw    0x22                ; should be a quote
    call    transmitSerial
    movlw    '\n'
    call    transmitSerial
    return
   
badReading
    movlw    'O'
    call    transmitSerial
    movlw    'u'
    call    transmitSerial
    movlw    't'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    'o'
    call    transmitSerial
    movlw    'f'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    'r'
    call    transmitSerial
    movlw    'a'
    call    transmitSerial
    movlw    'n'
    call    transmitSerial
    movlw    'g'
    call    transmitSerial
    movlw    'e'
    call    transmitSerial
    movlw    '\n'
    call    transmitSerial
    return

lessThanMin
    movlw    'l'
    call    transmitSerial
    movlw    'e'
    call    transmitSerial
    movlw    's'
    call    transmitSerial
    movlw    's'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    't'
    call    transmitSerial
    movlw    'h'
    call    transmitSerial
    movlw    'a'
    call    transmitSerial
    movlw    'n'
    call    transmitSerial
    movlw    ' '
    call    transmitSerial
    movlw    '1'
    call    transmitSerial
    movlw    '5'
    call    transmitSerial
    movlw    '\n'
    call    transmitSerial
    return

;******************************************************************************
;  RS232 ROUTINES                                                        
;******************************************************************************

transmitSerial
    movwf    Byte
    movlw    10
    movwf    Count
    bcf        STATUS, C
    call    sendByte
    return

sendByte
    ; This first bit test is inverted to allow it to work with a straight wire to the pc
    ; If using a max232, this line should be btfsc STATUS, C
    btfss    STATUS, C        ; send the bit in carry
        goto    $ + 4
    nop
    bcf        TX                ; send a LOW
    goto    $ + 3
    bsf        TX                ; send a HIGH
    goto    $ + 1
    call    bitDelay        ; wait for bit period
    bsf        STATUS, C        ; now we pre-set carry with one
    rrf        Byte, F            ; rotate the next bit into the carry
    decfsz    Count, F
        goto    sendByte

bitDelay
    movlw    82                ; 205 for 20Mhz 38 for 4Mhz   try 81 and 3 nops for 2400
    addlw    0x0FF            ; take one away from counter
    nop                        ; need five instructions in loop
    btfss    STATUS, Z
        goto    $ - 3
    ;goto    $ + 1
    nop
                            ; try 417 total for 2400 baud at 4MHz
                            ; total delay is 1028 needed for 4800 baud at 20MHz
                            ; or to 208 for 4800 baud at 4MHz
    return

;******************************************************************************
;  Delay Code                                                             
;******************************************************************************

delay750us
    movlw    148            ; 750us less the 5us for set up, above
    movwf    delay
    decf    delay,F
    nop
    btfss    STATUS,Z
        goto    $ - 3
    nop
    return
   

;******************************************************************************
;  Mainline Code                                                             
;******************************************************************************

main
    bsf        TX
    call    init
    bank1
    movlw    0x00 ^ ( 1 << BUTTONBIT )
    movwf    TRISB ^ 0x080    ; enable all PORTB for output
    movlw    0
    movwf    TRISA ^ 0x080    ; enable all PORTA for output
    bank0
    bcf        EYES            ; assert low on EYES so no accidental pulse goes out
    movlw    'R'
    call    transmitSerial
    movlw    'e'
    call    transmitSerial
    movlw    'a'
    call    transmitSerial
    movlw    'd'
    call    transmitSerial
    movlw    'y'
    call    transmitSerial
    movlw    '\n'
    call    transmitSerial   
;loop   
;    btfsc    BUTTON
        call    takeRange
    ; call    delay50ms
;    goto    loop
    goto    $

END


    Here's the integrated "head" code:

;******************************************************************************
;                                                                            
;      Filename:  head.asm                                                                                
;          Date:  2010.06.28                                                  
;  File Version:  1.0.0                                                       
;                                                                             
;        Author:  Tom Hunt                                                   
;                                                                             
;******************************************************************************
;  NOTES: Pic 16f88, running @ 4MHz internal clock.                           
;          Integrate ping ultrasonic sensor and servo "neck"
;         Test look ahead, stop on block ( under threshold ), look left and
;          take reading, look right and take reading.  Then find most open way.
;          If no open way ( all under threshold ), do 180 degree turn.
;          Motor operation will be simulated with leds.
;
;          Eyes on RA0
;          Neck on RA1
;          Motor1 on RB6-7
;          Motor2 on RB4-5
;
;          TODO - Need a cap for maximum range on sensor.  If TMR1H > 8, set
;                 EyesLH to max value, indicating wide open space.
;                 I'm not sure this will work, but I need something to tell
;                 if we've gone beyond maximum range. Google time....
;        
;          Stupid note to self: CARRY SET MEANS POSITIVE YOU IDIOT!!!!
;                                                                
;******************************************************************************
;  CHANGE LOG:                                                               
;                                                                            
;                                                                            
;******************************************************************************

   list      R=DEC, p=16f88           ; list directive to define processor
   #include         ; processor specific variable definitions
    ; _INTRC_IO  _HS_OSC
        ;Program Configuration Register 1
        __CONFIG    _CONFIG1, _CP_OFF & _CCP1_RB0 & _DEBUG_OFF & _WRT_PROTECT_OFF & _CPD_OFF & _LVP_OFF & _BODEN_OFF & _MCLR_ON & _PWRTE_ON & _WDT_OFF & _INTRC_IO

        ;Program Configuration Register 2
        __CONFIG    _CONFIG2, _IESO_OFF & _FCMEN_OFF

; '__CONFIG' directive is used to embed configuration data within .asm file.
; The lables following the directive are located in the respective .inc file.
; See respective data sheet for additional information on configuration word.

;******************************************************************************
;  CONSTANT DEFINITIONS                                                      
;******************************************************************************

#define        TOOCLOSE        FLAGS,0
#define        UTURNLEFT        FLAGS,1
#define        EYES            PORTA,0
#define     EYESTRIS        TRISA^0x080,0
#define        MOTORPORT        PORTB
#define        MOTORRIGHTONE    7
#define        MOTORRIGHTTWO    6
#define        MOTORLEFTONE    5
#define        MOTORLEFTTWO    4
#define        SERVO            PORTA,1
#define        PULSECOUNT        40
#define        PULSEDELAY        0x040
#define        UTURN            0
#define        CANGOLEFT        1
#define        CANGORIGHT        2
#define        LEFTORRIGHT        3
#define        MAXRANGEH        8
#define        MAXRANGEL        143   
#define        THRESHOLDH        3
#define        THRESHOLDL        143        ; threshold equates to ~1 foot

;******************************************************************************
;  VARIABLE DEFINITIONS                                                      
;******************************************************************************

    CBLOCK 0x20
EYESLH:2    ; holds the time of a pulse back from eyes
LEFTLH:2    ; holds the time of the pulse back from the "eyes" for left look
RIGHTLH:2    ; holds the time of the pulse back from the "eyes" for right look
DIRECTION    ; next direction to take
DELAY:3
PULSE:2
PDELAY:2
    ENDC

    CBLOCK    0x70     ; interupt context-saving vars here see datasheet chp 15
FLAGS
w_temp
status_temp
pclath_temp
    ENDC



;******************************************************************************
;  MACRO DEFINITIONS                                                         
;******************************************************************************
bank0    MACRO
    bcf    STATUS, RP1
    bcf    STATUS, RP0
    ENDM

bank1    MACRO
    bcf    STATUS, RP1
    bsf    STATUS, RP0
    ENDM

;******************************************************************************
;  RESET VECTOR                                                              
;******************************************************************************
    ORG     0x000             ; processor reset vector
    goto    main              ; go to beginning of program

;******************************************************************************
;  INTERRUPT VECTOR                                                          
;******************************************************************************
    ORG     0x004             ; interrupt vector location
    MOVWF  w_temp                 ;Copy W to TEMP register
    SWAPF  STATUS, W              ;Swap status to be saved into W
    CLRF   STATUS                 ;bank 0, regardless of current bank, Clears IRP,RP1,RP0
    MOVWF  status_temp            ;Save status to bank zero STATUS_TEMP register
    MOVF   PCLATH, W              ;Only required if using page 1
    MOVWF  pclath_temp            ;Save PCLATH into W
    CLRF   PCLATH                 ;Page zero, regardless of current page

;    **********************************************
;                TMR1 Interrupt
;    **********************************************
    btfss    PIR1,TMR1IF            ; see if we timed out
        goto    finishInt        ; no timer 1 overflow, finish interrupt
    ; handle timeout
    bsf        TOOCLOSE

finishInt
    bcf        PIR1,TMR1IF            ; clear the interrupt flag

    MOVF   pclath_temp, W         ;Restore PCLATH
    MOVWF  PCLATH                 ;Move W into PCLATH
    SWAPF  status_temp, W         ;Swap STATUS_TEMP register into W
                                  ;(sets bank to original state)
    MOVWF  STATUS                 ;Move W into STATUS register
    SWAPF  w_temp, F              ;Swap W_TEMP
    SWAPF  w_temp, W              ;Swap W_TEMP into W
    retfie                    ; return from interrupt

;******************************************************************************
;  Initialization Code                                                     
;******************************************************************************

init
    bank0
    clrf    TMR1L
    clrf    TMR1H
    movlw    0x00 ^ ( (1<<
    movwf    INTCON                                ; enable global & peripheral interrupts
    movlw    0x00 ^ ( (1<<<
    movwf    T1CON                                ; prescale 1:8 and enable tmr1
    clrf    ADCON0                                ;  all digital
    bank1
    movlw    0x00
    movwf    ANSEL                                ; go all digital
    movlw    b'01100010'                            ; bits for OSCCON internal clock at 4MHz 0110 0000
    movwf    OSCCON^0x080               
    movlw    0x00 ^ ( (1<
    movwf    PIE1^0x080
    bank0
    return

;******************************************************************************
;  Ultrasonic Ranging Routines                                                       
;******************************************************************************
;    This code takes a reading and stores the result in EyesLH.  It also sets
;    flag tooclose if there is a bad reading/under threshold

; after cueing the pulse, we have a total wait of 750us

takeRange
    ; clear TOOCLOSE flag
    movlw    0x00            ; init EYESLH to zero
    movwf    EYESLH + 1
    movwf    EYESLH
    bcf        TOOCLOSE
    ; first, send init pulse to EYES
    bank1
    bcf        EYESTRIS     ; make EYES output enabled
    bank0
    bsf        EYES
    goto    $ + 1        ; five ops at 4MHz gives 5us
    goto    $ + 1        ; we use 11us
    goto    $ + 1
    goto    $ + 1
    goto    $ + 1
    nop
    bcf        EYES
    bank1
    bsf        EYESTRIS     ; make EYES input enabled
    bank0
    ; now set everything up and wait for total of 750us
    call    delay750us
   
    clrf    TMR1L            ; reset tmr1
    clrf    TMR1H
    bcf        PIR1,TMR1IF        ; clear the overflow flag for tmr1
    bcf        TOOCLOSE        ; clear tooclose flag
    bsf        T1CON,TMR1ON    ; start the timer1
   
waitForRange
    ; get range and set TOOCLOSE flag, or return EYESLH.
    ; If max range, set EYESLH to max range value.
    btfsc    TOOCLOSE        ; flag set by interrupt if we time out
        goto    gotTooClose
    ; TRIAL CODE - No idea if this works.  Want to set allclear
    ; if TMR1H goes above 8
    btfss    TMR1H,7
        goto    keepChecking
    btfsc    TMR1H,0
        goto    setAllClear

keepChecking
    btfsc    EYES            ; wait for send to be done
        goto waitForRange
    ; if we get here, we have a range
    ; if not TOOCLOSE, set EYESLH
    bcf        T1CON,TMR1ON      ; turn off timer1
    movf    TMR1L,W            ; capture timer1 values in EyesLH
    movwf    EYESLH
    movf    TMR1H,W
    movwf    EYESLH + 1
   
    ; Test for reading less than THRESHOLD
    ; if high greater than THRESHOLDH(3), we're good, return
    ; if eyes greater than threshhold, result is negative, carry is clear, return value
    movf    EYESLH + 1,W
    sublw    THRESHOLDH            ; test Eyes > Thresh by Thresh - Eyes
    btfss    STATUS,C            ; if carry is clear, EYESLH is greater than THRESHOLDH
        return                    ; return the good reading
   
    ; if high less than THRESHOLDH(3), gotTooClose
    ; if eyes less than threshold, set flag and return
    movlw    THRESHOLDH
    subwf    EYESLH + 1,W            ; test Eyes < Thresh by Eyes - Thresh
    btfss    STATUS,C                ; if carry set, EYESLH is less than THRESHOLDH
        goto    gotTooClose

    ; if eyes high equals THRESHOLDH(3), check THRESHOLDL
    ; if eyes low is less than threshold low, set flag and return
    movlw    THRESHOLDL
    subwf    EYESLH,W            ; test EYESL < THRESHL by EYESL - THRESHL
    btfss    STATUS,C            ; if carry is set, EYESL is less than THRESHL
        goto gotTooClose
    return

setAllClear
    ; we hit max range. set EyesLH to max value
    movlw    MAXRANGEH
    movwf    EYESLH + 1
    movlw    MAXRANGEL
    movwf    EYESLH
    return
   
gotTooClose
    ;redo this for straight, left, and right reading
    ; set EYESLH to zero and set TOOCLOSE flag
    bsf        TOOCLOSE
    movlw    0x00
    movwf    EYESLH + 1
    movwf    EYESLH
    return

;******************************************************************************
;  Servo Neck Code                                                             
;******************************************************************************
;     Generic code to move the servo "Neck"

moveServo
    movlw    PULSEDELAY
    movwf    DELAY
    movf    PULSE,W
    movwf    PULSE + 1
    call    pulseOut
    decfsz    DELAY,F
        goto    $ - 4
    return

; pulses the servo.  20ms total cycle, duty cycle determines position.
; value in pulse will determine time pulse is high
; value of one = .5 ms, 2 = 1ms, etc
; servo needs 6 delay values:
; 10 o'clock needs 2.0ms on and 18.0ms off  pulse = 4  2000  and 18000 instructions
; 12 o'clock needs 1.5ms on and 18.5ms off  pulse = 3  1500  and 18500 instructions
;  2 o'clock needs 1.0ms on and 19.0ms off  pulse = 2  1000  and 19000 instructions
pulseOut
    movlw    PULSECOUNT
    movwf    PDELAY + 1
    ; cheezy hack til I redo this: add one to pulse
    incf    PULSE + 1,F
    bsf        SERVO            ; start sending high to servo

pulseLoop
    decf    PULSE + 1,F            ; every 500us, decrement pulse
    btfsc    STATUS,Z
        bcf    SERVO            ; if zero send low to servo
   
    movlw    99
    movwf    PDELAY
halfMs                        ; 500us delay inner loop
    decf    PDELAY,F
    nop
    btfss    STATUS,Z
        goto     $ - 3
    decfsz    PDELAY + 1,F
        goto    pulseLoop
    return

;******************************************************************************
;  Look Code                                                             
;******************************************************************************
;    This code looks around, moving the neck and taking readings

lookAhead
    ; move to 12 o'clock
    movlw    3
    movwf    PULSE
    call    moveServo
    ; take reading
    call    takeRange        ; puts reading in EYESLH
    return

lookLeft
    ; move to 10 o'clock
    movlw    4
    movwf    PULSE
    call    moveServo
    ; take reading and store in LEFTLH
    call    takeRange
    movf    EYESLH + 1,W
    movwf    LEFTLH + 1
    movf    EYESLH,W
    movwf    LEFTLH
    return

lookRight
    ; move to 2 o'clock
    movlw    2
    movwf    PULSE
    call    moveServo
    ; take reading, store in RightLH
    call    takeRange
    movf    EYESLH + 1,W
    movwf    RIGHTLH + 1
    movf    EYESLH,W
    movwf    RIGHTLH
    return

;******************************************************************************
;  Decision Code                                                             
;******************************************************************************
;    This code compares readings to determine next direction

determineNewDirection
    ; figure out which way to turn: left, right, or uturn
    call    allStop                ; stop moving till we know where to go
    movlw    UTURN
    movwf    DIRECTION
    call    lookLeft            ; if too close, dir stays zero
    btfss    TOOCLOSE            ; else dir is cangoleft
        incf    DIRECTION,F       
   
    call    lookRight
    btfsc    TOOCLOSE
        goto    pickATurn
    movlw    CANGORIGHT            ; add 2 to direction
    addwf    DIRECTION,F            ; it will be 2 if right only, 3 if both
pickATurn
    movf    DIRECTION,W
    sublw    LEFTORRIGHT
    btfsc    STATUS,Z            ; dir = leftorright ? goto compareLeftRight
        goto    compareLeftRight
    movf    DIRECTION,W
    sublw    CANGOLEFT
    btfsc    STATUS,Z            ; dir = left ? turn left
        goto    turnLeft
    btfss    STATUS,C            ; dir = 0 ? uturn
        goto    turnRight        ; result was 1-2=-1, negative, carry clear
    goto    uturn                ; result was 1-0=0,  positive, carry set
    return                        ; safety return

compareLeftRight
    movf    LEFTLH + 1,W            ; right - left, test if negative
    subwf    RIGHTLH + 1,W            ; right >= left high? turn right : turn left
    btfsc    STATUS,C                ; if carry set, left is higher
        goto    turnLeft
    goto    turnRight
    return                        ; safety return

;******************************************************************************
;  Move Code                                                             
;******************************************************************************
;     This code controls the motors: straight, left turn, right turn, and turnaround

allStop
    ; stop all motors and figure things out
    ; debug code: clear all motor leds
    movlw    0x00
    movwf    MOTORPORT
    ; debug pause to see leds off
    call    oneSecDelay
    return

moveAhead
    ; all clear ahead.  Move forward
    movlw    0x00 ^ ( ( 1 << MOTORLEFTONE ) + ( 1 << MOTORRIGHTONE ) )
    movwf    MOTORPORT
    ; debug pause to see leds
    call    oneSecDelay
    return

turnLeft
    ; turn left ~45 degrees?
    movlw    0x00 ^ ( ( 1 << MOTORLEFTONE ) + ( 1 << MOTORRIGHTTWO ) )
    movwf    MOTORPORT
    ; debug pause to see leds
    call    oneSecDelay
    return

turnRight
    ; turn right ~45 degrees?
    movlw    0x00 ^ ( ( 1 << MOTORLEFTTWO ) + ( 1 << MOTORRIGHTONE ) )
    movwf    MOTORPORT
    ; debug pause to see leds
    call    oneSecDelay
    return

uturn
    ; artificial: turn on all motor leds
    movlw    0x00 ^ ( ( 1 << MOTORLEFTONE ) + ( 1 << MOTORLEFTTWO ) + ( 1 << MOTORRIGHTONE ) + ( 1 << MOTORRIGHTTWO ))
    movwf    MOTORPORT
    call    oneSecDelay
    ; turn left or right ~180 degrees. direction alternates on each call
    ;btfss    UTURNLEFT
    ;    goto    uRight
    ; uturn left
    ;bcf        UTURNLEFT             ; togggle for different uturn next time
    ;call    turnLeft
    ;call    turnLeft
    return
uRight
    bsf        UTURNLEFT            ; togggle for different uturn next time
    call    turnRight
    call    turnRight
    return

;******************************************************************************
;  Delay Code                                                             
;******************************************************************************

delay750us
    movlw    148            ; 750us less the 5us for set up, above
    movwf    DELAY
    decf    DELAY,F
    nop
    btfss    STATUS,Z
        goto    $ - 3
    nop
    return

oneSecDelay
    movlw    0x3  ; 0x3 is about 1 sec at 4 MHz
    movwf    DELAY + 2
outerLoop                ; (254*5+8) * 255 + 2 = 325892 instructions
    movlw     0xff        ; at 4MHz is about 1/3 of a second, .065 sec at 20MHz
    movwf     DELAY
    movlw    0xff       
    movwf   DELAY + 1
loop
    decf    DELAY, F       
    btfsc    STATUS, Z
       decfsz    DELAY + 1, F
               goto    loop
       decfsz    DELAY + 2, F
               goto    outerLoop
       return

;******************************************************************************
;  Mainline Code                                                             
;******************************************************************************

main
    call    init
    bank1
    ; PORTB is for the motors: all output
    movlw    0x00
    movwf    TRISB ^ 0x080    ; enable all PORTB for output
    ; eyes and neck. All intially output
    movlw    0x00
    movwf    TRISA ^ 0x080    ; enable all PORTA for output
    bank0
    bcf        EYES            ; assert low on EYES so no accidental pulse goes out
    clrf    MOTORPORT        ; turn off motors
explore
    call    lookAhead
    btfsc    TOOCLOSE
        call    determineNewDirection    ; we got too close, turn to new direction
    call    moveAhead        ; move forward
    goto explore

END