Parkeringssensor ombyggd till ultraljudsdetektor (Biltema 43-142)
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.
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.