heise online · c't · iX · Technology Review · Telepolis · mobil · Security · Netze · heise open · heise resale · Autos · c't-TV · Jobs · Kiosk
Zum Inhalt
c't

c't Projekte - c't-Bot und c't-Sim - Mailinglisten

c't-Bot und c't-Sim


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


Copyright © 2007 Heise Zeitschriften Verlag Kritik, Anregungen bitte an c't-WWW Datenschutzhinweis   Impressum