There are a set of basic services which must be set up before any serious code runs on the AVR. These
include setting up the stack, calling reset routines, and running the main loop. It also involves defining the
relationship between the logical functions of the ports and registers and their physical equivalents.
ROBOTDEF.INC
This file defines a series of macros which make the physical I/O locations and other values more meaningful.
;-----------------------------------------------------------------
;
; Name: RobotDef.inc
; Title: Defines for robot I/O, etc.
; Version: 1.0
; Last updated: 2001.03.10
; Target: AT90S2313
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
; Defines for robot I/O
;
; Bit numbers begin with b
; Bitmasks begin with m
; Registers begin with r
; Other values are all uppercase
;
;-----------------------------------------------------------------
.include "2313def.inc"
;-----------------------------------------------------------------
;
; Robot specific port designations.
;
;-----------------------------------------------------------------
;
; Port D
;
|
These are the bit designations for port D, which is connected to the front and back bump switches, and the left, right and back eye drives.
These bit numbers are used in turn to define mask values which have a '1' bit in the appropriate position, and some
compound masks for 'all bump switches' and 'all eye drives'. The data direction register on Port D will
be outputs only for the eye drives, the others are inputs or are used for the UART.
|
; UART on pins D0, D1
; Bit #s
.equ bBUMPF =PIND2
.equ bBUMPB =PIND3
.equ bEYEDRIVEL =PIND4
.equ bEYEDRIVER =PIND5
.equ bEYEDRIVEB =PIND6
; Mask values
.equ mBUMPF =(1<<bBUMPF)
.equ mBUMPB =(1<<bBUMPB)
.equ mEYEDRIVEL =(1<<bEYEDRIVEL)
.equ mEYEDRIVER =(1<<bEYEDRIVER)
.equ mEYEDRIVEB =(1<<bEYEDRIVEB)
.equ mBUMPS =(mBUMPF|mBUMPB)
.equ mEYEDRIVES =(mEYEDRIVEL|mEYEDRIVER|mEYEDRIVEB)
.equ mDDRD =mEYEDRIVES
;-----------------------------------------------------------------
;
; Port B
;
|
Again, these are bit designations, this time for port B, which is connected to the analog to digital converter RC network, the IRED drive,
and the motor drives. Also again, these are used to construct masks for convenience's sake. Two DDR values are
given, one for reading from the RC network, and one for discharging it.
|
; Analog comparator on B0, B1
; Bit #s
.equ bRCREAD =PINB0
.equ bANALOGIN =PINB1
.equ bRCDRIVE =PINB2
.equ bIREDDRIVE =PINB3
.equ bMOTORLB =PINB4
.equ bMOTORLA =PINB5
.equ bMOTORRA =PINB6
.equ bMOTORRB =PINB7
; Mask values
.equ mRCREAD =(1<<bRCREAD)
.equ mANALOGIN =(1<<bANALOGIN)
.equ mRCDRIVE =(1<<bRCDRIVE)
.equ mIREDDRIVE =(1<<bIREDDRIVE)
.equ mMOTORLA =(1<<bMOTORLA)
.equ mMOTORLB =(1<<bMOTORLB)
.equ mMOTORRB =(1<<bMOTORRB)
.equ mMOTORRA =(1<<bMOTORRA)
.equ mMOTORS =(mMOTORLA|mMOTORLB|mMOTORRA|mMOTORRB)
.equ mDDRBREAD =(mRCDRIVE|mIREDDRIVE|mMOTORS)
.equ mDDRDISCHARGE =(mDDRBREAD|mRCREAD)
;-----------------------------------------------------------------
;
; General purpose registers
|
The registers are assigned particular roles. rRETURN is a general purpose return value from a subroutine call.
rSCRATCHn and rISCRATCHn are just general scratch registers, for use in the main loop and the interrupt routines
respectively. I have saved the bother of pushing the status register during an interrupt routine by dedicating
a register to that task, rISREGSAVE. rTEMPn and rITEMPn are high registers which are used for temporary values,
differing from the scratch registers in that they can operate with immediate values. rPARAMn are registers used
mainly for passing parameters to subroutines, and rLOOPI and rLOOPJ are two loop counters. The X, Y and Z
registers retain their usual designations.
|
.def rRETURN =r0 ; return value from functions
.def rSCRATCH =r7
.def rSCRATCH2 =r8
.def rSCRATCH3 =r9
.def rSCRATCH4 =r10
.def rISCRATCH =r11 ; for use in interrupt routines
.def rISCRATCH2 =r12 ; ditto
.def rISCRATCH3 =r13 ; ditto
.def rISCRATCH4 =r14 ; ditto
.def rISREGSAVE =r15 ; sreg saved here in interrupts
.def rTEMP =r16
.def rTEMP2 =r17
.def rITEMP =r18 ; for use in interrupt routines
.def rITEMP2 =r19 ; again, for use in interrupt routines
.def rPARAM1 =r20 ; parameter 1 for function calls
.def rPARAM2 =r21 ; parameter 2 for function calls
.def rLOOPI =r22 ; loop counters
.def rLOOPJ =r23 ; by tradition, I is outer loop, J inner
;-----------------------------------------------------------------
;
; Macros
|
I noticed that the LDS and STS instructions (to store to/from locations in RAM) take up 2 words of space, but that an
indexed load via a register will only take 1. Since code space is going to be tight in the 2313, as it only has 1024 words
of flash memory, I have dedicated the Y register to always point to the start of the RAM, so I can access the first 64 bytes
of RAM using these LDY and STY macros.
|
; LDY r,addr
.macro LDY
ldd @0,Y+(@1-RAMSTART)
.endmacro
; STY addr,r
.macro STY
std Y+(@0-RAMSTART),@1
.endmacro
.listmac
|
These simple debugging macros helped me debug the
monitor routines but are now unused.
|
; DEBUG displayed_value
.macro DEBUG
push rTEMP
ldi rTEMP,($FF-@0)
out PORTB,rTEMP
pop rTEMP
.endmacro
; DEBUG displayed_register
.macro DEBUGR
push rTEMP
mov rTEMP,@0
com rTEMP
out PORTB,rTEMP
pop rTEMP
.endmacro
;-----------------------------------------------------------------
|
Finally, the clock rate for the processor, which is used later to work out the UART baud rate register value.
|
.equ ROBOT_CLOCKRATE =4000000
;-----------------------------------------------------------------
ROBOT.ASM
This file performs initial setup and runs the main loop.
;-----------------------------------------------------------------
;
; Name: Robot.asm
; Title: Basic setup and services for robot.
; Version: 1.0
; Last updated: 2001.03.11
; Target: AT90S2313
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
; Simple robot systems integration testbed.
;
;-----------------------------------------------------------------
.nolist
.include "robotdef.inc"
.list
;-----------------------------------------------------------------
;
; Data
|
First, we define any data required for this module. This is just a tick count which is incremented by the main
timer 0 interrupt.
|
.dseg
robot_tickcount:
.byte 2 ; tick count
;-----------------------------------------------------------------
;
; Interrupt service vectors
|
Interrupts are routed to the correct places. Unused interrupts just have a reti instruction.
|
.cseg
.org 0
rjmp robot_reset ; Reset vector
reti ; INT0 vector
reti ; INT1 vector
rjmp eye_capture ; T/C1 capture vector
reti ; T/C1 compare match vector
rjmp eye_overflow ; T/C1 overflow vector
rjmp robot_tick ; T/C0 overflow vector
rjmp uart_receive ; UART receive service
reti ; UART data reg empty service
reti ; UART transmit service
reti ; Analog comparator service
;-----------------------------------------------------------------
;
; Start of actual code.
;
;-----------------------------------------------------------------
;
; Reset vector - generic system.
;
|
The reset vector goes to some initial setup code. This prepares the way for executing the main loop.
|
; Before calling reset functions for each subsystem, it sets up
; the following:
;
; 1. RAM set to all 0's
; 2. Y -> RAMSTART
; 3. Sleep mode enabled, t0, t1 on rising edge
; 4. 2kHz interrupt on timer 0
; 5. XH, YH, ZH all 0
|
First, we must always set the stack pointer.
|
robot_reset:
ldi rTEMP,RAMEND ; Set initial stack ptr location at ram end
out SPL,rTEMP
|
Since the RAM and EEPROM are less than 256 bytes in size, we can set the X, Y and Z high registers to 0 and forget about them.
|
clr XH
clr YH
clr ZH
|
Now we clear the RAM, since this is cheaper than explicitly setting most of the data values to 0. This loop also leaves
Y pointing at the start of RAM, which allows us to use the LDY and STY macros defined in ROBOTDEF.INC.
|
ldi YL,RAMEND+1 ; Just past end since we are predecrementing.
robot_ramclear:
st -Y,YH ; clear
cpi YL,RAMSTART ; done?
brne robot_ramclear ; no.. go round again. Y-> RAMSTART at end
|
Timer 0 is set up as an approximately 2kHz timer, and enabled. The sleep mode is enabled also.
|
; set up timer0
ldi rTEMP,(1<<TOIE0) ; timer interrupt mask 0 enable
out TIMSK,rTEMP
ldi rTEMP,$02 ; clock prescaler for T0 = clk/8 (02)
out TCCR0,rTEMP
; set sleep mode
ldi rTEMP,$2A ; enable sleep mode, interrupt t0 and t1 on falling edge only
out MCUCR,rTEMP
|
Each module can publish a reset routine - these are all called at this point to initialize each one.
|
; now call all the reset functions
rcall reset_uart
rcall reset_eyes
rcall reset_state
rcall reset_monitor
|
In addition to the reset routines, we now do some additional setup. This should by rights be in the EYES.ASM module,
now that I come to review the code. This calibrates the eyes.
|
; and, go!
sei ; enable interrupts and off we go!
ldi rLOOPI,16
rcall calibrate_eyes_counted ; skip first few calibration cycles
ldi zl,low(robot_calibrate*2)
ldi zh,high(robot_calibrate*2)
rcall uart_writestringlf
rcall calibrate_eyes ; and then actually calibrate the eyes after we've settled
ldi zl,low(robot_calibrate_done*2)
ldi zh,high(robot_calibrate_done*2)
rcall uart_writestringlf
|
Now we arrive at the main loop. This sleeps until something happens, then calls routines to update each of
the modules, and goes round again forever.
|
robot_mainloop:
sleep ; sleep till next interrupt
rcall update_monitor ; update monitor
rcall update_state ; update state machine
rjmp robot_mainloop ; Infinite loop - never terminates
;-----------------------------------------------------------------
;
; Timer 0 vector - free running 2kHz counter.
|
Timer 0's interrupt routine just increments the tick counter and calls 'tick' routines in the modules that
need an interrupt-driven update.
|
robot_tick:
in rISREGSAVE,SREG
ldy rISCRATCH,robot_tickcount
inc rISCRATCH
sty robot_tickcount,rISCRATCH
rcall state_tick
rcall update_eyes
out SREG,rISREGSAVE
reti
;-----------------------------------------------------------------
;
; Messages
|
Here we just define some debug messages to be sent to the UART.
|
robot_calibrate:
.db "Calibrating eyes...",$00
robot_calibrate_done:
.db "...done",$00
;-----------------------------------------------------------------
;
; Include other subsystems
|
And this includes all the other modules.
|
.include "eyes.asm"
.include "uart.asm"
.include "state.asm"
.include "monitor.asm"
;-----------------------------------------------------------------
The eyes are abstracted from a series of analog inputs to a simple set of on/off bits by these routines.
EYES.ASM
This file contains all the code for dealing with the eyes.
;-----------------------------------------------------------------
;
; Name: eyes.asm
; Title: Eye circuit services for robot
; Version: 1.0
; Last updated: 2001.03.10
; Target: AT90S2313
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
|
I really went to town on the comments in this file...
|
; The eye circuit referred to above is a photodiode operating
; in zero bias mode (cathode to ground) connected to a two stage
; simple transistor amplifier, which is in turn attached to the
; negative input of the comparator. The circuit diagram for this can
; be summarized as follows:
;
; Photodiode cathode to ground, anode through 100nF capacitor to
; base of T1. T1 is a BC639, base also through 150k to collector,
; emitter to ground, collector to 30k to drive pin. Collector of
; T1 decoupled to next stage with 100nF capacitor.
;
; Pins EYEDRIVEL,R and B (PD4-6) supply the power to drive the
; first stage amplifiers for the photodiodes.
;
; All three first stage outputs are commoned onto a 1k resistor
; into the base of the next stage, T2, also a BC639, base also
; through 30k to collector, collector through 6k8 to positive
; supply. Collector also feeds into inverting input of on-chip
; comparator.
;
; Simple R-C ADC (3k into 10nF) is driven from RCDRIVE (PB2) and
; the junction is connected to the noninverting input of the
; on-chip comparator (RCREAD).
;
; Timer 1 is used in input capture mode to capture a positive edge on
; the comparator.
;
; Pin IREDDRIVE (PB3) is used to drive an IRED (high=on) through
; another pair of transistors (to amplify current) which is then
; used to take alternate on/off light/dark readings which are then
; totalled as a difference over several cycles.
;
; PUBLISHED FUNCTIONS
;
; reset_eyes
;
; Resets this module. Called on reset. Sets I/O settings, T1
; compare mode and count rate, discharges RC network.
;
;
; update_eyes
;
; Called on a 2kHz counter to actually run the sampling.
;
; The IRED is run at 50% duty during the sampling. Input counts are
; accumulated using successive addition and subtraction to implement
; a simple high-pass filter which resonates at around 1kHz (which is
; not coincidentally the IRED drive frequency).
;
; This filter effects an average over many cycles (64 in this example)
; to help eliminate random noise, which should add and subtract from
; the accumulator evenly.
;
;
; read_eyes
;
; Reads eye states. Returns R0 as a bitvector, with bit 0 indicating
; EyeL state, bit 1 EyeR, and bit 2 EyeB. Whether the eye is "on" or
; not is determined by the thresholds set by the calibration cycle.
;
; Tests suggest that a measurable signal can be obtained from objects
; between 10 and 20cm away from the eye, depending on size, reflectivity,
; etc.
;
;
; calibrate_eyes
;
; Calibrates the eyes over a few cycles, setting the threshold of each
; eye to a sensible value.
;
; For a real-world app, it would be a good idea to run a calibration
; phase to avoid false readings. This should read the eye in a quies-
; cent state (nothing in view) and get estimates of the peak, minimum
; and typical counts from the integrator/filter. A sensible threshold
; can then be established for triggering the device.
;
;
;-----------------------------------------------------------------
;
; Defines
|
When we are reading the eyes, we take a certain number of samples (EYE_SAMPLES), which are in phase with the IRED drive,
and then switch eyes. After this we need to wait for EYE_REST samples before sampling the eye again, to allow
the amplifier to start up and stabilise - remember that we are switching eyes by switching the power to
the first stage of the photodiode amplifier.
|
.equ EYE_SAMPLES =$40 ; number of samples taken for reading
.equ EYE_REST =$10 ; number of cycles to wait after switching eyes
.equ EYE_CALIBRATECOUNT =$40 ; number of cycle to calibrate eye
;-----------------------------------------------------------------
;
; Variables
.dseg
eye_count: .byte 1 ; interrupt counter
eye_active: .byte 1 ; active eye
eye_state: .byte 1 ; eye drive mask
eye_ired: .byte 1 ; ired drive state
eye_accuml: .byte 1 ; accumulator for dsp-style filter calculation
eye_accumh: .byte 1
eye_valuel: .byte 1 ; accumulator is saturated to lie between
eye_valuer: .byte 1 ; +00 and +FF during transfer.
eye_valueb: .byte 1
eye_thresholdll: .byte 1 ; low thresholds set by calibrate_eye
eye_thresholdrl: .byte 1 ; these are also used to store the MAXIMUM value
eye_thresholdbl: .byte 1 ; encountered during calibration.
eye_thresholdlh: .byte 1 ; high thresholds set by calibrate_eye
eye_thresholdrh: .byte 1 ; paradoxically, these are also used to store
eye_thresholdbh: .byte 1 ; the MINIMUM value encountered during calibration
eyes_triggered: .byte 1 ; current trigger state for each eye
;-----------------------------------------------------------------
;
; Code
;
; Y is assumed to point to the RAM.
; RAM is assumed to be cleared to 0's on reset.
.cseg
;-----------------------------------------------------------------
;
; reset_eyes
|
This is just the reset routine for the eye module. This sets up the initial state for the eye drive pins,
the current eye, and sets up and enables the ADC and timer 1, which we will be using to actually do the sampling.
|
reset_eyes:
; discharge at start
ldi rTEMP,(EYE_SAMPLES+EYE_REST) ; count + recovery time
sty eye_count,rTEMP
ldi rTEMP,mEYEDRIVEL ; Initial photodiode drive mask
sty eye_state,rTEMP
rcall eye_drive
; set up interrupt masks to enable timer 1 interrupt for timeout
; and input capture.
in rTEMP,TIMSK
ori rTEMP, (1<<TOIE1)|(1<<TICIE)
out TIMSK,rTEMP
; set up analog comparator mode to allow input capture
ldi rTEMP,$07 ; ACIC enable, rising edge
out ACSR,rTEMP
ret
;-----------------------------------------------------------------
;
; update_eyes
; Called from within the timer routine on 2kHz timer 0.
|
This is called at 2kHz, and the first part just decides whether we are sampling or resting. We count down eye-count
from EYE_SAMPLES+EYE_REST - if the count is above EYE_SAMPLES, then we're still resting. If it's negative, then we've
expired and need to transfer the digital filter accumulator into the appropriate eye value location. Otherwise, we're doing
a sample.
|
update_eyes:
ldy rITEMP,eye_count ; tick counter
dec rITEMP
sty eye_count,rITEMP
brmi update_eyes_resetcount
cpi rITEMP,EYE_SAMPLES ; still resting?
brcc update_eyes_skipcount
|
This is the sample routine. We set the start count for timer 1 to FF00 (i.e. 256 counts before overflowing), set the
RC network to start ramping up in voltage, and start timer 1. The actual reading happens on the interrupts from timer 1.
|
ser rITEMP ; start count = $FF00
clr rITEMP2
out TCNT1H,rITEMP
out TCNT1L,rITEMP2
ldi rITEMP,mDDRBREAD
out DDRB,rITEMP ; set data direction for reading
sbi PORTB,bRCDRIVE ; drive RC network, all other bits unchanged
sbic ACSR,ACO ; already triggered? skip if ok
rjmp update_eyes_premature ; premature timeout!
ldi rITEMP, $41 ; start counter1, no predivider
out TCCR1B,rITEMP
update_eyes_skipcount:
ret
update_eyes_premature:
ldy rITEMP2,eye_ired
ldi rITEMP,$08 ; toggle IRED drive after reading
eor rITEMP2,rITEMP
sty eye_ired,rITEMP2
ret
|
Here we've finished reading so we clamp the eye readings into the range 00..FF, and store it in one of the three eye
value variables. We also advance to driving the next eye.
|
update_eyes_resetcount:
; Store accumulator value in appropriate eye.
clr rITEMP2
ldy rISCRATCH,eye_accumh
and rISCRATCH,rISCRATCH
brmi eye_valuefound
ser rITEMP2
brne eye_valuefound
ldy rITEMP2,eye_accuml
eye_valuefound:
mov rISCRATCH2,XL
ldi XL,eye_valuel ; address of eye
ldy rISCRATCH,eye_active ; +active eye index
add XL,rISCRATCH
st X,rITEMP2
mov XL,rISCRATCH2
ldy rITEMP,eye_state ; advance to next eye
cpi rITEMP,mEYEDRIVEB
breq eye_state_reset
inc rISCRATCH
add rITEMP,rITEMP
rjmp eye_next_done
eye_state_reset:
clr rISCRATCH
ldi rITEMP,mEYEDRIVEL
eye_next_done:
sty eye_active,rISCRATCH
sty eye_state,rITEMP
clr rISCRATCH
sty eye_accumh,rISCRATCH
sty eye_accuml,rISCRATCH
sty eye_ired,rISCRATCH
ldi rITEMP,(EYE_SAMPLES+EYE_REST) ; count + recovery time
sty eye_count,rITEMP
rjmp eye_drive
;-----------------------------------------------------------------
;
; Timer1 vectors - capture and accumulate value.
|
This is the overflow interrupt for timer 1. If the timer has overflowed, then the RC network has not reached
the input voltage in the allotted time and we assign a limit value of FF to the value read.
|
eye_overflow:
in rISREGSAVE,SREG
ldi rITEMP, $40 ; stop counter1 - timed out
out TCCR1B,rITEMP
clr rISCRATCH
com rISCRATCH ; $FF
rjmp eye_setvalue
|
This is what normally happens - the RC voltage has surpassed the input voltage from the eye preamplifier, and has triggered
an input capture event. This copies the current value of timer 1 to the ICR1 registers in I/O space. We can now use this to
work out how long it took to get there and thus get a reasonable guess at the voltage.
|
eye_capture:
in rISREGSAVE,SREG
ldi rITEMP, $40 ; stop counter1
out TCCR1B,rITEMP
in rISCRATCH2,ICR1H ; unused, but required for semantics of 16 bit read.
in rISCRATCH ,ICR1L
|
Whatever the value, it is alternatively added and subtracted from a 16-bit accumulator. This is so that noise and DC
levels will cancel out, but any signal in phase with the IRED drive (which is also driven from the same timer 0 interrupt
which starts the sampling process) will actually increase in magnitude.
|
eye_setvalue:
clr rISCRATCH2
ldy rISCRATCH3,eye_accuml
ldy rISCRATCH4,eye_accumh
ldy rITEMP2,eye_ired
and rITEMP2,rITEMP2
brne eye_subtract
add rISCRATCH3,rISCRATCH
adc rISCRATCH4,rISCRATCH2 ; 0
rjmp eye_add
eye_subtract:
sub rISCRATCH3,rISCRATCH
sbc rISCRATCH4,rISCRATCH2 ; 0
eye_add:
rcall eye_drive
ldi rITEMP,mIREDDRIVE ; toggle IRED drive after reading
eor rITEMP2,rITEMP ; IRED state still in rITEMP2
sty eye_ired,rITEMP2
sty eye_accuml,rISCRATCH3
sty eye_accumh,rISCRATCH4
out SREG,rISREGSAVE
reti
;-----------------------------------------------------------------
;
; eye_drive
; Common routine to reset ADC RC network
; PD drive value passed in register DRIVE
eye_drive:
in rITEMP,PORTD
cbr rITEMP,mEYEDRIVES
ldy rISCRATCH,eye_state
or rITEMP,rISCRATCH
out PORTD,rITEMP ; and output drive values
in rITEMP,PORTB
cbr rITEMP,(mANALOGIN|mRCDRIVE|mIREDDRIVE)
ldy rISCRATCH,eye_ired
or rITEMP,rISCRATCH
out PORTB,rITEMP
ldi rITEMP,mDDRDISCHARGE ; outputs on B0, B2, B3-B7
out DDRB,rITEMP
ret
;-----------------------------------------------------------------
;
; read_eyes
; Read eye state and return as bit vector. This uses the low and high
; thresholds, together with the current state, as hysteresis.
read_eyes:
ldi rPARAM1,$04 ; mask for eye 2 to start
clr rRETURN
ldi ZL,eye_valuel+3 ; one past end of value array
read_eyes_loop:
ld rSCRATCH,-Z
ldd rSCRATCH2,Z+(eye_thresholdll-eye_valuel) ; low threshold
ldy rTEMP,eyes_triggered
and rTEMP,rPARAM1 ; check if this eye triggered already
brne read_eyes_hysteresis ; yes - continue to use low threshold
ldd rSCRATCH2,Z+(eye_thresholdlh-eye_valuel) ; else use high one instead
read_eyes_hysteresis:
cp rSCRATCH,rSCRATCH2
brcs read_eyes_low
or rRETURN,rPARAM1 ; higher than selected threshold so set
read_eyes_low:
lsr rPARAM1 ; finished all eyes?
brne read_eyes_loop ; not yet
sty eyes_triggered,rRETURN ; store for next time
ret
;-----------------------------------------------------------------
;
; calibrate_eyes
; Waits for EYE_CALIBRATECOUNT full eye cycles and records
; min and max for each eye, which are then used to set the thresholds
; for sensing
calibrate_eyes:
ldi rLOOPI,EYE_CALIBRATECOUNT
calibrate_eyes_counted:
clr rTEMP
sty eye_thresholdll,rTEMP ; maxima = minvalue to start with
sty eye_thresholdrl,rTEMP
sty eye_thresholdbl,rTEMP
ser rTEMP
sty eye_thresholdlh,rTEMP ; minima = maxvalue to start with
sty eye_thresholdrh,rTEMP
sty eye_thresholdbh,rTEMP
calibrate_eyes_mainloop:
calibrate_eyes_wait02:
ldy rTEMP2,eye_active
cpi rTEMP2,$02
brne calibrate_eyes_wait02
calibrate_eyes_wait00:
ldy rTEMP2,eye_active
cpi rTEMP2,$00
brne calibrate_eyes_wait00
; compare each value against min and max and adjust
; this is done with a predecrement on Z therefore we
; start z one beyond the end of the value array
; we can also easily check z for end condition which
; eliminates the need for a separate loop counter
ldi ZL,eye_valuel+3
calibrate_eyes_minimax:
ld rSCRATCH,-Z
ldd rSCRATCH2,Z+(eye_thresholdll-eye_valuel) ; max
ldd rSCRATCH3,Z+(eye_thresholdlh-eye_valuel) ; min
cp rSCRATCH,rSCRATCH2
brcs calibrate_eyes_notmax
std Z+(eye_thresholdll-eye_valuel),rSCRATCH
calibrate_eyes_notmax:
cp rSCRATCH,rSCRATCH3
brcc calibrate_eyes_notmin
std Z+(eye_thresholdlh-eye_valuel),rSCRATCH
calibrate_eyes_notmin:
cpi ZL,eye_valuel
brne calibrate_eyes_minimax
dec rLOOPI
brne calibrate_eyes_mainloop
; postprocess min/max into thresholds
; loop works the same way as for the minimax calculation
ldi ZL,eye_thresholdll+3
|
Having found the minimum and maximum values for each eye, we can now set the thresholds based on them. There are several
different variants of this, some of which are commented out. For the half-difference threshold, which I found OK, we set
the "off" threshold to max, and the "on" threshold to max plus half the difference between max and min.
|
calibrate_eyes_threshold:
ld rSCRATCH,-Z ; max
ldd rSCRATCH2,Z+(eye_thresholdlh-eye_thresholdll) ; min
sub rSCRATCH2,rSCRATCH ; min-max = -(max-min)
; Half difference threshold
asr rSCRATCH2 ; (min-max)/2
sub rSCRATCH,rSCRATCH2 ; max-(min-max)/2 = max+(max-min)/2
; Single difference threshold
;
; sub rSCRATCH,rSCRATCH2 ; max-(min-max) = max+(max-min)
; Double difference threshold
;
; sub rSCRATCH,rSCRATCH2 ; max-(min-max) = max+(max-min)
; sub rSCRATCH,rSCRATCH2 ; max+(max-min)*2
std Z+(eye_thresholdlh-eye_thresholdll), rSCRATCH ; store back again
cpi ZL,eye_thresholdll
brne calibrate_eyes_threshold
ret
;-----------------------------------------------------------------
The monitor uses some basic RS232 services to communicate with a host PC for debugging.
RS232.ASM
This file contains all the code for dealing with the UART.
;-----------------------------------------------------------------
;
; Name: uart.asm
; Title: AVR Uart services
; Version: 1.0
; Last updated: 2001.02.03
; Target: AT90Sxxxx (All devices)
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
; UART service which uses interrupts to read characters
; from the serial port and buffer them, but which handles trans-
; mission synchronously, without interrupts.
;
;-----------------------------------------------------------------
.dseg
|
These declarations define the buffer which will be used for storing incoming characters from the UART. uart_rhead and uart_rtail
are the adresses in the buffer of the head (where a new character will be written by the UART) and tail (where a character will be read
from by the program) of a circular buffer. The number of characters currently in the buffer is uart_rsize, and any errors
are reported with a non-0 value in uart_rerror.
|
uart_rhead: .byte 1
uart_rtail: .byte 1
uart_rsize: .byte 1
uart_rerror: .byte 1
|
The buffer itself needs to be a specific size and at a specific position.
|
.equ UART_BUFSIZE =$20 ; must be power of 2
.equ UART_BUFFER =$A0 ; must be on a UART_BUFSIZE boundary
|
And here's the standard calculation of baud rate register values.
|
.equ UART_BAUDRATE =9600
.equ UART_BAUDK =(ROBOT_CLOCKRATE/(16*UART_BAUDRATE))-1
;-----------------------------------------------------------------
.cseg
;-----------------------------------------------------------------
;
; Reset vector - setup
|
The reset routine just sets up the head and tail, and enables the UART for interrupt-driven receive and polled
transmit. It also outputs a welcome message. Remember that addresses in flash should be multiplied by 2! At the end
it restores the zero value in ZH, since other routines may rely on it.
|
reset_uart:
ldi rTEMP,UART_BUFFER
sty uart_rhead,rTEMP
sty uart_rtail,rTEMP
ldi rTEMP,UART_BAUDK
out UBRR,rTEMP ; load baudrate
ldi rTEMP,(1<<TXEN)|(1<<RXEN)|(1<<RXCIE) ; enable transmit/receive, enable rx int.
out UCR,rTEMP
ldi ZH,high(Message*2) ; haha! Word address
ldi ZL,low(Message*2)
rcall uart_writestringlf
clr ZH
ret
;-----------------------------------------------------------------
;
; Receive vector - buffer input
|
This is the receive interrupt vector. It checks for errors and then writes the character into the buffer and
updates the head pointer.
|
uart_receive:
in rISREGSAVE,SREG
mov rISCRATCH4,XL
sbic USR,FE ; check for
rjmp uart_rxerr_fe ; framing error
ldy rITEMP,uart_rsize
cpi rITEMP,UART_BUFSIZE
breq uart_rxerr_bf ; buffer full
inc rITEMP
sty uart_rsize,rITEMP
ldy XL,uart_rhead
in rITEMP,UDR
st X+,rITEMP
andi XL,(UART_BUFSIZE-1)
ori XL,UART_BUFFER
sty uart_rhead,XL
sbic USR,OR ; check for
rjmp uart_rxerr_or ; overrun
uart_rxend:
mov XL,rISCRATCH4
out SREG,rISREGSAVE
reti
uart_rxerr_fe:
uart_rxerr_bf:
in rITEMP,UDR ; dummy read to clear RxC
uart_rxerr_or:
ldi rITEMP,1 ; overflow, framing error
sty uart_rerror,rITEMP
rjmp uart_rxend
;-----------------------------------------------------------------
;
; Read from receiver buffer char=rRETURN, wait until character ready
|
This reads a character from the buffer. In this version, it first checks to see if any characters are ready.
If not, then it will loop until one comes in before falling through to read it.
|
uart_readcharwait:
rcall uart_charready
breq uart_readcharwait
|
This reads a character from the buffer. If no characters are ready, then it will perform incorrectly.
|
uart_readchar:
push XL
ldy XL,uart_rtail
ld rRETURN,x+
andi XL,(UART_BUFSIZE-1)
ori XL,UART_BUFFER
sty uart_rtail,XL
cli
ldy rSCRATCH,uart_rsize
dec rSCRATCH
sty uart_rsize,rSCRATCH
sei
pop XL
ret
;-----------------------------------------------------------------
;
; Ask if a character is ready from the UART (eq=no, ne=yes)
|
This is simple to find out - if there is a character, then the number of characters in the buffer is non-0.
|
uart_charready:
ldy rSCRATCH,uart_rsize
and rSCRATCH,rSCRATCH
ret
;-----------------------------------------------------------------
;
; Write to transmitter a string followed by a cr/lf
uart_writestringlf:
rcall uart_writestring
; falls through to uart_writecrlf...
;-----------------------------------------------------------------
;
; Write to transmitter a cr/lf pair
uart_writecrlf:
ldi rPARAM1,$0D
rcall uart_writechar
ldi rPARAM1,$0A
; falls through to uart_writechar again...
;-----------------------------------------------------------------
;
; Write to transmitter char=rPARAM1
|
This is a simple, polled write character routine. It waits until the UDRE bit is set (UART Data Register Empty) which
indicates that it is safe to transmit another character.
|
uart_writechar:
sbis USR,UDRE
rjmp uart_writechar
out UDR,rPARAM1
ret
;-----------------------------------------------------------------
;
; Write to transmitter hex value=rPARAM1
|
This converts the value in rPARAM1 into two hex digits (0..9, A..F) and outputs them to the serial port.
|
uart_writehex:
push rPARAM1
swap rPARAM1
rcall uart_writehexit
pop rPARAM1
uart_writehexit:
andi rPARAM1,$0F
subi rPARAM1,-48
cpi rPARAM1,$3A
brcs uart_writechar
subi rPARAM1,-7
rjmp uart_writechar
;-----------------------------------------------------------------
;
; Write 0-terminated string in Flash at Z to transmitter
uart_writestring:
lpm ; r0,Z
adiw ZL,1
mov rPARAM1,r0
and rPARAM1,rPARAM1
breq uart_writestringend
rcall uart_writechar
rjmp uart_writestring
uart_writestringend:
ret ; NB: Shared with uart_writestring
;-----------------------------------------------------------------
;
; Write 0-terminated string in RAM at Z to transmitter, with crlf
; if reqd.
;
; - - - UNUSED - - - - - - - - - - - - - - - - - - - - - - - - - -
;
;uart_writestringram:
; clr zh
;uart_writestringramloop:
; ld rPARAM1,z+
; and rPARAM1,rPARAM1
; breq uart_writestringend
;
; rcall uart_writechar
;
; rjmp uart_writestringramloop
;
;uart_writestringramlf:
; rcall uart_writestringram
; rjmp uart_writecrlf
;
; - - - UNUSED - - - - - - - - - - - - - - - - - - - - - - - - - -
;-----------------------------------------------------------------
;
; Constant Data
;-----------------------------------------------------------------
;
; Message
Message:
.db $0d,$0a,"UART Ready",$00
;-----------------------------------------------------------------
The main personality of the robot is based on a state machine description stored in the EEPROM.
STATE.ASM
This file defines the state machine interpreter.
;-----------------------------------------------------------------
;
; Name: state.asm
; Title: State machine for running the robot
; Version: 1.0
; Last updated: 2001.03.17
; Target: AT90S2313
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
|
Once again, comments'R'us.
|
; The high-level behavior of the robot is determined by a state
; machine, which specifies the response to several stimuli.
;
; The states are written into the EEPROM, either by serial
; programming, or via the separate robot monitor subsystem.
;
; Each state is stored as a compressed description, as follows:
;
; Byte 0:
; Bits 7-4, motor output. 00 = stop, 01=reverse,
; 10=forward, 11=random state, for each motor
; Bit 3 reserved
; Bits 0-2, timeout value, approximately...
; 0000 = 1/8 second
; 0001 = 1/4 second
; 0010 = 1/2 second
; 0011 = 1 second
; 0100 = 2 seconds
; 0101 = 4 seconds
; 0110 = 8 seconds
; 0111 = 16 seconds
;
; Byte 1..n:
; Bits 7-4: Destination state for event 2n-2
; Bits 3-0: Destination state for event 2n-1
;
; Destination states are specified relative to this state:
; 0000 = +0 (self transition)
; 0001 = +1
; ...
; 0111 = +7
; 1000 = +0 (no transition - event inactive)
; 1001 = -7
; ...
; 1111 = -1
|
The difference between a self transition (0000) and a no transition (1000) is that a self transition resets the timer
associated with this state, but a no transition does not. Also, a transition off either end of the state table wraps back to the
other end. So in the common 32-state case, a +1 transition from state 31 will give state 0, and a -1 from state 0 will give state 31.
|
;
; Initial state is always state 0
; States are numbered 0-9, A-Z for convenience.
; Maximum number of states is set by available EEPROM space (128B)
;
;-----------------------------------------------------------------
;
; Defines
|
These define how many events there are, and the offsets to the motor byte and the start of the responses to events
within a particular state block. STATE_SIZE is the total number of bytes in a state block - for 6 events, this works out as 4.
|
.equ STATE_NUMEVENTS =$06 ; timeout, eyes1-3, bump1-2
.equ STATE_MOTOR =$00
.equ STATE_RESPONSE =$01
.equ STATE_SIZE =$01+(STATE_NUMEVENTS+1)/2
|
Using the above, we calculate how many state blocks will fit in the eeprom, and how much space they actually take.
|
.equ STATE_COUNT =(128/STATE_SIZE)
.equ STATE_SPACE =(STATE_SIZE*STATE_COUNT)
|
Some masks which will divide up the motor byte into the correct parts.
|
.equ STATE_mMOTORL =(mMOTORLA|mMOTORLB)
.equ STATE_mMOTORR =(mMOTORRA|mMOTORRB)
.equ STATE_mMOTORS =mMOTORS
.equ STATE_mTIMEOUT =$07
|
Some masks which will divide up the trigger byte into the correct parts.
|
.equ mEVENT_TIMEOUT =$01
.equ mEVENT_BUMPF =$02
.equ mEVENT_BUMPB =$04
.equ mEVENT_EYEL =$08
.equ mEVENT_EYER =$10
.equ mEVENT_EYEB =$20
;-----------------------------------------------------------------
;
; Macros
;
; Multiply register by state size.
; May use rSCRATCH as temporary register
; use: mulss rN
; rN <- rN * STATE_SIZE
.macro mulss
lsl @0
lsl @0
.endmacro
;-----------------------------------------------------------------
;
; Variables
.dseg
state_current: .byte 1 ; pointer to current state block
state_ticklo: .byte 1 ; tick count for timeout
state_tickhi: .byte 1 ; tick count for timeout
state_events: .byte 1 ; bitvector for triggered events
state_randoma: .byte 1 ; state for random number generator
state_randomb: .byte 1
state_randomc: .byte 1
state_lastbump: .byte 1 ; last bump switch state
state_lasteye: .byte 1 ; last eye proximity state
;-----------------------------------------------------------------
;
; Code
;
; Y is assumed to point to the RAM.
; RAM is assumed to be cleared to 0's on reset.
.cseg
;-----------------------------------------------------------------
;
; reset_state
;
; Reset state machine, update random number generator, etc.
reset_state:
ldi rTEMP,(mEVENT_BUMPF|mEVENT_BUMPB)
sty state_lastbump,rTEMP ; and store last bump state
;Init the random number generator
sty state_randoma,rTEMP ;since a 00,00,00 state will not
sty state_randomb,rTEMP ;progress.
sty state_randomc,rTEMP ;
; set pullups on port D for bump sensors
sbi PORTD,bBUMPF
sbi PORTD,bBUMPB
clr zl
rcall state_load ; load the first state
in rTEMP2,PORTB ; but output a zero motor state to I/O port.
cbr rTEMP2,mMOTORS
out PORTB,rTEMP2
ret
;-----------------------------------------------------------------
;
; state_load
;
; Load the current state and reinitialize state machine for
; this state. New current state pointer passed in in Z.
|
Within the state machine, current states are stored and manipulated as pointers. This saves us doing multiplies all the time.
Here, we are setting a new state so we need to read and set the motor states...
|
state_load:
sty state_current,zl
rcall state_readee
mov rTEMP,rRETURN
mov rTEMP2,rRETURN
andi rTEMP,STATE_mMOTORL
cpi rTEMP,STATE_mMOTORL
brne state_loadml
push rTEMP2
rcall state_random
rcall state_random
andi rTEMP,STATE_mMOTORL
pop rTEMP2
state_loadml:
andi rTEMP2,STATE_mMOTORR
cpi rTEMP2,STATE_mMOTORR
brne state_loadmr
push rTEMP
rcall state_random
rcall state_random
add rTEMP,rTEMP ; need to shift this
add rTEMP,rTEMP ; else we get same 2 bits as above
andi rTEMP,STATE_mMOTORR
mov rTEMP2,rTEMP
pop rTEMP
state_loadmr:
or rTEMP,rTEMP2
; output motor state to I/O port.
in rTEMP2,PORTB
cbr rTEMP2,mMOTORS
or rTEMP2,rTEMP
out PORTB,rTEMP2
|
...and set the new timeout value for this state. This is (1<<TIMEOUT)+16 ticks of the main state machine clock.
|
; load tick count
ldi rTEMP,1
rcall state_readee
mov rTEMP2,rRETURN
andi rTEMP2,STATE_mTIMEOUT
rjmp state_loadloopstart
state_loadloop:
add rTEMP,rTEMP
dec rTEMP2
state_loadloopstart:
brne state_loadloop
ldi rTEMP2,$10 ; leeway
sty state_ticklo,rTEMP2
sty state_tickhi,rTEMP
ret
;-----------------------------------------------------------------
;
; update_state
;
; Update current state from bits in state_events. Bit 0 = event 0,
; etc. Also reads eyes.
|
This runs through each possible event source in turn, and if an event has happened it will trigger a transition to
a new state.
|
update_state:
rcall read_eyes
; Uncomment this block to use eye *transitions* rather than states
; to trigger events.
;
; ldy rTEMP,state_lasteye
; sty state_lasteye,rRETURN
;
; eor rTEMP,rRETURN ; catch transitions
mov rTEMP,rRETURN
lsl rTEMP
lsl rTEMP
lsl rTEMP ; shift up to proper place
ldy rPARAM1,state_events ; get events
or rPARAM1,rTEMP ; or in the eyes
sty state_events,rPARAM1 ; and store
;
; Now actually check all the bits in rPARAM1 in turn
;
ldi rPARAM2,$01 ; mask to clear bit later
ldy zl,state_current
ldi rLOOPI,STATE_NUMEVENTS-1
update_state_loop:
inc zl
rcall state_readee
sbrc rPARAM1,0
rcall update_state_now ; update state with HIGH nybble
lsr rPARAM1
lsl rPARAM2
dec rLOOPI
brmi update_state_exit
swap rRETURN
sbrc rPARAM1,0
rcall update_state_now ; update state with HIGH nybble
lsr rPARAM1
lsl rPARAM2
dec rLOOPI
brpl update_state_loop
update_state_exit:
ret
update_state_now:
; update state according to high nybble of rRETURN.
; clear event bit
ldy rTEMP,state_events
com rPARAM2
and rTEMP,rPARAM2
com rPARAM2
sty state_events,rTEMP
; is this a null transition? if so, continue.
mov rTEMP,rRETURN
andi rTEMP,$F0
cpi rTEMP,$80
breq update_state_notrans ; null transition - skip
; otherwise, we have a real event causing a transition
ldy zl,state_current ; get current state for later on, in case we do a self transition
mov rLOOPJ,rLOOPI ; save this for later too
clr rLOOPI ; no more loops
cpi rTEMP,$00
breq update_state_selftrans ; self transition - short cut, no uart output
; sign extend difference
asr rTEMP
asr rTEMP
asr rTEMP
asr rTEMP
mulss rTEMP ; get difference
add zl,rTEMP ; and add
brpl update_state_nneg
subi zl,-STATE_SPACE ; adjust if -ve.
rjmp state_load
update_state_nneg:
cpi zl,STATE_SPACE
brcs update_state_novr
subi zl,STATE_SPACE
update_state_novr:
;
; Write triggered event to UART, and continue
;
|
If a state change happens, then a debugging message is sent to the UART. This is of the form: "C:E->N" where C is the current
state, E is the event triggered, and N is the new state. States are numbered from 0..9 then from A..Z (or less far if fewer states are
available).
|
push zl
ldy rPARAM1,state_current
rcall monitor_statetochar
rcall uart_writechar
ldi rPARAM1,':'
rcall uart_writechar
ldi rPARAM1,STATE_NUMEVENTS-1+'0'
sub rPARAM1,rLOOPJ
rcall uart_writechar
ldi rPARAM1,'-'
rcall uart_writechar
ldi rPARAM1,'>'
rcall uart_writechar
mov rPARAM1,zl
rcall monitor_statetochar
rcall uart_writechar
rcall uart_writecrlf
pop zl
update_state_selftrans:
rjmp state_load
update_state_notrans:
; push rPARAM1
; rcall uart_writecrlf
; pop rPARAM1
ret
;-----------------------------------------------------------------
;
; state_tick
;
; Tick counter for state machine timeout values, etc.
state_tick:
ldy rISCRATCH,state_ticklo
dec rISCRATCH
sty state_ticklo,rISCRATCH
brne state_tickend
; 1/8 s basic tick. Sample the bump switches.
in rITEMP2,GIFR ; read IFR for bumps
out GIFR,rITEMP2 ; and reset set bits
; uncomment next block to allow events triggered on
; bump switch TRANSITIONS, not levels.
; ldy rITEMP,state_lastbump
; sty state_lastbump,rITEMP2
; eor rITEMP2,rITEMP ; catch TRANSITIONS not LEVELS
swap rITEMP2
lsr rITEMP2
andi rITEMP2,(mEVENT_BUMPF|mEVENT_BUMPB)
ldy rITEMP,state_events
or rITEMP,rITEMP2 ; indicate events
; now check for timeout
ldy rISCRATCH,state_tickhi
dec rISCRATCH
sty state_tickhi,rISCRATCH
brne state_notimeout
ori rITEMP,mEVENT_TIMEOUT ; indicate events
state_notimeout:
sty state_events,rITEMP ; and put event status back
state_tickend:
ret
;-----------------------------------------------------------------
;
; state_readee
;
; Read from eeprom, address Z, return in rRETURN
state_readee:
sbic EECR,EEWE ; check not writing
rjmp state_readee
out EEAR,zl
sbi EECR,EERE
in rRETURN,EEDR
ret
;-----------------------------------------------------------------
;
; state_random
;
; Random number generator
; Maximal length 19 bit shift register sequence, adapted from the
; "getting started" notes of Dave van Horn. Thanks, Dave!
state_random:
; rSCRATCH3 rSCRATCH2 rSCRATCH
;22222111 11111110 00000000
;43210987 65432109 87654321
ldy rSCRATCH,state_randoma
ldy rSCRATCH2,state_randomb
ldy rSCRATCH3,state_randomc
mov rTEMP,rSCRATCH ;Make copy
mov rTEMP2,rSCRATCH3 ;Make copy
rol rSCRATCH ;Shift the bits D7->Carry
rol rSCRATCH2 ;Carry->D0 D7->Carry
rol rSCRATCH3 ;Carry->D0 D7->Carry, but that will be fixed.
;Now we have to Xor the bits to see what
;goes in to bit 1
rol rTEMP2 ;Move bit 19->20
andi rTEMP2,$08 ;Important that D1,0 be zero
andi rTEMP,$13 ;Mask out irrelevants, protecting bit 19
or rTEMP2,rTEMP ;Get bits 5->21 2->18 1->17
andi rTEMP2,$18 ;Nuke bits 18,17
lsr rTEMP2 ;19->19 5->20
lsr rTEMP2 ;19->18 5->19
lsr rTEMP2 ;19->17 5->18
eor rTEMP2,rTEMP ;First xor, result in rTEMP2 (5x2 in 02 and 19x1 in 01)
mov rTEMP,rTEMP2 ;Make a copy
ror rTEMP ;Move the 5x2 result to 01
andi rTEMP2,$01 ;Mask off everything else
andi rTEMP,$01 ;in both
eor rTEMP2,rTEMP ;
brne state_random_1 ;If one, do that, else
state_random_0:
;Set a zero in the lsb of the low byte
mov rTEMP,rSCRATCH ;Get the low byte
andi rTEMP,$FE ;Make the LSB zero
mov rSCRATCH,rTEMP ;Put it back
rjmp state_random_exit ;Bye bye
state_random_1:
;Set a one in the lsb of the low byte
mov rTEMP,rSCRATCH ;Get the low byte
ori rTEMP,$01 ;Make the LSB one
mov rSCRATCH,rTEMP ;Put it back
state_random_exit:
sty state_randoma,rSCRATCH
sty state_randomb,rSCRATCH2
sty state_randomc,rSCRATCH3
mov rRETURN,rSCRATCH ; and return it
ret
;-----------------------------------------------------------------
;-----------------------------------------------------------------
;
; Default state machine
; Derived from program loaded into monitor as in the comments
.eseg
.org 0
; +------------------------ state number
; | ++-------------------- motor state
; | || +----------------- timeout value
; | || | Next state...
; | || | +------------- ...on timeout
; | || | | ++---------- ...on bump switches, fore or back
; | || | | || +++------ ...on eye activation, left, right or back
; | || | | || |||
.db $F5,$13,$25,$42 ; s0 ?? 5 1 32 542 ; exploring 4s, run away
.db $F5,$62,$14,$31 ; s1 ?? 5 7 32 542 ; exploring 4s, run away
.db $93,$E1,$83,$28 ; s2 FF 3 0 3- 54- ; shy forward
.db $63,$D8,$F8,$8F ; s3 RR 3 0 -2 --2 ; shy backward
.db $23,$FF,$EF,$8E ; s4 R0 3 3 32 3-2 ; reverse left
.db $43,$EE,$D8,$ED ; s5 0R 3 3 32 -32 ; reverse right
.db $F5,$AD,$CF,$EC ; s6 ?? 5 0 32 542 ; exploring 4s, run away
.db $F5,$92,$14,$31 ; s7 ?? 5 0 98 BA8 ; exploring 4s; aggressive!
.db $63,$E1,$83,$28 ; s8 RR 3 6 9- BA- ; attack backward, then run away
.db $93,$D8,$F8,$8F ; s9 FF 3 6 -8 --8 ; attack forward, then run away
.db $13,$FF,$EF,$8E ; sA F0 3 9 98 9-8 ; attack right, then forward, then run away
.db $83,$EE,$D8,$ED ; sB 0F 3 9 98 -98 ; attack left, then forward, then run away
|
Examination of the above machine reveals why the thing was so cowardly on the day. It only has one aggressive state,
from which any and all transitions lead to nonaggressive states! This was done to avoid it getting stuck in a state where
it continually attacked a wall or some other obstacle, but in reality it proved to be far too cautious.
|
;-----------------------------------------------------------------
In order to aid debugging and allow the personality to be changed using only a RS232 terminal, I included a simple
debug monitor.
MONITOR.ASM
This file defines the debug monitor.
;-----------------------------------------------------------------
;
; Name: monitor.asm
; Title: Robot monitor service
; Version: 1.0
; Last updated: 2001.03.19
; Target: AT90S2313
;
;-----------------------------------------------------------------
;
; DESCRIPTION
;
; Monitor service which runs from the serial port.
;
; This implements a simple monitor for influencing the behavior
; of the robot and loading eeprom data tables, etc.
;
; Command set
;
; ;
; Comment - rest of line ignored
;
; 0..7
; Trigger event 0..7 (6,7 reserved for now)
;
; d<s>
; Display state <s>
;
; s<s> mm t s s s s s s
; mm=motor state (0,F,R,?)
; t=timeout 0-7
; s=destination state
;
; States are numbered 0-9, A-Z (or a-z)
;
; Execution of the state machine for the robot is suspended
; until completion of a monitor command with trailing crlf
;
;-----------------------------------------------------------------
.dseg
|
Thes variables define the state of the monitor. The selected state is in monitor_select, and the state block being
constructed is in monitor_sblock. The state block is only loaded into the eeprom when fully constructed, to prevent
errors causing too much havoc.
|
monitor_select: .byte 1 ; selected state
monitor_sblock: .byte 4 ; state block
;-----------------------------------------------------------------
.cseg
;-----------------------------------------------------------------
;
; reset_monitor
; Reset monitor status. Should be called after reset_uart.
reset_monitor:
ldi zl,low(monitor_welcome*2)
ldi zh,high(monitor_welcome*2)
monitor_exitmessage:
rcall uart_writestringlf
clr zh
ret
;-----------------------------------------------------------------
;
; update_monitor
; Main monitor routine.
|
This just sits and listens for characters on the UART. If nothing has been received, it goes straight back to the main loop.
|
update_monitor:
rcall uart_charready
brne update_monitor_charready
ret
update_monitor_charready:
rcall monitor_readchar
|
First, we check for digits 0-7 for event triggering.
|
cpi rPARAM1,'0'
brlt update_monitor_notdigit
cpi rPARAM1,'8'
brlt update_monitor_event
|
Check for carriage returns or comments. We can't use the character literal ';' because even though it's in single quotes
it still signals the start of a comment.
|
update_monitor_notdigit:
cpi rPARAM1,$0d
breq update_monitor_comment
cpi rPARAM1,$0a
breq update_monitor_comment
cpi rPARAM1,$3B ; semicolon - can't use ';' because it starts a comment. Bleah.
breq update_monitor_comment
|
OK, so now we check for the main commands d (for display) and s (for state load). If it's not one of them, we emit a * as
an error marker and hop into the error routine.
|
cpi rPARAM1,'d'
breq update_monitor_displaystate
cpi rPARAM1,'s'
breq update_monitor_setstate
ldi rPARAM1,'*'
rcall uart_writechar
rcall monitor_ignoreline_charread
rjmp update_monitor_error
|
For comments, we just ignore the rest of the line.
|
update_monitor_comment:
rcall monitor_ignoreline_charread
rjmp update_monitor_exit
|
For event triggers, we work out which event should be triggered and write the appropriate bit into the state machine's
events variable. To the state machine, this is indistingushable from a real event.
|
update_monitor_event:
ldi rTEMP,$01
update_monitor_event_loop:
cpi rPARAM1,'0'
breq update_monitor_event_set
lsl rTEMP
dec rPARAM1
rjmp update_monitor_event_loop
update_monitor_event_set:
ldy rTEMP2,state_events
or rTEMP2,rTEMP
sty state_events,rTEMP2
rjmp update_monitor_exit
|
This displays the current contents of a particular state block in the same format as it was entered. The next character
after the d is taken to be the state block number and the rest of the line is ignored. Only when an end-of-line character is
received is the actual state transmitted to the RS232 port.
|
update_monitor_displaystate:
rcall monitor_readchar
rcall monitor_ignoreline ; ignore rest of line before continuing
rcall monitor_chartostate
rcall uart_writecrlf
update_monitor_showstate:
mov zl,rRETURN
rcall state_readee ; get motor bits etc.
mov rTEMP,rRETURN
ldi rPARAM1,'0'
andi rTEMP,STATE_mMOTORL ; motor bits
breq update_monitor_motorl
ldi rPARAM1,'R'
cpi rTEMP,mMOTORLA
breq update_monitor_motorl
ldi rPARAM1,'F'
cpi rTEMP,mMOTORLB
breq update_monitor_motorl
ldi rPARAM1,'?'
update_monitor_motorl:
rcall uart_writechar
mov rTEMP,rRETURN
ldi rPARAM1,'0'
andi rTEMP,STATE_mMOTORR ; motor bits
breq update_monitor_motorr
ldi rPARAM1,'R'
cpi rTEMP,mMOTORRA
breq update_monitor_motorr
ldi rPARAM1,'F'
cpi rTEMP,mMOTORRB
breq update_monitor_motorr
ldi rPARAM1,'?'
update_monitor_motorr:
rcall uart_writechar
ldi rPARAM1,' '
rcall uart_writechar
mov rPARAM1,rRETURN
rcall uart_writehexit
ldi rPARAM1,' '
rcall uart_writechar
ldi rLOOPI,3
update_monitor_ds_loop:
inc zl
rcall state_readee
mov rPARAM2,rRETURN
rcall update_monitor_display_dest
swap rPARAM2
rcall update_monitor_display_dest
dec rLOOPI
brne update_monitor_ds_loop
rjmp update_monitor_exit
|
This dsets the current contents of a particular state block. The next character
after the s is taken to be the state block number and the rest of the line is used to determine what the state block contents
are. The format of this information is shown in the comment at the top of the file. Only when an end-of-line character is
received is the actual state block created, and if there are any errors then a * is emitted at the error position along
with an error message and the state block is not loaded into the eeprom.
|
update_monitor_setstate:
rcall monitor_readchar ; read state character
rcall monitor_chartostate ; convert to pointer
mov zl,rRETURN
cpi zl,EEEND
brlt update_monitor_ss_ok1
;
; Note: This is interleaved here in order that it's not too far away from
; the code that uses it.
;
update_monitor_ss_error:
ldi rPARAM1,'*'
rcall uart_writechar
rcall monitor_ignoreline ; ignore rest of line before continuing
rjmp update_monitor_error
update_monitor_ss_ok1:
sty monitor_select,zl ; store state pointer
rcall monitor_readchar ; read first motor character
clr rPARAM2 ; rPARAM2 = first state block byte
cpi rPARAM1,'0'
breq update_monitor_ss_r
ori rPARAM2,mMOTORLA
cpi rPARAM1,'R'
breq update_monitor_ss_r
cpi rPARAM1,'r'
breq update_monitor_ss_r
ori rPARAM2,mMOTORLB
cpi rPARAM1,'?'
breq update_monitor_ss_r
cbr rPARAM2,mMOTORLA
cpi rPARAM1,'F'
breq update_monitor_ss_r
cpi rPARAM1,'f'
brne update_monitor_ss_error
update_monitor_ss_r:
rcall monitor_readchar ; read 2nd motor character
cpi rPARAM1,'0'
breq update_monitor_ss_t
ori rPARAM2,mMOTORRA
cpi rPARAM1,'R'
breq update_monitor_ss_t
cpi rPARAM1,'r'
breq update_monitor_ss_t
ori rPARAM2,mMOTORRB
cpi rPARAM1,'?'
breq update_monitor_ss_t
cbr rPARAM2,mMOTORRA
cpi rPARAM1,'F'
breq update_monitor_ss_t
cpi rPARAM1,'f'
brne update_monitor_ss_error
update_monitor_ss_t:
rcall monitor_readchar ; read timeout character
subi rPARAM1,'0'
brcs update_monitor_ss_error
cpi rPARAM1,8
brge update_monitor_ss_error
or rPARAM2,rPARAM1
ldi xl,monitor_sblock ; point to state block to fill in
st x+,rPARAM2 ; store in parameter block
clr rPARAM2
ldi rLOOPI,6 ; now read 6 state characters
update_monitor_ss_loop:
rcall monitor_readchar ; read state character
ldi rTEMP,$08
cpi rPARAM1,'-' ; null transition?
breq update_monitor_ss_null
rcall monitor_chartostate ; get destination state
mov rTEMP,rRETURN ; get destination state
ldy rTEMP2,monitor_select ; get current state
sub rTEMP,rTEMP2 ; subtract
asr rTEMP
asr rTEMP ; /4 = state number
subi rTEMP,-16 ; add 16 to make the compares easier
cpi rTEMP,(16-7)
brlt update_monitor_ss_error2 ; too far back - error
cpi rTEMP,(16+8)
brlt update_monitor_ss_ok2
update_monitor_ss_error2:
rjmp update_monitor_ss_error ; too far forward - error
update_monitor_ss_ok2:
andi rTEMP,$0F ; bottom nybble only
update_monitor_ss_null:
or rPARAM2,rTEMP ; or in the state
mov rTEMP,rLOOPI ; get current loop counter
andi rTEMP,$01 ; odd or even?
breq update_monitor_ss_even
st x+,rPARAM2 ; store state
clr rPARAM2 ; and clear ready for next loop
rjmp update_monitor_ss_next
update_monitor_ss_even:
swap rPARAM2 ; swap nybble up into top of state
update_monitor_ss_next:
dec rLOOPI
brne update_monitor_ss_loop
;
; Here, we should have a complete state block. Write it to the eeprom.
;
ldy zl,monitor_select
ldi xl,monitor_sblock
ldi rLOOPI,4 ; 4 bytes to write to eeprom
update_monitor_ss_write:
ld rPARAM1,x+
rcall monitor_write_eeprom ; write rPARAM1 to eeprom at Z
inc zl
dec rLOOPI
brne update_monitor_ss_write
rcall monitor_ignoreline ; ignore rest of line before continuing
rcall uart_writecrlf
rjmp update_monitor_exit
update_monitor_error:
ldi zl,low(monitor_errormsg*2)
ldi zh,high(monitor_errormsg*2)
rjmp monitor_exitmessage
update_monitor_exit:
ldi zl,low(monitor_exitmsg*2)
ldi zh,high(monitor_exitmsg*2)
rjmp monitor_exitmessage
;-----------------------------------------------------------------
;
; update_monitor_display_dest
; Takes zl = current state, rPARAM2 (high bits) = transition code
update_monitor_display_dest:
mov rPARAM1,rPARAM2
andi rPARAM1,$F0
cpi rPARAM1,$80
brne update_monitor_dd_non0
ldi rPARAM1,'-'
rjmp uart_writechar
update_monitor_dd_non0:
asr rPARAM1
asr rPARAM1
add rPARAM1,zl
rcall monitor_statetochar
rjmp uart_writechar
;-----------------------------------------------------------------
;
; monitor_ignoreline
; Ignore the rest of the line. Call monitor_ignoreline_charread if
; the first character of the rest of the line has already been read.
; Preserves rPARAM1
monitor_ignoreline_charread:
push rPARAM1
rjmp monitor_ignoreline_charread2
monitor_ignoreline:
push rPARAM1
monitor_ignoreline_loop:
rcall monitor_readchar
monitor_ignoreline_charread2:
cpi rPARAM1,$0D
breq monitor_ignoreline_exit
cpi rPARAM1,$0A
brne monitor_ignoreline_loop
monitor_ignoreline_exit:
pop rPARAM1
ret
;-----------------------------------------------------------------
;
; monitor_chartostate
; Convert incoming character in rPARAM1 to state pointer in rRETURN
monitor_chartostate:
cpi rPARAM1,'A'
brlt monitor_chartostate_2
andi rPARAM1,$FF-$20 ; assume caps
subi rPARAM1,7
monitor_chartostate_2:
subi rPARAM1,'0'
mov rRETURN,rPARAM1
lsl rRETURN
lsl rRETURN
ret
;-----------------------------------------------------------------
;
; monitor_statetochar
; Convert state pointer in rPARAM1 to character in rPARAM1
monitor_statetochar:
lsr rPARAM1
lsr rPARAM1
cpi rPARAM1,10
brlt monitor_statetochar_2
subi rPARAM1,-7
monitor_statetochar_2:
subi rPARAM1,-'0'
ret
;-----------------------------------------------------------------
;
; monitor_readchar
; Read character with echo back to host. Character is in BOTH rPARAM1
; and rRETURN. Skips spaces
monitor_readchar:
rcall uart_readcharwait
mov rPARAM1,rRETURN
rcall uart_writechar
cpi rPARAM1,' '
breq monitor_readchar
ret
;-----------------------------------------------------------------
;
; monitor_write_eeprom
; Write rPARAM1 to eeprom at Z
monitor_write_eeprom:
sbic EECR,EEWE ;if EEWE not clear
rjmp monitor_write_eeprom ; wait more
out EEAR,zl ; output address
out EEDR,rPARAM1 ; output data
cli ; critical section
sbi EECR,EEMWE ; set master write enable, remove if 1200 is used
sbi EECR,EEWE ; set EEPROM Write strobe
; This instruction takes 4 clock cycles since
; it halts the CPU for two clock cycles
sei ; end critical section
ret
;-----------------------------------------------------------------
;
; Constant Data
;-----------------------------------------------------------------
;
; Message
monitor_welcome:
.db "Monitor ready",$00
monitor_errormsg:
.db $0d,$0a,"Error at *",$00
monitor_exitmsg:
.db " OK",$00
;-----------------------------------------------------------------
Build Process
All of these files should be loaded into an AVR studio project, with ROBOT.ASM being the main target file.
The build process should complete with no errors or warnings, and is then ready to be uploaded into the 2313
using whatever programming hardware is appropriate. I use a simple dongle and PonyProg, although I have just taken delivery
of Atmel's STK500 starter kit which can also act as a serial ISP programmer so I will be looking at that in some
detail later on.
|
