BACK
Program Source Code
The following is a complete listing of the most recent version of the
program code used to control the robot. The program comments in the
header describe the robots behavior with this code.
;------------------------------------------------------------------
;
; FIRE FIGHTING ROBOT PROGRAM
; by Jason Plumb, Brent Short, and John Walter
; EE3333 - Project Lab 3
; Spring 1998
;
; 3/24 Goes to every room and looks for the candle. If the candle
; is present, it will go toward it and look for the circle. When it
; reaches the circle it will turn the fan on and blow out the candle.
; It will not exit back out of the house once it blows out the candle...
; it will just sit there.
;
; 3/28 - This code has been reduced in size
; * FWD, BK, and STOP have been replaced with hard coded values
; * fwd_motors and bk_motors routines have been implemented
;
; 3/31 - This is the reduced code with modulation added in.
; Specifically, it uses PWM for the LED switch and the logic
; has been reversed
;
; 4/3 - Candle threshold is set for using flashlight reflector
; 4/4 - This has step count added back in and has been modified
; so that the robot will detect the candle circle if it is
; < 12" from the door
; * This code has been modified so that the robot aligns properly
; when entering the rooms. This is done in the go_in routine
; Note: This code needs serious reduction - it fills almost
; all available byte erasable eeprom
; 4/9 - won't hit wall if candle is close -makes sure it has
; gone a certain distance
;------------------------------------------------------------------
; Begin program equates here
;------------------------------------------------------------------
RAMSTART: equ $0800 ; Start of RAM for the HC12
PROGSTART equ $0D00 ; Start of program in jump EEPROM
PORTA: equ $0000 ; Address of port a on hc12
DDRA: equ $0002 ; Data direction register for port A
PORTB: equ $0001 ; Address of port b on hc12
DDRB: equ $0003 ; Data direction register for port B
PORTE: equ $0008 ; Address of port e on hc12
DDRE: equ $0009 ; Data direction register for port E
ATDSTAT: equ $0066
ADR0H: equ $70 ;A/D Converter Result Register 0
ADR1H: equ $72 ;A/D Converter Result Register 1
ADR2H: equ $74 ;A/D Converter Result Register 2
ADR3H: equ $76 ;A/D Converter Result Register 3
ADR4H: equ $78 ;A/D Converter Result Register 4
ADR5H: equ $7A ;A/D Converter Result Register 5
ADR6H: equ $7C ;A/D Converter Result Register 6
ADR7H: equ $7E ;A/D Converter Result Register 7
RAMSTRT: EQU $0800 ; start of internal ram
RAMSIZE: EQU $0400 ; Size of internal ram
STACK: EQU RAMSTRT+RAMSIZE ; top of stack
ATDCTL2: equ $0062
ATDCTL3: equ $0063
ATDCTL4: equ $0064
ATDCTL5: equ $0065
ATDSTATH: equ $66 ;ATD Status Register High
ATDSTATL: equ $67 ;ATD Status Register Low
COPCTL: equ $16 ;COP Control Register
COPRST: equ $17 ;Arm/Reset COP Timer register
PWEN: equ $0042 ; PWM enable register
PWPER0: equ $004C ; PWM period channel for channel 0
PWCLK: equ $0040 ; PWM clocks and concatenate (used to concat ch0 and
ch1)
PWDTY0: equ $0050 ; PWM duty cycle for channel 1
;----------------------------------------------------------------------
; Begin data segment
;----------------------------------------------------------------------
org RAMSTART
temp db $00
tempfw dw $00
tempw dw $00
NUM_STEP90 db $87
speed dw $05FF
led_speed dw $0000
sensor_diff db $0
rtmtr db $58,$3D
ltmtr db $58,$3D
;FWD db $58
;BK db $54
;STOP db $a7
; RAM variables used for holding sensor readings begin here
s0loff db $FF
s1loff db $FF
s2loff db $FF
s3loff db $FF
s4loff db $FF
s5loff db $FF
s6loff db $FF
s7loff db $FF
s0lon db $FF
s1lon db $FF
s2lon db $FF
s3lon db $FF
s4lon db $FF
s5lon db $FF
s6lon db $FF
s7lon db $FF
s1_close_thresh dw $0050
s2_close_thresh db $FF
s3_close_thresh db $FF
s5_close_thresh db $FF
s6_close_thresh db $FF
s2_far_thresh db $FF
s3_far_thresh db $FF
s5_far_thresh db $FF
s6_far_thresh db $FF
s2_center_thresh db $FF
s3_center_thresh db $FF
s5_center_thresh db $FF
s6_center_thresh db $FF
wall_status db $00
pwall_status db $00
abs_thresh db $00 ; The absolute threshold for any given sensor
exist_thresh db $18
sens_addr dw $0072 ; Parameter to follow_wall function
sens_far_thresh db $00
sens_close_thresh db $00
follow_wall db $00 ;0=right, not zero = left
low_step dw $00 ;step count for lowest candle value
low_value db $00 ;lowest candle value - init to $FF
old_y dw $00
old_x dw $00 ; Artificial stack vars
abovect db $00
extinguished db $00
stepct db $00
old_rt db $00
in_circle db $00
;----------------------------------------------------------------------
; Begin main program
;----------------------------------------------------------------------
org PROGSTART
;----------------------------------------------------------------------
; Initialization stuff here:
;----------------------------------------------------------------------
INIT_STUFF:
ldaa #$0
staa COPCTL ; Disable the COP watchdog timer
staa ATDCTL3
staa extinguished
staa stepct
staa in_circle
ldaa #$FF
staa DDRB ; Set direction for port B (all output)
staa PORTB ; Write initial PORT B value (unlock mot)
movb #%11101101, DDRA ; Set direction for A (outputs)
;ldaa ;PORTA
ldaa #%00001101 ; Ensure that calibration LED and IR LEDs on and Fan
is off
staa PORTA ; Put byte back on port A
movb #$80, ATDCTL2 ; Set up ATD to function normally
;movb #$00, ATDCTL3 ; Select continue conversion in BGND Mode
; Ignore FREEZE in ATDCTL3
movb #$01, ATDCTL4 ; Select Final Sample time = 2 A/D clocks
; Prescaler = Div by 4 (PRS4:0 = 1)
;The next x lines set up RAM variables during init...
;that way, they aren't lost when we power down.
;(not used, hard coded)movb #$81, NUM_STEP90
;(speed is hard coded in the delay routine)
;movw #$1700, speed ; #$1D10, speed ; Speed variable
ldd #$003D
std rtmtr
std ltmtr
;movw #$003D, rtmtr
;movw #$003D, ltmtr
;movb #$40, s1_close_thresh ; Just hard coded for now
;movw #$01F6, steps_360 ; (was 1f8) (was 200)
;movb #$55, candle_thresh
; The next 4 lines set up and start PWM for modulating IR LEDs
movb #$40, PWCLK ; Concat channels 0 and 1
movb #$03, PWEN ; Enable PWM
movb #$1F, PWPER0 ; Set channel 0 period
movb #$10, PWDTY0 ; Set channel 0 duty cycle
;movb #$00, stepct
;jsr INIT_STUFF ; Do initialization stuff
;----------------------------------------------------------------------
;end init
;----------------------------------------------------------------------
jsr WaitForTone ; Actually, wait for button
jsr Read
ldaa ADR6H ; Get right wall calibration value
ldab ADR2H
addd #$0A0A ; Add for close threshold (due to modulatioN)
staa s6_close_thresh ; Calculate and store thresholds
stab s2_close_thresh
subd #$1414
staa s6_far_thresh
stab s2_far_thresh
jsr wait_toggle ; Should turn it back on
jsr WaitForTone ; tone or button
movb #%00110011, PORTB ; Write initial value to port B
basic: ;
ldy #$00FF
jsr fwdabit
movb #$FF, follow_wall ;left wall
jsr Follow
jsr RoomL ; First room (island)
jsr right_90
movb #$00, follow_wall ;right wall
jsr Follow
jsr RoomR ; Second room
jsr right_90
jsr Follow
jsr RoomR ; Third room
jsr right_90
jsr Follow
jsr RoomR ; Fourth (last) room
jsr left_90
ldaa #$FF
staa count_steps
ldaa #$FF
staa follow_wall ; Follow left wall out of house
staa done
jsr Follow
jsr Room
jsr right_90
ldy #$00DA
jsr fwdabit
jsr Follow
quit: movb #%11111111, PORTB
stop: bra stop
;----------------------------------------------------------------------
; This will wait for the tone to be present for a given period of time
; before continuing. If the button is pressed, it will also exit
;----------------------------------------------------------------------
WaitForTone:
;stx old_x
ldx #$3000
Tone2: ldaa PORTA
bita #%00000010 ; Check for button
bne ToneExit
bita #%00010000
beq TonePres ; The tone is present
bra WaitForTone
TonePres: dex
beq ToneExit
bra Tone2
ToneExit:; ldx old_x ; Artificial stack
;----------------------------------------------------------------------
; This will toggle the status of the calibration LED
;----------------------------------------------------------------------
ToggleCLED:
ldaa PORTA
eora #%00000100
staa PORTA
;ldy old_y ; Artificial stack
rts
;----------------------------------------------------------------------
;wait $100 delay cycles - used for calib. LED - then go on to toggle
;----------------------------------------------------------------------
wait_toggle:
;sty old_y ; Artificial stack
ldy #$0100
wtlp1: jsr DELAY ; Wait before turning LED back on
dey
bne wtlp1
bra ToggleCLED
;----------------------------------------------------------------------
Read: movb #%01010100 , ATDCTL5 ; Initializes ATD, MULT=1 (do conversions
on AN0-AN7)
; Run conversions on subsequent channels
WTCONV: BRCLR ATDSTATH,#$80,WTCONV ; Wait for Sequence Complete Flag
rts
;-----------------------------------------------------
; This will follow the right or left wall until the floor
; sensor is detected
;-----------------------------------------------------
Follow:
ldaa stepct ; Used to go fwd every 3rd step
cmpa #$03 ; If A is not equal to 3
bne Follow_1 ; Then jump to follow1
movb #$00, stepct ; Otherwise, reset stepct
bra Follow2 ; And jumpt to follow2 (go 1 step fwd)
Follow_1:
inca ; Increment A
staa stepct
jsr Read
ldaa ADR0H ;floor sensor
cmpa #$80
bhi no_line
jmp left_motor
no_line:
ldaa ADR3H ; Get value of front sensor (1 = 3 temp)
cmpa #$50 ; was 70 ; Compare to close threshold
blo Follow1a ; If we didnt' "hit" a front wall, branch
jsr right_90
bra Follow
Follow1a:
ldaa follow_wall ; 0 for right wall, 1 for left
beq wall_right ; If zero, follow right wall
ldab ADR2H ; Otherwise, get left sensor value
cmpb s2_far_thresh ; Compare to the far threshold
bhi not_too_far ; If we're higher than far thresh, we're ok so far
bra foll_1bC
not_too_far: ; Not too far from left wall
cmpb s2_close_thresh ; Compare to the left close threshold
blo Follow2 ; If lower than close thresh, just go fwd
bra wall_rtC
wall_right: ; If here, we're following right wall
ldab ADR6H ; Get front right sensor value
cmpb s6_far_thresh ; Compare it with far thresh
bhi Follow1b ; If higher, we're ok
wall_rtC: jsr left_motor ; Otherwise, turn in one step
bra Follow
Follow1b:
cmpb s6_close_thresh ; Compare sensor value to close threshold
blo Follow2 ; If higher, we're aligned and can go fwd
foll_1bC:
jsr right_motor ; Otherwise, we need to turn away
bra Follow
Follow2:
jsr fwd_motors ; Set up motor directions for both fwd
jsr GO ; Go forward 1 step
bra Follow ; Keep following
;----------------------------------------------------------------------
; This will set the directions of the motors to pivot left
;----------------------------------------------------------------------
piv_left_motors:
movb #$58, rtmtr
movb #$54, ltmtr
rts
;----------------------------------------------------------------------
; This will set the directions of the motors to pivot right
;----------------------------------------------------------------------
piv_right_motors:
movb #$54, rtmtr
movb #$58, ltmtr
rts
;----------------------------------------------------------------------
; This will set the directions of both motors to fwd
;----------------------------------------------------------------------
fwd_motors:
ldaa #$58
staa rtmtr
staa ltmtr
rts
;----------------------------------------------------------------------
; This will set the directions of both motors to bk
;----------------------------------------------------------------------
bk_motors:
ldaa #$54
staa rtmtr
staa ltmtr
rts
;----------------------------------------------------------------------
; This will move just the right motor forward. Used in turning
;----------------------------------------------------------------------
right_motor:
movb #$58, rtmtr
movb #$a7, ltmtr
bra lmgo
;----------------------------------------------------------------------
; This will move just the left motor forward. Used in turning
;----------------------------------------------------------------------
left_motor:
movb #$58, ltmtr
movb #$a7, rtmtr
lmgo: jmp GO
;rts
;----------------------------------------------------------------------
; This will turn the car 90 degrees left or right
;----------------------------------------------------------------------
left_90: jsr piv_left_motors
;movb #$58, rtmtr
;movb #$54, ltmtr
bra all_set
right_90: jsr piv_right_motors
;movb #$54, rtmtr
;movb #$58, ltmtr
all_set: ldy #$0081 ; Load Y with # of steps in 90 deg (NUM_STEP90)
loop_90: jsr GO
dey
bne loop_90
rts
;----------------------------------------------------------------------
; Goes forward or backward a bit depending on the value passed in Y
;----------------------------------------------------------------------
;bakabit: jsr bk_motors
; bra bakabl
fwdabit: jsr fwd_motors
bakabl: jsr GO
dey
bne bakabl
rts
;----------------------------------------------------------------------
;Room Routine - This is what the robot does when it enters the room
;----------------------------------------------------------------------
RoomL: jsr right_motor
bra into
RoomR: jsr left_motor
bra into
Room: jsr fwd_motors
into: movb #$00, PWEN ; Disable PWM (turn off IR LEDs)
rm1: ;jsr go_in ; Go into the room
;------------------------------------------------------------------
; go_in will move the robot into the room from the doorway
; It will exit out when it goes 12 inches or detects the candle
; circle on the floor
;------------------------------------------------------------------
go_in:
ldaa rtmtr
staa old_rt ; Preserve the original rt mot direction
go_o_line:
ldaa old_rt
cmpa #$58
bne go_o_lineL
jsr right_motor
bra go_o_line1
go_o_lineL:
jsr left_motor
go_o_line1:
jsr GO
jsr fwd_motors
jsr GO
jsr Read ; Read sensors
ldaa ADR0H ; Get floor sensor
cmpa #$80 ; Compare with the floor threshold
blo go_o_line ; Keep going fwd until line disappears
ldaa extinguished ; Not used (yet)
beq look
jmp leave
look: ldy #$00DA ; Should be 12 inches
; jsr fwd_motors ; Set motor direction to fwd
go_in_al: jsr GO ; Go fwd 1 step
jsr Read ; Read sensors
ldaa ADR0H ; Get floor sensor
cmpa #$80 ; Compare with line
blo go_bail ; If line found, we need to bail
dey
bne go_in_al
bra turn
go_bail:
ldaa #$FF
staa in_circle
jsr GO
dey
cpy #$0050
bhi go_bail
;ldy #$006C
;jsr fwdabit
;rts
turn: movb #$FF, low_value ; Init strongest candle reading (lowest
#)
jsr piv_left_motors ; Set up motor directions for a left pivot
ldy #$01F6 ; Y gets number of steps in a 360 deg turn
T360: jsr Read ; Read sensor values
ldaa ADR7H ; Get candle sensor value
cmpa low_value ; compare with the low_value
bhi not_stronger ; If higher, skip
staa low_value ; Otherwise, store as new low value
sty low_step ; Store Y into the low_step count var
not_stronger:
jsr GO ; Do one step CCW
dey ; Decrement step counter
bne T360 ; If we haven't done a 360, keep going
; If here, we've done a whole 360
ldaa low_value ; Otherwise, get the lowest value
cmpa #$55 ; Compare to candle threshold
bhi leave ; If higher, then candle not in room
jsr piv_right_motors ; Set up motor directions for a right pivot
ldy low_step
ldab #$0C
aby ; Add $0C to Y to "fix" turn
turn_back:
jsr GO
dey
bne turn_back
jsr fwd_motors
ldx #$0000
FoundIt:
ldaa in_circle
bne fan_on
inx
jsr GO ; Step forward one step
jsr Read ; Read sensors
ldaa ADR0H ; Check floor sensor
cmpa #$80 ; Compare with threshold
bhi FoundIt ; If higher, need to keep going fwd
fan_on: ldaa PORTA ; If we're here, we hit candle circle
anda #%11110111 ; Make fan turn on (reverse logic)
staa PORTA
jsr wait_toggle ; Keep fan on for ~ 1/2 sec
ldaa PORTA
oraa #%00001000
staa PORTA ; Turn fan back off
staa extinguished ; a != 0
jsr bk_motors
inx
back: jsr GO
dex
bne back
jsr piv_left_motors
ldy low_step
straight:
jsr GO
dey
bne straight
;jmp quit
leave:
movb #$03, PWEN ; Turn IR LEDs on before backing to wall
;jmp Back_to_wall
;rts
;----------------------------------------------------------------------
;backs until it sees a wall
;----------------------------------------------------------------------
Back_to_wall:
jsr bk_motors ; Set up motor directions for backward
no_wall: jsr GO
jsr Read
ldaa ADR4H
cmpa #$70
blo no_wall
rts
;----------------------------------------------------------------------
; This routine moves both motors one step
; The current state of the motor does not matter
;----------------------------------------------------------------------
GO: clc ; clear carry bit
ldab PORTB ; read the current motor byte
andb #%11110000 ; Mask off right stepper bits
jsr rtmtr ; carry out shift
bcc NEXT1 ; If we didn't carry, skip to NEXT1
orab #%00010000 ; If we carried, set LSB of this nibble
NEXT1: bitb #%00001000 ; check if we overflowed right
beq NEXT3 ; if not, branch
eorb #%10001000 ; if so, fix it
NEXT3: stab temp ; store right motor nibble in temp
clc ; clear carry
ldab PORTB ; read the current motor byte
andb #%00001111 ; Mask off left stepper bits
jsr ltmtr ; carry out shift
bcc NEXT4 ; if didn't carry, skip to NEXT4
orab #%00001000 ; if carried, fix it
NEXT4: bitb #%00010000 ; check if overflowed left
beq NEXT2 ; if not, branch
eorb #%00010001 ; if so, fix it
NEXT2: orab temp ; combine the two motor nibbles
stab PORTB ; send out to motor driver
jsr DELAY
rts
;----------------------------------------------------------------------
; This delay routine is used for delaying between steps
;----------------------------------------------------------------------
DELAY: xgdx ;(exchange X and D to save X) stx old_x
ldx #$1200 ; Load X with speed var (used for delaying)
dloop: dex ; Decrement X
cpx #$0000 ; If X != 0
bne dloop ; Loop again
xgdx ;(exchange X and D to save X) ldx old_x ; Fake stack
eof: rts
BACK
|
