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
; _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
; _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
; _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
No comments:
Post a Comment