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