Parkeringssensor ombyggd till ultraljudsdetektor (Biltema 43-142)
Hoppa till navigering
Hoppa till sök
Den utskrivbara versionen stöds inte längre och kanske innehåller renderingsfel. Uppdatera din webbläsares bokmärken och använd standardutskriftsfunktionen istället.
Hur man modifierar Biltemas parkeringssensor 43-142 (2009-10-12) till en kollisionsgivare med 4 oberoende ultraljudsgivare och utvärderingsenhet. Lämplig till diverse robotprojekt. Upprinnelsen till denna guide var behovet av billiga kollisionsgivare till en gräsklipparrobot.
Genom att ersätta den befintliga mikrokontrollern SONIX SN8P2602 (U3) i utvärderingsenheten med en PIC16F84A som är pinkompatibel. Resultatet blir en kollisionsgivare med 4 oberoende sensorer som ger signal på varsitt stift i mini-DIN kontakten.
Pinout efter modifiering:
Programmet detekterar närvaro av objekt i intervallet ca 10 - 60 cm framför sensorerna. Varje sensor behandlas individuellt och ger en +5V-signal på "sin" pinne i kontakten.
C-kod med några tidskritiska avsnitt i inline assembler. Kompilera med t ex SDCC.
/*
ultrasens_v1.c
090420 /niroma
Processor:PIC16F84A
Kristall: 4MHz (bör funka med befintlig)
Kompilator: SDCC : avr/pic16/pic14/xa51 2.8.0 #5117 (Jun 2 2008) (UNIX)
Syfte: Modifiera Biltemas parkeringssensor så att den får fyra utgångar, en för
resp sensor, som ger signal när det finns föremål närmare än ca 60cm.
-------------------------------------------------------------------------------------------------------
090420
Signal Riktning Funktion
CHIRP_A ut Sänder signal till transmitter
CHIRP_B ut
CHIRP_C ut
CHIRP_D ut
EKO in Tar emot filtrerat signaleko
DMUX_A ut Val av sensor bit 0
DMUX_B ut " 1
DETECT_A ut Signal till överordnat system vid detekterat föremål
DETECT_B ut
DETECT_C ut
DETECT_D ut
------------------
Summa 11
Mätvärden parkeringssensor i orginalutförande
Ant pulser per chirp = 20
Periodtid pulser = 25us
Tid kanalval till chirp = 15.2ms
Tid chirp till chirp = 18.4ms
Tid sista chirp till kanalval = 12.8ms
Funktion: Först väljs sensor med DMUX_A och B och därefter skickas chirpen på motsvarande
utgång CHIRP_A-D. Chripen består av 20 pulser med frekvens drygt 38kHz.
Eko-signalen filtreras av ett analogt filter och går in på EKO.
-------------------------------------------------------------------------------------------------------
091011
Saker att förbättra:
* Lägg till watchdog
* Förbättra detektering på korta avstånd (< 10cm)
-------------------------------------------------------------------------------------------------------
*/
#include <pic16f84a.h>
code unsigned int at 0x2007 config =_CP_OFF & _PWRTE_OFF & _WDT_OFF & _HS_OSC;
//Div definitioner
#define ALARM RB0 //pin 6
#define CHIRP_A RB1 //pin 7
#define CHIRP_B RB2 //pin 8
#define CHIRP_C RB3 //pin 9
#define CHIRP_D RB4 //pin 10
#define DMUX_B RB5 //pin 11
#define DMUX_A RB6 //pin 12
#define EKO RB7 //pin 13
#define DETECT_A RA0 //pin 17
#define DETECT_B RA1 //pin 18
#define DETECT_C RA2 //pin 1
#define DETECT_D RA3 //pin 2
//Globala variabler
unsigned char cLoopCnt, cCounterA, cCounterB;
unsigned char cTimePos, cTimeNeg;
unsigned int iAvrTimePos, iAvrTimeNeg;
void chirpA(void)
{
cLoopCnt = 20; // 20 pulser
TMR0 = 0; // Nollar för tidmätning
T0IF = 0; // Nollar för timeout vid tidmätningen
_asm
BANKSEL _PORTB_bits
chirpAloop:
BSF _PORTB_bits,1 // 1:a
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
BCF _PORTB_bits,1 // 0:a
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
NOP
DECFSZ _cLoopCnt,F
GOTO chirpAloop
_endasm;
}
void chirpB(void)
{
cLoopCnt = 20;
TMR0 = 0;
T0IF = 0;
_asm
BANKSEL _PORTB_bits
chirpBloop:
BSF _PORTB_bits,2
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
BCF _PORTB_bits,2
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
NOP
DECFSZ _cLoopCnt,F
GOTO chirpBloop
_endasm;
}
void chirpC(void)
{
cLoopCnt = 20;
TMR0 = 0;
T0IF = 0;
_asm
BANKSEL _PORTB_bits
chirpCloop:
BSF _PORTB_bits,3
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
BCF _PORTB_bits,3
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
NOP
DECFSZ _cLoopCnt,F
GOTO chirpCloop
_endasm;
}
void chirpD(void)
{
cLoopCnt = 20;
TMR0 = 0;
T0IF = 0;
_asm
BANKSEL _PORTB_bits
chirpDloop:
BSF _PORTB_bits,4
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
BCF _PORTB_bits,4
GOTO $+1
GOTO $+1
GOTO $+1
GOTO $+1
NOP
DECFSZ _cLoopCnt,F
GOTO chirpDloop
_endasm;
}
void measureTimes(void)
{// Mäter till pos och neg flank. Adderar in värdena i medelvärdesvariablerna. Väntar till timeout.
cTimePos = 0; //Sätter värden som gör att signal inte ges vid
cTimeNeg = 255; //uteblivet eller felaktigt eko.
while(!T0IF){
if(EKO){
cTimePos = TMR0;
break;
}
}
while(!T0IF){
if(!EKO){
cTimeNeg = TMR0;
break;
}
}
iAvrTimePos += cTimePos; //Adderar mätvärdena till medelvärdessumma
iAvrTimeNeg += cTimeNeg;
while(!T0IF); //Väntar ut mättiden
}
unsigned char evaluate(void)
{//Utvärderar medelvärdessumman. (Anv summan för att slippa en division i kontrollern.)
if(iAvrTimePos>(3*2500/64)) //Ligger den positiva flanken inom 2.5ms?
return 1;
if(iAvrTimeNeg<(3*3700/64)) //Ligger den negativa flanken inom 3700us (~60cm)?
return 1;
return 0;
}
void wait(void)
{ //From PicLoops 2.2
_asm
;PIC Time Delay = 0.01500200 s with Osc = 4000000 Hz
movlw D'20'
movwf _cCounterB
movlw D'121'
movwf _cCounterA
waitloop: decfsz _cCounterA,1
goto waitloop
decfsz _cCounterB,1
goto waitloop
_endasm;
}
void main(void)
{
//Initiering***************************************
//I/O
PORTA = 0;
TRISA = 0xF0; //1111 0000
PORTB = 0;
TRISB = 0x80; //1000 0000
NOT_RBPU = 1; //ej pull-ups
//TMR0
T0CS = 0; // Klocka TMR0 med fClk/4
PSA = 0; // Prescaler till TMR0
PS2 = 1; // Sätter prescaler till 1:64
PS1 = 0; // Ger drygt 16ms mättid med upplösningen 64us @ fClk=4MHz
PS0 = 1; // 64us motsvarar (teoretiskt) en sträcka på ca 11mm
//Huvudloop****************************************
while(1){
//Sensor A
DMUX_A = 0;
DMUX_B = 0;
wait(); //15ms
iAvrTimePos = iAvrTimeNeg = 0;
chirpA(); //Nollar TMR0 och skickar chirp.
measureTimes(); //Tid till pos & neg flank
chirpA();
measureTimes();
chirpA();
measureTimes();
if(evaluate())
DETECT_A = 1;
else
DETECT_A = 0;
//Sensor B
DMUX_A = 0;
DMUX_B = 1;
wait();
iAvrTimePos = iAvrTimeNeg = 0;
chirpB();
measureTimes();
chirpB();
measureTimes();
chirpB();
measureTimes();
if(evaluate())
DETECT_B = 1;
else
DETECT_B = 0;
//Sensor C
DMUX_A = 1;
DMUX_B = 1;
wait();
iAvrTimePos = iAvrTimeNeg = 0;
chirpC();
measureTimes();
chirpC();
measureTimes();
chirpC();
measureTimes();
if(evaluate())
DETECT_C = 1;
else
DETECT_C = 0;
//Sensor D
DMUX_A = 1;
DMUX_B = 0;
wait();
iAvrTimePos = iAvrTimeNeg = 0;
chirpD();
measureTimes();
chirpD();
measureTimes();
chirpD();
measureTimes();
if(evaluate())
DETECT_D = 1;
else
DETECT_D = 0;
}
}
Programmera PICen med t.ex PICKIT2.