Biltemas parkeringssensor som robotgivare

[niroma]

Denna tråd beskriver hur man modifierar en parkeringssensor ur Biltemas sortiment till en kollisionsgivare med 4 oberoende ultraljudsgivare, lämplig till diverse robotprojekt.

Upprinnelsen till denna guide var behovet av billiga kollisionsgivare till en gräsklipparrobot.

Parkeringssensorn jag utgått ifrån är artnr 43-142, som bl a innehåller 4 ultraljudsgivare och en utvärderingsenhet.


I korthet består konverteringen i att ersätta den befintliga mikrokontrollern i utvärderingsenheten med en PIC16F84A. Resultatet av modifieringen blir en kollisionsgivare med 4 oberoende sensorer som ger signal på varsitt stift i minidin-kontakten.

OBS!
Inget ansvar eller krav på ersättning e dyl kan ställas på författare eller detta forum för förverkad garanti, skador på egendom, djur eller personer p g a modifieringar gjorda enligt denna guide.

[niroma]

Koden som behövs för PIC16F84A är enl följande:

Kod: Markera allt

/*

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;
	}

}

[niroma]

Mikrokontrollern (U3) i utvärderingsenheten är en SONIX SN8P2602 vilken är pinkompatibel med PIC16F84A.


Programmet är skrivet i C med några tidskritiska avsnitt i inline assembler.
Kompilera med t ex SDCC.

Programmera PICen med t ex PICKIT2 och montera den försiktigt i sockeln för U3.
(Det kan vara klokt att även vara försiktig med SONIX-kontrollern ifall man ångrar sig eller behöver kontrollmäta på den.)

Programmet detekterar närvaro av objekt i intervallet ca 10-60cm framför sensorerna.
Varje sensor behandlas individuellt och ger en +5V-signal på "sin" pinne i kontakten.

Pinout för kontakten efter modifiering är enl. nedan:


Lycka till!
Kom gärna med synpunkter eller förslag på förbättringar.
---------------------------------------
niroma

[sp4m]

Trevligt projekt! Skall defenitivt lägga denna guide på minnet. Kan komma till pass när man behöver ett nytt projekt att sätta igång med.

[blueint]

Wikifierar den om ingen har något emot det..

Edit: fixat: Parkeringssensor ombyggd till ultraljudsdetektor (Biltema 43-142)

[Glenn]

Coolt, extra tufft att bara kunna byta ut MCU'n rakt av pinkompatibelt sådär

var den socklad också eller är det du som lödit loss den gamla och socklat ?

[blueint]

Vore intressant att veta om det finns fler objekt som man kan byta MCU på såpass rakt av.

[victor_passe]

På nexas nya fjärr kan man byta mot en pic12F675 eller dyl.
Vcc, Vss, pullup finns, resten går till olika funktioner

[dar303]

Mycket snyggt!
Om sensorn går att hitta i kina/på internet skulle man kunna översätta artikeln och lägga upp på instructables/makeblog/boingboing, den skulle nog få mycket uppmärksamhet bland internationella robotbyggare.

[niroma]

Roligt att projektet uppskattas!

MCUn var socklad från början så det behövdes ingen lödning.

Skulle vara kul om någon har tid/lust att förbättra programmet för detektering på korta avstånd. Som det funkar nu så detekteras inte objekt nära sensorerna vilket kan få t ex en gräsklipparrobbot att bli otrevligt närgången...
Behövs lite mera oscilloskop-mätningar och kod för detta men har tyvärr inte tid att labba med det nu på ett tag.

[007sweden]

PCB-004 V2.2 har tyvärr ingen sockel, utan MCU'n är lödd direkt på kortet.

[niroma]

Har nu gjort några mätningar på parkeringssensorn. Obs att jag bara har tillgång till versionen på bilden i början av tråden; PCB-004 V2.1. Förhoppningsvis är det inte mer än den lödda kontrollern som 007sweden hittade som skiljer mot senare versioner.

Mätningarna är gjorda inomhus med enbart en av ultraljudsgivarna, kanal C, ansluten. Ultraljudsgivaren är riktad med framsidan mot golvet och mätningarna är gjorda på de avstånd som är angivna i graferna. Det skall noteras att kurvformen från givaren, ekot, varierar i utseende beroende på infallsvinkel, typ av objekt mm så kurvorna är väl en sorts "idealfall". Gjorde två mätningar på varje höjd p g a att det ibland verkar som den tredje chirp-signalen får lite annorluna svar.
I vänsterkant av graferna kan tid mellan markörerna, dX och läget för den blå markören X avläsas. Användbart om man vill modofiera utvärderingen av ekot.

























Nedan följer några kurvor på chirp-signalen och hur SONIX-kontrollern arbetar: