added some pics

libraries/CodeRacer/CodeRacer.cpp

@@ -40,6 +40,16 @@ CodeRacer::CodeRacer()
 
 	coderracer_activ = false;
 	_coderracer_activ = true;
+	_drive_forward = false;
+	_schwenk = false;
+
+	_last_ledswitch_ms = millis();
+	_last_led_on = 0;
+	_led_count = 1;
+
+	_min_abstand_cm = 0;
+
+	_servo_look_around_ms = millis();
 	
 }
 
@@ -88,6 +98,15 @@ CodeRacer::CodeRacer(int 	tasterpin , int 	servopin,
 	coderracer_activ = false;
 	_coderracer_activ = true;
 	_drive_forward = false;
+	_schwenk = false;
+
+	_last_ledswitch_ms = millis();
+	_last_led_on = 0;
+	_led_count = 1;
+
+	_servo_look_around_ms = millis();
+
+	_min_abstand_cm = 0;
 
 }
 
@@ -132,6 +151,9 @@ void CodeRacer::begin() {
 	pinMode(_taster_pin, INPUT_PULLUP);
 	attachInterrupt(digitalPinToInterrupt(_taster_pin), set_taster_state, FALLING);
 
+	// Random
+	randomSeed(analogRead(0));
+
 	Serial.println("CodeRacer initialisiert ... ");
 }
 
@@ -166,7 +188,9 @@ void CodeRacer::analog_write(int pin, int speed) {
 
 void CodeRacer::anhalten() {
 	Serial.println("RACER_ANHALTEN");                            // Meldung am Monitor ausgeben
+	_schwenk = false;
 	_drive_forward = false;
+	_drive_normal_forward = false;
 	//Rechten Motor abschalten 
 	digitalWrite(_motor_rechts_frwd_pin, LOW);
 	digitalWrite(_motor_rechts_back_pin, LOW);
@@ -226,8 +250,41 @@ void CodeRacer::vorwaerts()
 			}
 		}
 	}
+}
 
+void CodeRacer::rueckwaerts()
+{
 
+	Serial.println("RACER_RUECKWAERTS");                            // Meldung am Monitor ausgeben
+
+	if (false == _drive_forward) {
+		// Rechten Motor auf rückwärts stellen 
+		digitalWrite(_motor_rechts_frwd_pin, LOW);
+		digitalWrite(_motor_rechts_back_pin, HIGH);
+		// Linken Motor auf rückwärts stellen 
+		digitalWrite(_motor_links_frwd_pin, LOW);
+		digitalWrite(_motor_links_back_pin, HIGH);
+		// Motoren beide anschalten 
+		analog_write(_motor_rechts_enable_pin, MOTORRE_MAX_TEMPO);
+		analog_write(_motor_links_enable_pin, MOTORLI_MAX_TEMPO);
+		// LEDs setzen
+		digitalWrite(_led_frwd_pin, HIGH);
+		digitalWrite(_led_stop_pin, LOW);
+		digitalWrite(_led_links_pin, LOW);
+		digitalWrite(_led_rechts_pin, LOW);
+
+		_drive_forward = true;
+		_drive_normal_forward = false;
+		_drive_forward_ms = millis();
+
+	}
+	else {
+		if (false == _drive_normal_forward) {
+			if ((millis() - _drive_forward_ms) > MOTOR_MAX_SPEED_START_MS) {
+				normal_tempo();
+			}
+		}
+	}
 }
 
 void CodeRacer::links()
@@ -283,20 +340,25 @@ void CodeRacer::rechts()
 void CodeRacer::servo_rechts()
 {
 	Serial.println("SERVO_RECHTS");																// Meldung am Monitor ausgeben
+	_schwenk = false;
 	_servo->write(_servo_rechts);																		// Servo auf den Winkel rechts drehen
+	_schwenk_position = _servo_rechts;															// Servo schaut nach rechts
 	delay(1000);																										// Kurz warten, dass der Servo die Stellung erreicht
 }
 
 void CodeRacer::servo_links()
 {
 	Serial.println("SERVO_LINKS");																	// Meldung am Monitor ausgeben
+	_schwenk = false;
 	_servo->write(_servo_links);																		// Servo auf den Winkel links drehen
+	_schwenk_position = _servo_links;																// Servo schaut nach links
 	delay(1000);																										// Kurz warten, dass der Servo die Stellung erreicht
 }
 
 void CodeRacer::servo_mitte()
 {
 	Serial.println("SERVO_MITTE");																	// Meldung am Monitor ausgeben
+	_schwenk = false;
 	_servo->write(_servo_mitte);																		// Servo auf den Winkel links drehen
 	_schwenk_position = _servo_mitte;																// Servo schaut direkt nach vorn
 	_schwenk_millis = millis();
@@ -305,6 +367,7 @@ void CodeRacer::servo_mitte()
 
 void CodeRacer::servo_schwenk()
 {
+	_schwenk = true;
 	if (millis() - _schwenk_millis > SERVO_SCHWENK_MS) {
 		_schwenk_position = _schwenk_position + _schwenk_richtung;
 		if (_schwenk_position >= _schwenk_links) {
@@ -320,14 +383,112 @@ void CodeRacer::servo_schwenk()
 	}
 }
 
+void CodeRacer::knight_rider()
+{
+	if (millis() - _last_ledswitch_ms > LED_SWITCH_MS) {
+		_last_ledswitch_ms = millis();
+		if (_last_led_on >= 5) {
+			_led_count = -1;
+		}
+		if(_last_led_on <= 0) {
+			_led_count = 1;
+		}
+		_last_led_on = _last_led_on + _led_count;
+		switch (_last_led_on) {
+		case 0:
+			digitalWrite(_led_links_pin, HIGH);
+			digitalWrite(_led_stop_pin, LOW);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, LOW);
+			break;
+		case 1:
+			digitalWrite(_led_links_pin, HIGH);
+			digitalWrite(_led_stop_pin, LOW);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, LOW);
+			break;
+		case 2:
+			digitalWrite(_led_links_pin, LOW);
+			digitalWrite(_led_stop_pin, HIGH);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, LOW);
+			break;
+		case 3:
+			digitalWrite(_led_links_pin, LOW);
+			digitalWrite(_led_stop_pin, LOW);
+			digitalWrite(_led_frwd_pin, HIGH);
+			digitalWrite(_led_rechts_pin, LOW);
+			break;
+		case 4:
+			digitalWrite(_led_links_pin, LOW);
+			digitalWrite(_led_stop_pin, LOW);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, HIGH);
+			break;
+		case 5:
+			digitalWrite(_led_links_pin, LOW);
+			digitalWrite(_led_stop_pin, LOW);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, HIGH);
+			break;
+		}
+	}
+}
+
 long CodeRacer::abstand_messen()
+{
+	long abstand_cm = 0;
+	long min_abstand_cm = 1000;
+	int servo_drehrichtung = 0;
+
+	//Serial.print("ABSTAND_MESSEN. Aktuelle Servo position:");
+	//Serial.println(_schwenk_position);
+	// wenn der Racer nicht fährt, dann wird mehrfach gemessen und der Servo um die aktuelle Positin gedreht ... wenn er faährt oder schwenk aktiv ist , wird einfach nur gemessen
+	if (((true == _drive_forward) || (true == _drive_normal_forward) || (H_SERVO_MITTE == _schwenk_position) || _schwenk == true)) {
+		// messen beim fahren ... 
+		//Serial.println("ABSTAND_MESSEN im fahren, direkt nach vorn oder Schwenk() ist aktiv ...");
+		min_abstand_cm = abstand_messen_intern();
+	}	else {
+		// messen im Stand und ohne schwenk ... 
+		//Serial.println("ABSTAND_MESSEN im Stand nach links oder rechts...");
+		// sind wir links oder rechts ?
+		if (H_SERVO_LINKS == _schwenk_position) {
+			//Serial.println("ABSTAND_MESSEN. Linke Seite.");
+			//links ...
+			servo_drehrichtung = SERVO_SCHWENK_RE;
+		}
+		if (H_SERVO_RECHTS == _schwenk_position) {
+			//rechts ...
+			//Serial.println("ABSTAND_MESSEN rechte Seite.");
+			servo_drehrichtung = SERVO_SCHWENK_LI;
+		}
+		// drehen und messen ... kleinsten Wert merken 
+		do {
+			_schwenk_position = _schwenk_position + servo_drehrichtung;
+			_servo->write(_schwenk_position);
+			//Serial.print("ABSTAND_MESSEN. Servo position:");
+			//Serial.println(_schwenk_position);
+			delay(50);
+			abstand_cm = abstand_messen_intern();
+			if (abstand_cm < min_abstand_cm) {
+				min_abstand_cm = abstand_cm;
+			}
+		} while ((_schwenk_position > H_SERVO_MITTE_LINKS) || (_schwenk_position < H_SERVO_MITTE_RECHTS));
+	}
+
+	Serial.print("ABSTAND_MESSEN. Der Abstand in cm ist (mindestens):");
+	Serial.println(min_abstand_cm);
+	_min_abstand_cm = min_abstand_cm;
+
+	return(min_abstand_cm);
+}
+
+long CodeRacer::abstand_messen_intern()
 {
 	long abstand_cm;
-	long single_echo_dauer_us;
-	long long echo_dauer_us;
+	long long echo_dauer_us[3] = { 0,0,0 };
 	int messnr = 0;
 
-	echo_dauer_us = 0;
 	do{
 		// Messung starten - ein kurzer Pulse "HIGH" wird zum TRIG pin des Ultraschallsensors geschickt
 		pinMode(_us_echo_pin, OUTPUT);
@@ -339,27 +500,39 @@ long CodeRacer::abstand_messen()
 		digitalWrite(_us_trigger_pin, LOW);
 
 		// Messung der Dauer in Mikrosekundenmeasure bis das ECHO Pin vom Ultraschallsensor HIGH wird 
-		single_echo_dauer_us = pulseIn(_us_echo_pin, HIGH, US_MAX_ECHO_TIME_US);
-		if (single_echo_dauer_us == 0) {
-			echo_dauer_us = echo_dauer_us + US_MAX_ECHO_TIME_US;
+		echo_dauer_us[messnr] = pulseIn(_us_echo_pin, HIGH, US_MAX_ECHO_TIME_US);
+		if (echo_dauer_us[messnr] == 0) {
+			echo_dauer_us[messnr] = US_MAX_ECHO_TIME_US;
 			messnr++;
 		}	else {
-			if (single_echo_dauer_us > 200) {
-				echo_dauer_us = echo_dauer_us + single_echo_dauer_us;
+			if (echo_dauer_us[messnr] > 200) {
 				messnr++;
 			}
 		}
-	} while (messnr < 1);
-	//echo_dauer_us = echo_dauer_us >> 4;
+	} while (messnr < 3);
+
+	// the largest and the smallest values will be ignored ...
+	if (echo_dauer_us[0] > echo_dauer_us[1])
+	{
+		std::swap(echo_dauer_us[0], echo_dauer_us[1]);
+	}
+	if (echo_dauer_us[1] > echo_dauer_us[2])
+	{
+		std::swap(echo_dauer_us[1],echo_dauer_us[2]);
+	}
+	if (echo_dauer_us[0] > echo_dauer_us[1])
+	{
+		std::swap(echo_dauer_us[0], echo_dauer_us[1]);
+	}
 	// convert into cm ... 344m/sec is the speed of noise - thus 34400cm/sec ... or 34,400cm/milisec ... or 0,0344cm/microsec
 	// the echo has to go the distance twice - forth and back - so the duration has to be the half of the measured one
 	// distance_cm = echo_duration/2 * 0,0344   or   distance_cm = echo_duration/2 / 29,1 or distance_cm = echo_duration * 0,0172
 	// distance_cm = (echo_duration/2) / 29.1;
-	abstand_cm = echo_dauer_us * 0.0172;
+	abstand_cm = echo_dauer_us[1] * 0.0172;
 	
 	//Messwert am Monitor anzeigen
-	Serial.print("ABSTAND_MESSEN. Der Abstand in cm ist (mindestens):");
-	Serial.println(abstand_cm);
+	//Serial.print("ABSTAND_MESSEN. Der Abstand in cm ist (mindestens):");
+	//Serial.println(abstand_cm);
 
 	return(abstand_cm);
 }
@@ -368,12 +541,35 @@ bool CodeRacer::start_stop() {
 	if (_coderracer_activ != coderracer_activ) {
 		_coderracer_activ = coderracer_activ;
 		if (true == _coderracer_activ) {
+			digitalWrite(_led_links_pin, LOW);
+			digitalWrite(_led_frwd_pin, LOW);
+			digitalWrite(_led_rechts_pin, LOW);
 			digitalWrite(_led_stop_pin, LOW);
+			servo_rechts();
+			servo_links();
+			servo_mitte();
+			delay(500);
 		}
 		else {
 			anhalten();
 			servo_mitte();
-			digitalWrite(_led_stop_pin, HIGH);
+			_servo_look_around_ms = millis() + random(5000, 120000);
+		}
+	}
+
+	if (false == _coderracer_activ) {
+		knight_rider();
+		if (_servo_look_around_ms < millis()) {
+			_servo_look_around_ms = millis() + random(5000, 120000);
+			servo_links();
+			delay(500);
+			servo_rechts();
+			delay(800);
+			servo_mitte();
+			delay(300);
+			servo_links();
+			delay(100);
+			servo_mitte();
 		}
 	}
 		

libraries/CodeRacer/CodeRacer.h

@@ -1,4 +1,5 @@
 #include "Arduino.h"
+#include <algorithm>    // std::swap
 #include <ESP32Servo.h>
 #include "esp32-hal-ledc.h"
 
@@ -9,10 +10,12 @@
 #define H_TASTERPIN 17
 //----- Servo -----
 #define H_SERVOPIN 16               // Pin an dem der Servomotor angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
-#define H_SERVO_LINKS 140    // Wert um den Servo 45 Grad nach links zu drehen ... der kann je nach Servo anders sein
-#define H_SERVO_RECHTS 35    // Wert um den Servo 45 Grad nach rechtss zu drehen ... der kann je nach Servo anders sein
-#define H_SERVO_MITTE 90      // Wert um den Servo in die Mitte zu drehen ... der kann je nach Servo anders sein
-#define H_SERVO_SCHWENK_LINKS 135
+#define H_SERVO_LINKS 145						// Wert um den Servo 45 Grad nach links zu drehen ... der kann je nach Servo anders sein
+#define H_SERVO_MITTE_LINKS 100			// Bis hier geht die Entfernungsmessung im Stand links
+#define H_SERVO_RECHTS 35						// Wert um den Servo 45 Grad nach rechtss zu drehen ... der kann je nach Servo anders sein
+#define H_SERVO_MITTE_RECHTS 80			// Bis hier geht die Entfernungsmessung im Stand rechts
+#define H_SERVO_MITTE 90						// Wert um den Servo in die Mitte zu drehen ... der kann je nach Servo anders sein
+#define H_SERVO_SCHWENK_LINKS 140
 #define H_SERVO_SCHWENK_RECHTS 40
 
 #define SERVO_SCHWENK_LI 5
@@ -26,8 +29,8 @@
 #define US_MAX_ECHO_TIME_US 6000		// Wenn die Zeit für das Echo länger dauert als diese Zeit, ist der Racer weit genug weg und wir müssen nicht weiter warten. 
 
 //----- Motoren -----
-#define H_MOTORRE_TEMPO 205         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
-#define H_MOTORLI_TEMPO 200         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
+#define H_MOTORRE_TEMPO 215         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
+#define H_MOTORLI_TEMPO 212         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
 #define H_MOTORRE_SPEED 2           // Pin an dem der SPEED/ENABLE Pin des rechten Motors angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
 #define H_MOTORRE_FWRD 4            // Pin an dem der FORWÄRTS Pin des rechten Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
 #define H_MOTORRE_BACK 15           // Pin an dem der RÜCKWÄRTS Pin des rechten Motors angeschlossen ist. Was vorwärts und rückwärts ist, muss probiert und vielleicht umgesteckt werden.'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
@@ -39,7 +42,7 @@
 
 #define MOTORPWM_LINKS 5          // PWM-Kanal für linken Motor
 #define MOTORPWM_RECHTS 6         // PWM-Kanal für rechten Motor
-#define MOTORRE_MAX_TEMPO 240         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
+#define MOTORRE_MAX_TEMPO 244         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
 #define MOTORLI_MAX_TEMPO 240         // Geschwindigkeit Motor1 ... ein Wert zwischen 0 und 255
 
 #define MOTOR_MAX_SPEED_START_MS 10
@@ -47,8 +50,10 @@
 //----- Werte für die LEDs -----
 #define H_LED_VORWAERTS 26          // Pin an dem die VORWÄRTS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
 #define H_LED_STOP 25               // Pin an dem die STOP LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
-#define H_LED_LINKS 27              // Pin an dem die LINKS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
-#define H_LED_RECHTS 33             // Pin an dem die RECHTS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
+#define H_LED_LINKS 33              // Pin an dem die LINKS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
+#define H_LED_RECHTS 27             // Pin an dem die RECHTS LED angeschlossen ist. 'GPIO' muss man weglassen -> also z.B. nicht GPIO88 sondern nur 88.
+
+#define LED_SWITCH_MS 50
 
 static volatile bool coderracer_activ;
 static volatile unsigned long taster_last_pressed_ms;
@@ -90,6 +95,9 @@ class CodeRacer {
 		bool _drive_forward;
 		long _drive_forward_ms;
 		bool _drive_normal_forward;
+		bool _schwenk;
+
+		long _min_abstand_cm;
 
 
 		static void set_taster_state();
@@ -100,6 +108,14 @@ class CodeRacer {
 		void analog_write(int pin, int speed);
 		int _coderracer_activ;
 
+		long abstand_messen_intern();
+
+		long _last_ledswitch_ms;
+		int  _led_count;
+		int  _last_led_on;
+
+		long _servo_look_around_ms;
+
 	public:
 
 
@@ -121,6 +137,7 @@ class CodeRacer {
 		void anhalten();
 		void normal_tempo();
 		void vorwaerts();
+		void rueckwaerts();
 		void links();
 		void rechts();
 		void servo_rechts();
@@ -128,7 +145,7 @@ class CodeRacer {
 		void servo_mitte();
 		long abstand_messen();
 		void servo_schwenk();
-
+		void knight_rider();
 		bool start_stop();
 
 };