MicroMouse
http://www.robotstorehk.com/micromouse/micromouse.html
▲미로 구매 사이트
마이크로마우스주행알고리즘(세미나)-icefire7.hwp
micromouse.pdf▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼
; Micromouse control and PWM Software
;
; Dirk Becker, 30/04/99, Race - version
; Sonja Lenz
; Michael Gims
;
; Group B2
;
; Port RA1, RA0 as input (Speed up/down)
; Port Rpwm2, RB4 as PWM output
;
;
; RA00 --> Sensor 3 (all Sensor inputs are active High)
; RA01 --> Sensor 4
; RA02 --> Start/test engine button (Active High)
; RA03 --> Sensor 5
; RA04 --> Pulse IR-diode out (via FET) (active Low output)
; RB00 --> Sensor 6
; RB01 --> Sensor 7
; RB02 --> PWM 1 - Brake Out (High= Brake ON)
; RB03 --> PWM 2 - Brake Out (High= Brake ON)
; RB04 --> PWM 1 - Output (High= Pulse ON) (Left )
; RB05 --> PWM 2 - Output (High= Pulse ON) (Right)
; RB06 --> Sensor 1
; RB07 --> Sensor 2
;
;
; Sensor Location (Bit locations)
;
; +------------------------------------+
; I I
; I L 4 I
; I M 0 I
; I 1 1 I
; I 1 I
; I I front
; I L P I ---------->
; I M I I
; I 2 C I
; I I
; I I
; +-----+-+-----------------------+-+--+
; I I I I
; I I I I
; I I S-2
; S-1 S-0
; I I S-3
; +-+ +-+
;
; The PWM and Brake outputs are hardware protected against
; not allowed conditions in order to prevent the
; transistors from damage
;
;
;
LIST P=16f84;f=inhx8m
ERRORLEVEL -302 ; suppress bank selection messages
_CP_OFF equ H'3FFF' ;code protect off
_PWRTE_ON equ H'3FFF' ;Power on timer on
_WDT_OFF equ H'3FFB' ;watch dog timer off
_HS_OSC equ H'3FFE' ;crystal oscillator
__CONFIG _CP_OFF & _PWRTE_ON & _WDT_OFF &_HS_OSC
; *************************************
; * DEFINITIONS *
; *************************************
pcl equ 2 ; Registers
status equ 3
porta equ 5
portb equ 6
intcon equ 0bh
trisa equ 85h
trisb equ 86h
optreg equ 81h
tmr0 equ 01h
c equ 0 ; Bits in status
z equ 2
rp0 equ 5
out1 equ 4 ; PWM Output Bits
out2 equ 5
brk1 equ 2 ; Brake Ouput Bits
brk2 equ 3
btn equ 2 ; Button (Port A) Pressed=1
diode equ 4 ; Pulse Out for IR-Diodes
toif equ 2 ; Bits in intcon
w equ 0 ; Register destinations
f equ 1 ;
stackw equ 0ch ; stack to push pop the W-register ;
Variables
stacks equ 0dh ; stack to push pop the Status-register
pwm1 equ 0eh ; Ram address for PWM1 setting
pwm2 equ 0fh ; Ram address for PWM2 setting
pwmhelp equ 010h ; Ram address for PWM settings saving
n equ 011h ; Ram address for variable n
m equ 012h ; Ram address for variable m
k equ 013h ; --- "" --- k
pulse equ 014f ; Helping counter for pulses
sensors equ 015h ; For IR-sensor
walls equ 016h ; Count number of walls
l equ 017h ; Ram address for variable l
pwmmax equ .230 ; PWM maximum speed
wait equ .2 ; Wait delay time
maxspd equ .7 ; Max speed the mouse can drive
wallcnt equ .20 ; Walls to drive around
goto init
; **********************************************************
; * ISR Routine *
; * needs 0..F in pwm1 and pwm2 for generating pwm pulses *
; * outputs directly to the motors *
; **********************************************************
org 04h
pwmisr
movwf stackw ; copy W-register to save stack
swapf status, w ; Swap Status-reg to be saved in W
movwf stacks ; Save Status to stacks
movf pwm1, w ; Compare pwmhelp-counter
subwf pwmhelp, w ; with PWM1 setting
btfsc status, c ; and set PWM1 Output to
bcf portb, out1 ; on or
btfss status, c
bsf portb, out1 ; off
movf pwm2, w ; Compare pwmhelp-counter
subwf pwmhelp, w ; with PWM1 setting
btfsc status, c ; and set PWM1 Output to
bcf portb, out2 ; on or
btfss status, c
bsf portb, out2 ; off
incf pwmhelp, f ; Increase PWM reference counter
bcf pwmhelp, 4 ; But not >0Fh
movlw pwmmax ; sets w register
bcf status, rp0 ; select bank 0
movwf tmr0 ; Set TMR0 to desired PWM resolution value
bcf intcon, toif ; Clear interrupt flag
swapf stacks, w ; Swap nibbles in stacks reg
; and place into W
movwf status ; restore Status-register
swapf stackw, f ; Swap nibbles in stackw and place into stackw
swapf stackw, w ; swap nibbles in stackw and restore to W-register
retfie
; ISR END
; *****************************************************
; * The initalisation is all done here *
; *****************************************************
init
bcf status, rp0 ; select bank 0
clrf porta ; set porta 0
bsf status, rp0 ; select bank 1
movlw 0fh ; set port A0 & A1
movwf trisa ; as input
bcf status, rp0 ; select bank 0
clrf portb ; set porta 0
bsf status, rp0 ; select bank 1
movlw 0c3h ; set port pwm1 & pwm2 & BRK1 & BRK2
movwf trisb ; as output
bsf status, rp0 ; select bank 1
movlw 40h ; select TMR0
movwf optreg ; as counter with no divider
bcf status, rp0 ; select bank 0
movlw 0e0h ; Enable TMR0
movwf intcon ; interrupts
clrf pwmhelp ; Clear PWMHELP address,
clrf pwm1 ; PWM1
clrf pwm2 ; and PWM2
clrf n ; clear variables
clrf m
clrf k
clrf sensors
bsf porta, diode ; Turn off IR-diodes
start
movlw 0 ; sets w register
bcf status, rp0 ; select bank 0
movwf tmr0 ; Set TMR0 to desired PWM resolution value
; **********************************************************
; * M A I N P R O G R A M M E *
; **********************************************************
main
bsf porta, diode ; Turn Off diodes (It's safer)
movlw wallcnt ; Set wallcounter
movwf walls
call btnpress
bcf portb, brk1
bcf portb, brk2
movlw 00h
movwf pwm1
movwf pwm2
; ****************************** The Mouse routines start here
***************
doagain
call straight ; Go ahead
call turn ; Drive the curve
call stop
call delay
decfsz walls, f
goto doagain
goto main ; End main function ------------------------------
; **********************************************************
; * Procedure Btnpress *
; * waits until Starting Button is pressed once and *
; * released *
; **********************************************************
;
btnpress
btfss porta, btn ; is start
goto btnpress ; button pressed?
nop
movlw 50 ; ca 50us
movwf k ; loop for
loop2
incfsz k, f ; preventing
goto loop2 ; button from jittering
loop1
btfsc porta, btn ; is the button
goto loop1 ; released again?
nop
return
; **********************************************************
; * Procedure Checksensors *
; * checks the IR-Diodes and writes the result into the *
; * so called sensor variable sensor - sensor1 is bit 0 *
; * it also provides all the timing needed for the sensors *
; **********************************************************
;
checksensors
clrf sensors ;
bcf porta, diode ; Turns the diodes ON
movlw .20 ; Set counter register
movwf k ; to delay
loop4
decfsz k, f ; for sensor
goto loop4 ; checking
bsf porta, diode ; Turn the Sensors off again
btfsc portb, 7 ; Check sensor 1
bsf sensors, 0 ;
btfsc portb, 6 ; Check sensor 2
bsf sensors, 1 ;
btfsc porta, 1 ; Check sensor 3
bsf sensors, 2 ;
btfsc porta, 0 ; Check sensor 4
bsf sensors, 3 ;
movf sensors, w
movlw 0ffh ; Delay to turn the sensors
movwf k ; long enough off
loop5
decfsz k, f ;
goto loop5 ;
return
; **********************************************************
; * Subroutine Stop *
; * Stops the engine WITH setting brakes *
; **********************************************************
;
stop
bsf portb, brk1 ;Stop both motors and brake them
bsf portb, brk2
clrw
movwf pwm1
movwf pwm2
return
; **********************************************************
; * Subroutine Delay *
; * provides a delay *
; **********************************************************
;
delay
movlw .255
movwf n
stopagain1
movlw .146
movwf k
stopagain2
movlw 1
movwf m
stopagain3
incfsz m, f
goto stopagain3
incfsz k, f
goto stopagain2
incfsz n, f
goto stopagain1
return
; **********************************************************
; * Subroutine straight *
; * Drives straight along the wall *
; **********************************************************
;
straight
movlw .10
movwf n
straighton
bcf portb, brk1 ; release the brakes
bcf portb, brk2
call checksensors
clrw
btfsc sensors, 0
call straightrun
clrw
btfsc sensors, 3 ; Has right sensor wall contact?
call slow2 ; --> Yes: Jump slow 2
clrw
btfsc sensors, 2 ; Has left sensor wall contact?
call slow1 ; ; --> Yes: Jump slow 2
bcf sensors, 1 ; Sensor 1 should not be in use
movf sensors, w
btfss status, z
goto straighton
decfsz n, f
goto straighton
call stop
call delay
btfsc sensors, 0 ; Was it a gap?
goto straight ; --> Do it again
btfsc sensors, 2 ; ---- "" -----
goto straight
btfsc sensors, 3 ; ---- "" -----
goto straight
call stop
call delay
return
; **********************************************************
; * Subroutine slow1 *
; * Drives PWM1, with checking sensor 2--> result in w *
; **********************************************************
;
slow1
movlw maxspd
movwf pwm2
movlw maxspd-2 ; If only outer sensor has wall contact steer left
btfsc sensors, 0
movlw maxspd-1 ; If mid and outer sensor have contact steer
slightly
movwf pwm1
return
; **********************************************************
; * Subroutine slow2 *
; * Drives PWM2, with checking sensor 3 --> result in w *
; **********************************************************
;
slow2
movlw maxspd
movwf pwm1
movlw maxspd-2 ; If only outer sensor has wall contact steer right
btfsc sensors, 0 ;
movlw maxspd-1 ; If mid and outer sensor have contact steer
slightly
movwf pwm2
return
; **********************************************************
; * Subroutine Turn *
; * This subrutine drives right around the corner *
; **********************************************************
;
turn
call checksensors ; Check 'em again
bsf portb, brk2 ; Stop the
clrw ; right wheel
movwf pwm2 ; (BRAKE IT!)
bcf portb, brk1 ; Turn with Left wheel
movlw .4
movwf pwm1
btfss sensors, 3
goto turn
return
END ; it's enough now