## c't-Projekte - Mailinglisten

[Voriger (Datum)] [Nächster (Datum)] [Voriger (Thread)] [Nächster (Thread)]
[Nach Datum][Nach Thread]

### [ct-bot] Noch'n Sensorpatch zum Testen

Absender: Michail Brzitwa
Datum: Mi, 22.03.2006 13:40:15

    Hallo,

    anbei ein experimenteller Patch, der eine einheitliche Skalierung der
    meisten Sensorenwerte, eine definierbare Abruffrequenz und eine
    laufende Mittelwertbildung für die Abstandssensoren einführt. Der
    Maussensor ist von dieser Behandlung ausgeschlossen, der läuft wie
    bisher.

    Im Header sensor_correction.h werden für die Sensoren BORDER, DISTANCE,
    LIGHT und LINE linke/rechte Maximum/Minimumwerte definiert, die laut
    beschriebener Meßmethode individuell ermittelt werden können (bei mir
    liefern z.B. die Kantensensoren links/rechts ziemliche Unterschiede).

    Im selben Header stehen auch die skalierten Maximalwerte (momentan
    96, sollte durch 2^5 teilbar sein und achtbittig bleiben), und die
    Häufigkeit des Sensorakquisition. Bei mir dauert ein Hauptschleifen-
    durchgang in ct-Bot.c etwa 3ms, daher lohnt es sich z.B. nur alle
    10 Läufe von bot_sens_isr(), die Abstandssensorwerte zu holen.

    Die Abstandswerte werden auf positive Steigung gesetzt (kleiner
    Abstand->kleine Werte), allerdings habe ich die Funktion sensor_abstand()
    gelöscht, da wir zum einen niemals gesichert mm-Auflösung haben
    können (wie schon Harald Härtl in seiner sensor_abstand()-Änderung
    treffend bemerkte), zum anderen als Angleichung an englische Namen
    durch eine Funktion sensor_conv_to_metric() ersetzt, die momentan
    allerdings nichts ändert.

    Die Umsetzung von Fabian Recktenwald eines laufenden Mittelwerts der
    letzten n Distanzwerte habe ich auch eingebaut, allerdings nur der
    letzten 4 (statt 8) Werte, da im Patch nur bestenfalls alle ca. 30ms
    gepollt wird.

    Meinen Tests nach ist ein solcher Mittelwert etwas besser als die
    Lösung von Harald Härtl eines gleitenden Mittelwertes, allerdings
    nur für sich etwas schneller bewegende Objekte (Bot oder Hinderniss).
    Wir sprechen hier aber über subjektive Wahrnehmungen, ich kann
    kein exaktes Aufbau+Meßbeschreibungsszenario liefern, in dem das
    bessere Verhalten an Werten ablesbar ist.

    Da die oben genannten Sensorwerte nun alle zwischen [0,96] liegen
    (bei zu geringen Maximalwerten natürlich auch drüber, macht aber nix),
    ändert der Patch auch die Zonenabstände und die BORDER_DANGEROUS-
    Konstante in bot-local.h.

    Ich habe keine Ahnung, was im c't-Sim geändert werden müsste, damit
    das zusammenläuft (vielleicht irgendwas in CtBotSim.java oder
    CtBot.java). Gruß,
    --
    Michail Brzitwa           <michail@xxxxxxxxxx>            +49-511-343215

    Index: include/bot-local.h
    ===================================================================
    RCS file: /ctbot/ct-Bot/include/bot-local.h,v
    retrieving revision 1.4
    diff -u -r1.4 bot-local.h
    --- include/bot-local.h	17 Mar 2006 17:09:24 -0000	1.4
    +++ include/bot-local.h	22 Mar 2006 12:43:07 -0000
    @@ -35,11 +35,11 @@
     #define WHEEL_TO_WHEEL_DIAMETER 97 /*!< Abstand der beiden Raeder in mm */

     /* Einstellunge fuer die Verhaltensregeln */
    -#define BORDER_DANGEROUS	0x360	/*!< Wert, ab dem wir sicher sind, dass es eine Kante ist */
    +#define BORDER_DANGEROUS	90	/*!< Wert, ab dem wir sicher sind, dass es eine Kante ist */

    -#define COL_CLOSEST		100		/*!< Abstand in mm, den wir als zu nah betrachten -- je nach echtem Sensor ist das schon zu nah! */
    -#define COL_NEAR			300		/*!< Nahbereich */
    -#define COL_FAR			400		/*!< Fernbereich */
    +#define COL_CLOSEST			10		/*!< Abstand in mm, den wir als zu nah betrachten -- je nach echtem Sensor ist das schon zu nah! */
    +#define COL_NEAR			50		/*!< Nahbereich */
    +#define COL_FAR				80		/*!< Fernbereich */

     #define ZONE_CLOSEST	0			/*!< Zone fuer extremen Nahbereich */
     #define ZONE_NEAR		1			/*!< Zone fuer Nahbereich */
    Index: include/sensor_correction.h
    ===================================================================
    RCS file: /ctbot/ct-Bot/include/sensor_correction.h,v
    retrieving revision 1.1
    diff -u -r1.1 sensor_correction.h
    --- include/sensor_correction.h	4 Mar 2006 14:54:23 -0000	1.1
    +++ include/sensor_correction.h	22 Mar 2006 12:43:10 -0000
    @@ -25,8 +25,86 @@
     #ifndef SENSOR_CORRECTION_H_
     #define SENSOR_CORRECTION_H_

    -#define SENSDISTSLOPE	  65330		/*!< Steigung der Sensorkennlinie */
    -#define SENSDISTOFFSET		-3		/*!< Offset der Sensorkennlinie */
    +/*!
    + * Ist folgendes Symbol definiert, werden Sensorenwerte nicht skaliert.
    + */
    +
    +// #define SENS_DONT_SCALE
    +
    +
    +/*!
    + * Werteumfang Kantensensoren links/rechts:
    + * Meßmethode Maximum: c't-Bot in dunklem Raum mehr als einen Meter über
    + *                     den Boden halten.
    + * Meßmethode Minimum: c't-Bot in hellem Raum auf weisses Papier stellen
    + *                     und nach vorn kippen.
    + */
    +
    +#define SENS_BORDER_LEFT_MIN		0x0012
    +#define SENS_BORDER_LEFT_MAX		0x03FF
    +#define SENS_BORDER_RIGHT_MIN		0x0012
    +#define SENS_BORDER_RIGHT_MAX		0x03AF
    +
    +/*!
    + * Werteumfang Distanzsensoren links/rechts
    + * Meßmethode Maximum: c't-Bot in hellem Raum auf normale Oberfläche
    + *                     stellen und helles Objekt solange den Sensoren
    + *                     nähern, bis Werte wieder fallen.
    + * Meßmethode Minimum: c't-Bot in dunklem Raum mehr als einen Meter von
    + *                     der nächsten Wand halten.
    + *
    + */
    +
    +#define SENS_DISTANCE_LEFT_MIN		0x0003
    +#define SENS_DISTANCE_LEFT_MAX		0x0230
    +#define SENS_DISTANCE_RIGHT_MIN		0x0003
    +#define SENS_DISTANCE_RIGHT_MAX		0x0230
    +
    +/*!
    + * Werteumfang Lichtsensoren links/rechts
    + * Meßmethode Maximum: Lampe direkt auf Sensor richten.
    + * Meßmethode Minimum: Sensor mit dunklem Objekt abdecken.
    + */
    +
    +#define SENS_LIGHT_LEFT_MIN			0x0000
    +#define SENS_LIGHT_LEFT_MAX			0x0360
    +#define SENS_LIGHT_RIGHT_MIN		0x0000
    +#define SENS_LIGHT_RIGHT_MAX		0x0360
    +
    +/*!
    + * Werteumfang Liniensensoren links/rechts
    + * Meßmethode Maximum: c't-Bot in hellem Raum auf rauhes, schwarzes
    + *                     Objekt stellen (z.B. DVD-Hülle innen).
    + * Meßmethode Minimum: c't-Bot in hellem Raum auf weisses Papier
    + *                     stellen
    + */
    +
    +#define SENS_LINE_LEFT_MIN			0x0010
    +#define SENS_LINE_LEFT_MAX			0x03FF
    +#define SENS_LINE_RIGHT_MIN			0x0010
    +#define SENS_LINE_RIGHT_MAX			0x03FF
    +
    +/*!
    + * Obere Grenze der skalierten Sensorenwerte (unter Grenze ist 0)
    + * Werte müssen durch 32 (2^5, siehe sensor_scale()) teilbar sein
    + */
    +
    +#define SENS_BORDER_MAX				(32*3)
    +#define SENS_DISTANCE_MAX			(32*3)
    +#define SENS_LIGHT_MAX				(32*3)
    +#define SENS_LINE_MAX				(32*3)
    +
    +
    +/*!
    + * Acquisitionshäufigkeit der Sensoren (alle n Läufe der Routine
    + * bot_sens_isr()).
    + */
    +
    +#define SENS_BORDER_ACQFREQ			2
    +#define SENS_DISTANCE_ACQFREQ		10
    +#define SENS_LIGHT_ACQFREQ			5
    +#define SENS_LINE_ACQFREQ			2
    +

     #endif /*SENSOR_CORRECTION_H_*/
    Index: mcu/sensor-low.c
    ===================================================================
    RCS file: /ctbot/ct-Bot/mcu/sensor-low.c,v
    retrieving revision 1.5
    diff -u -r1.5 sensor-low.c
    --- mcu/sensor-low.c	19 Mar 2006 20:42:51 -0000	1.5
    +++ mcu/sensor-low.c	22 Mar 2006 12:43:11 -0000
    @@ -105,6 +105,7 @@
     	timer_2_init();
     }

    +
     /*!
      * Konvertiert einen Sensorwert vom analogen Abstandssensor in mm
      * Da die Sensoren eine nichtlineare Kennlinie haben,
    @@ -113,13 +114,31 @@
      * @param sensor_data Die Daten vom analogen Sensor
      * @return Distanz in mm
      */
    -int16 sensor_abstand(int16 sensor_data){
    -	// TODO reale Kennlinie beachten!!!!
    -	return SENSDISTSLOPE / (sensor_data - SENSDISTOFFSET);
    +int16 sensor_conv_to_metric(int16 sensor_data){
    +	return (sensor_data);
    +}
    +
    +
    +
    +/*!
    + * Skaliert einen Sensorwert vom Eingabeintervall [inscale_min,inscale_max]
    + * auf das Ausgabeinterval [0,outscale_max]. Ist invert != 0, wird das Resultat
    + * bezüglich outscale_max invertiert.
    + */
    +
    +int16_t sensor_scale(int16 inval,int16 inscale_min,int16 inscale_max,int16 outscale_max,int8 invert){
    +#ifndef SENS_DONT_SCALE
    +	int16	tmp = (inval - inscale_min) << 5;

    -//	return 1023-sensor_data;
    +	tmp /= inscale_max - inscale_min;
    +	tmp *= outscale_max >> 5;
    +	return (invert ? outscale_max - tmp : tmp);
    +#else
    +		return (inval);
    +#endif
     }

    +
     /*!
      * Alle Sensoren aktualisieren
      * Derzeit pollt diese Routine alle Sensoren. Insbesondere bei den
    @@ -132,31 +151,66 @@
      * zu aktualiseren, der Bot braucht auch Zeit zum nachdenken ueber Verhalten
      */
     void bot_sens_isr(void){
    +	static uint16 nRuns = 0;
    +
    +	// Berechnung des ständigen Mittelwerts der letzten 4 Messungen:
    +	// Arrays mit Werten des linken/rechten Sensors
    +	static int16 SDAL[4] = {0,0,0,0}, SDAsumL = 0;
    +	static int16 SDAR[4] = {0,0,0,0}, SDAsumR = 0;
    +	static int8 SDAp = 0;
    +
    +	++nRuns;
    +
     	ENA_on(ENA_KANTLED|ENA_MAUS|ENA_SCHRANKE|ENA_KLAPPLED);

      	// Aktualisiere die Position des Maussensors
     	sensMouseDY = maus_sens_read(MOUSE_DELTA_Y_REG);
    -
     	sensMouseDX = maus_sens_read(MOUSE_DELTA_X_REG);

    -	// ---------- analoge Sensoren -------------------
    -	sensLDRL = adc_read(SENS_LDR_L);
    -	sensLDRR = adc_read(SENS_LDR_R);

    -	sensBorderL = adc_read(SENS_KANTE_L);
    -	sensBorderR = adc_read(SENS_KANTE_R);
    -	ENA_off(ENA_KANTLED);
    -
    -	sensLineL = adc_read(SENS_M_L);
    -	sensLineR = adc_read(SENS_M_R);
    +	// Einige Sensoren nur jeden n-ten Aufruf lesen.
    +	// Analoge Sensoren lesen und skalieren: Kante links/rechts
    +	if ((nRuns % SENS_BORDER_ACQFREQ) == 0)
    +	{
    +		sensBorderL = sensor_scale(adc_read(SENS_KANTE_L),SENS_BORDER_LEFT_MIN,SENS_BORDER_LEFT_MAX,SENS_BORDER_MAX,0);
    +		sensBorderR = sensor_scale(adc_read(SENS_KANTE_R),SENS_BORDER_RIGHT_MIN,SENS_BORDER_RIGHT_MAX,SENS_BORDER_MAX,0);
    +	}
    +
    +	// Analoge Sensoren lesen und skalieren: Licht links/rechts
    +	if ((nRuns % SENS_LIGHT_ACQFREQ) == 0)
    +	{
    +		sensLDRL = sensor_scale(adc_read(SENS_LDR_L),SENS_LIGHT_LEFT_MIN,SENS_LIGHT_LEFT_MAX,SENS_LIGHT_MAX,0);
    +		sensLDRR = sensor_scale(adc_read(SENS_LDR_R),SENS_LIGHT_RIGHT_MIN,SENS_LIGHT_RIGHT_MAX,SENS_LIGHT_MAX,0);
    +	}
    +
    +	// Analoge Sensoren lesen und skalieren: Linie links/rechts
    +	if ((nRuns % SENS_LINE_ACQFREQ) == 0)
    +	{
    +		sensLineL = sensor_scale(adc_read(SENS_M_L),SENS_LINE_LEFT_MIN,SENS_LINE_LEFT_MAX,SENS_LINE_MAX,0);
    +		sensLineR = sensor_scale(adc_read(SENS_M_R),SENS_LINE_RIGHT_MIN,SENS_LINE_RIGHT_MAX,SENS_LINE_MAX,0);
    +	}
    +
     	ENA_off(ENA_MAUS);
    +	ENA_off(ENA_KANTLED);
    +
    +	// Analoge Sensoren lesen und skalieren: Abstand links/rechts
    +	if ((nRuns % SENS_DISTANCE_ACQFREQ) == 0)
    +	{
    +		if (++SDAp == 4)
    +			SDAp = 0;
    +		SDAsumL -= SDAL[SDAp];
    +		SDAsumR -= SDAR[SDAp];
    +
    +		SDAL[SDAp] = sensor_scale(adc_read(SENS_ABST_L),SENS_DISTANCE_LEFT_MIN,SENS_DISTANCE_LEFT_MAX,SENS_DISTANCE_MAX,1);
    +		SDAR[SDAp] = sensor_scale(adc_read(SENS_ABST_R),SENS_DISTANCE_RIGHT_MIN,SENS_DISTANCE_RIGHT_MAX,SENS_DISTANCE_MAX,1);
    +
    +		SDAsumL += SDAL[SDAp];
    +		SDAsumR += SDAR[SDAp];
    +
    +		sensDistL = sensor_conv_to_metric(SDAsumL >> 2);
    +		sensDistR = sensor_conv_to_metric(SDAsumR >> 2);
    +	}

    -	//Aktualisiere Distanz-Sensoren
    -	// Die Distanzsensoren sind im Normalfall an, da sie 50 ms zum booten brauchen
    -//	sensDistL= adc_read(SENS_ABST_L);
    -//	sensDistR= adc_read(SENS_ABST_R);
    -	sensDistL= sensor_abstand(adc_read(SENS_ABST_L));
    -	sensDistR= sensor_abstand(adc_read(SENS_ABST_R));

     	// ------- digitale Sensoren ------------------
     	sensDoor = (SENS_DOOR_PINR >> SENS_DOOR) & 0x01;

- Antworten:
  - Re: [ct-bot] Noch'n Sensorpatch zum Testen
    - From: Benjamin Benz

- Voriger (Datum): Re: [ct-bot] Anzeige und Selektion der Verhalten im Display, Michail Brzitwa
- Nächster (Datum): Re: [ct-bot] Verfeinerung des c't-Sim, Peter König
- Voriger (Thread): Re: AW: [ct-bot] Verfeinerung des c't-Sim, Peter König
- Nächster (Thread): Re: [ct-bot] Noch'n Sensorpatch zum Testen, Benjamin Benz
- Indizes:
  - Datum
  - Thread