| |
Hi,
I'm using the handyboard for a robotics competition, and I have been having
a problem when I use my compass and sonar together.
I am using a Daventech SRF04 ultrasonic ranger, and a Daventech compass.
Here is the problem: I can read the sonar using the library I have attached at
the end, and it all works fine until I start reading the compass. I can
continue reading the compass for as long as I want (and get accurate readings),
However the sonar routines no longer return when I call them. I use a
pulsewidth mesuring routine I found on an old handyboard mailing list to
measure the compass. I suspect an interrupt problem, but I don't know enought
assembly to know what is wrong.
Jeff
*************************
* SONAR LIBRARY V1.0 *
* December 29, 2001 *
* by Willem van Asperen *
* wasperen@hotmail.com *
*************************
ORG MAIN_START
* program equates
USERLATCH EQU $5000 ; defined in libexpbd.asm
USERVAL EQU $32 ; defined in libexpbd.asm
BASE EQU $1000
OUTBIT6SET EQU %01000000 ; expansion board digtal output bit 6
OUTBIT5SET EQU %00100000 ; expansion board digtal output bit 5
OUTBIT4SET EQU %00010000 ; expansion board digtal output bit 4
OUTBITSRES EQU %10001111 ; mask to reset both output bits
TCNT EQU $100E ; timer base count register
TCTL2 EQU $1021 ; timer control register 2
TMSK1 EQU $1022 ; timer interrupt mask register 1
TFLG1 EQU $1023 ; timer interrupt flag register 1
TMSK2 EQU $1024 ; timer interrupt mask register 2
TFLG2 EQU $1025 ; timer interrupt flag register 2
TIC3 EQU $1014 ; timer input capture value
TIC3INT EQU $BFEA ; Timer Input Capture 3 interrupt vector
TFLGIC3 EQU %00000001 ; timer 3 flags
IC3OFF EQU %11111100
IC3FALL EQU %00000010
IC3RISE EQU %00000001
DONE EQU $FFFF
tmp FDB 0
variable_sonar_distance0 FDB 0 ; latest measurements
variable_sonar_distance1 FDB 0
variable_sonar_distance2 FDB 0
variable_sonar_status FDB DONE
OUTBITS FCB OUTBIT4SET ; expansion board output bits
FCB OUTBIT5SET
FCB OUTBIT6SET
subroutine_initialize_module:
LDX #BASE
LDAA #OUTBITSRES ; make both output bits zero
ANDA USERVAL
STAA USERLATCH
STAA USERVAL
LDD #ping_return ; install interrupt vector
STD TIC3INT
BSET TCTL2,X #IC3FALL ; tell IC3 to respond to falling edge
BSET TMSK1,X #TFLGIC3 ; enable interrupt at IC3
RTS
*********************************
* subroutine _sonar_ping(in) *
* in : AB = 0 or 1 *
* out: AB = current timer value *
*********************************
subroutine__sonar_ping:
STD variable_sonar_status
LDX #OUTBITS ; point X to relevant bit patern for output
ABX
LDAA 0,X ; pulse high on out bit
ORAA USERVAL
STAA USERLATCH
STAA USERVAL
LDAA 0,X ; pulse low on out bit
COMA
ANDA USERVAL
STAA USERVAL
STAA USERLATCH
LDD TCNT ; store current time
STD tmp
RTS
ping_return:
LDD variable_sonar_status ; make X point to relevant result variable
ASLB
ADDD #variable_sonar_distance0
XGDX
LDD TIC3 ; calculate and store travel time
SUBD tmp
STD 0,X
LDAA #TFLGIC3 ; reset flag IC3
STAA TFLG1
LDD #DONE ; reset status
STD variable_sonar_status
RTI
/*end sonar lib*/
*****************************************************************
* Programm to measure a pulsewidth with the TIC2 port of
* the 68HC11
*
*
* |---------| |----------
* -------| |-----------|
*
* |<------->| pulsewidth
*
*
* Programm written for Pacificar-Project TWI, Switzerland
* by Thomas Hauri and Beat Buchmann
*
* Version 1.0
*
* last changes : 02 Okt 1997
*
* History : 06.03.97 : realizing testversion to test Handyboard
interrupt
* features
*
* : 02.10.97 : added and adjustetd some things to run with
* expansionboard
*
*
******************************************************************
* used registers
*****************
BASE EQU $1000
PORTA EQU $1000 * Port A data register
CFORC EQU $100B * Timer Compare Force Register
OC1M EQU $100C * Output Compare 1 Mask register
OC1D EQU $100D * Output Compare 1 Data register
TCNT EQU $100E * Timer Count Register
TIC2 EQU $1012 * Timer Input Capture register 2
TIC3 EQU $1014 * Timer Input Capture register 3
TOC3 EQU $101A * Timer Output Compare register 3
TCTL1 EQU $1020 * Timer Control register 1
TCTL2 EQU $1021 * Timer Control register 2
TMSK1 EQU $1022 * main Timer interrupt Mask register 1
TFLG1 EQU $1023 * main Timer interrupt Flag register 1
HPRIO EQU $103C * Highest Priority Interrupt and misc
TIC2INT EQU $EC * Timer Input Capture 2
TIC3INT EQU $EA * Timer Input Capture 3
TOC4INT EQU $E2 * Timer Input Capture 4
* addresses and masks
pos_edge_02 EQU %00000100 * Select positiv edge to detect
%0000xx00 on TIC2
neg_edge_02 EQU %00001000 * Select negativ edge to detect
%0000xx00 on TIC2
pos_edge_03 EQU %00000001 * Select positiv edge to detect
%000000xx on TIC3
neg_edge_03 EQU %00000010 * Select negativ edge on TIC3
tmsk1_flag_02 EQU %00000010 * using TIC2
tflg1_mask_02 EQU %11111101 * capture mask
tmsk1_mask_02 EQU %11111101 * used to disable TIC2 interrupt
tmsk1_flag_03 EQU %00000001 * using TIC3
tflg1_mask_03 EQU %11111110 * capture mask
tmsk1_mask_03 EQU %11111110 * used to disable TIC3 interrupt
ORG MAIN_START
* variables
************
local_counter FCB 0
temp FDB 0
run FCB 0
integrator FDB 0
variable_pulsewidth2 FDB 0
variable_pulse_done2 FDB 0
*************************************************************************
* initialization start
*************************************************************************
subroutine_initialize_module:
* include of ldxibase.asm
LDAA HPRIO
ANDA #$40 * test SMOD bit
BNE *+7
LDX #$FF00 * normal mode interrupts
BRA *+5
LDX #$BF00 * special mode interrupts
* Lets now install the interrupt routine into the TIC3 interrrupt
LDD TIC2INT,X
STD interrupt_code_exit+1
LDD #interrupt_code_start
STD TIC2INT,X
BCLR TFLG1-BASE,X tflg1_mask_02 * Clear TIC2 Bit in TFLG1
BCLR TMSK1-BASE,X tmsk1_flag_02 * Clear TIC2 Bit in TMSK1
RTS
*************************************************************************
* subroutine get_wavelength: starts measuring the wavelength
*
* inputs : must be 1
*
*************************************************************************
subroutine_get_pulsewidth2:
equal_one:
LDAA #1
STAA local_counter
go_on:
LDX #BASE
LDAA #pos_edge_02
STAA TCTL2-BASE,X * set hot_edge in Timer Control Register
LDD #0
STD integrator
STD variable_pulse_done2
LDAB #1
STAB run
LDD #$ffff * set pulsewidth to 65ms
STD variable_pulsewidth2
BCLR TFLG1-BASE,X tflg1_mask_02 * Clear TIC2 Bit in TFLG1
BSET TMSK1-BASE,X tmsk1_flag_02 * SET TIC2 Bit in TMSK1 (enable
interrupts)
RTS
*************************************************************************
* END measurement routine
*************************************************************************
*************************************************************************
* implementation of interrupt routine
*************************************************************************
interrupt_code_start:
LDAA run
BEQ interrupt_code_exit
CMPA #2
BHS second
first:
LDD TIC2 * take time at first rising edge of measurement
STD temp
INC run * set run -> next time interrupt is called
LDX #BASE
LDAA #neg_edge_02
STAA TCTL2-BASE,X * set hot_edge in Timer Control Register
BRA end * routine second will be called instead of
routine first
second:
LDD TIC2 * take time at following rising edge
SUBD temp * subtract times to get wavelength in 0.5 us
LSRD * divide by two to get the unit us and to avoid
* negative numbers from timer overflows
ADDD integrator * add length to integrator, which is used to
STD integrator * calculate the mean wavelength over 4
measurements
LDAB #1 * reset run
STAB run
end:
LDX #BASE
BCLR TFLG1-BASE,X tflg1_mask_02 * Clear TIC2 Bit in TFLG1
LDAA local_counter * check if end of measurement
BEQ stop_interrupt
DEC local_counter
BRA interrupt_code_exit
stop_interrupt:
BCLR TMSK1-BASE,X tmsk1_flag_02 * Clear TIC2 Bit in TMSK1(disable
interrupts)
CLR run
done:
LDD #1
STD variable_pulse_done2
LDD integrator
STD variable_pulsewidth2 * give back to Interactive C
interrupt_code_exit:
JMP $0000 * this is the end of the routine this value poked by
init routine
*************************************************************************
/*end pulsewidth lib*/
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
