library/Arduino/libraries/CodeRacer/CodeRacer.cpp

// the compiler switch for an ESP8266 is looking like this: #elif defined(ARDUINO_ARCH_ESP8266)

#include "CodeRacer.h"

//Konstruktor ohne Pins ... alle Vorgaben kommen aus dem CodeRacer.h Header file 
CodeRacer::CodeRacer()
{
	_taster_pin = H_TASTERPIN;
	_servopin = H_SERVOPIN;
	_us_trigger_pin = H_US_TRIG;
	_us_echo_pin = H_US_ECHO;
	_motor_links_frwd_pin = H_MOTORLI_FWRD;
	_motor_links_back_pin = H_MOTORLI_BACK;
	_motor_links_enable_pin = H_MOTORLI_SPEED;
	_motor_rechts_frwd_pin = H_MOTORRE_FWRD;
	_motor_rechts_back_pin = H_MOTORRE_BACK;
	_motor_rechts_enable_pin = H_MOTORRE_SPEED;
	_led_frwd_pin = H_LED_VORWAERTS;
	_led_stop_pin = H_LED_STOP;
	_led_links_pin = H_LED_LINKS;
	_led_rechts_pin = H_LED_RECHTS;

	_servo_dummy = new Servo();
	_servo = new Servo();

	_servo_mitte = H_SERVO_MITTE;
	_servo_links = H_SERVO_LINKS;
	_servo_rechts = H_SERVO_RECHTS;
	_schwenk_links = H_SERVO_SCHWENK_LINKS;
	_schwenk_rechts = H_SERVO_SCHWENK_RECHTS;

	_schwenk_position = _servo_mitte;
	_schwenk_richtung = SERVO_SCHWENK_LI;
	_schwenk_millis = millis();

	_motor_links_tempo = H_MOTORLI_TEMPO;
	_motor_rechts_tempo = H_MOTORRE_TEMPO;
	_drehung_links_ms = H_RACER_LINKS_MS;
	_drehung_rechts_ms = H_RACER_RECHTS_MS;

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

// Konstruktor mit pin <20>bergaben ... 

CodeRacer::CodeRacer(int 	tasterpin , int 	servopin,
	int us_trigger_pin, int us_echo_pin,
	int motor_links_frwd_pin, int motor_links_back_pin, int motor_links_enable_pin,
	int motor_rechts_frwd_pin, int motor_rechts_back_pin, int motor_rechts_enable_pin,
	int led_frwd, int led_stop, int led_links, int led_rechts
)
{
	_taster_pin = tasterpin;
	_servopin = servopin;
	_us_trigger_pin = us_trigger_pin;
	_us_echo_pin = us_echo_pin;
	_motor_links_frwd_pin = motor_links_frwd_pin;
	_motor_links_back_pin = motor_links_back_pin;
	_motor_links_enable_pin = motor_links_enable_pin;
	_motor_rechts_frwd_pin = motor_rechts_frwd_pin;
	_motor_rechts_back_pin = motor_rechts_back_pin;
	_motor_rechts_enable_pin = motor_rechts_enable_pin;
	_led_frwd_pin = led_frwd;
	_led_stop_pin = led_stop;
	_led_links_pin = led_links;
	_led_rechts_pin = led_rechts;

	_servo_dummy = new Servo();
	_servo = new Servo();

	_servo_mitte = H_SERVO_MITTE;
	_servo_links = H_SERVO_LINKS;
	_servo_rechts = H_SERVO_RECHTS;
	_schwenk_links = H_SERVO_SCHWENK_LINKS;
	_schwenk_rechts = H_SERVO_SCHWENK_RECHTS;

	_schwenk_position = _servo_mitte;
	_schwenk_richtung = SERVO_SCHWENK_LI;
	_schwenk_millis = millis();

	_motor_links_tempo = H_MOTORLI_TEMPO;
	_motor_rechts_tempo = H_MOTORRE_TEMPO;
	_drehung_links_ms = H_RACER_LINKS_MS;
	_drehung_rechts_ms = H_RACER_RECHTS_MS;

	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;

}



void CodeRacer::begin() {

	// Ultraschallsensor
	pinMode(_us_trigger_pin, OUTPUT);    // Ultraschallsensor: TRIG ist ein Ausgangspin. Es wird ein Signal zum Ultraschallsensor gesendet
	pinMode(_us_echo_pin, INPUT);        // Ultraschallsensor: ECHO ist ein Eingangspin. Es wird ein Signal vom Ultraschallsensor empfangen
	
	// Servo
	_servo->attach(_servopin);           // dem Servo Objekt "sagen" an welchen Pin am Schaltkreis der Server angeschlossen ist

	// Linker Motor
	pinMode(_motor_links_frwd_pin, OUTPUT);       // Linker Motor FORWARD ist ein Ausgang.
	pinMode(_motor_links_back_pin, OUTPUT);       // Linker Motor BACKWARD ist ein Ausgang.
	ledcSetup(MOTORPWM_LINKS, 5000, 8);              // channel 1, 50 Hz, 8-bit width
	ledcAttachPin(_motor_links_enable_pin, MOTORPWM_LINKS);    // Linker Motor SPEED mit Kanal 1 verbunden
	analog_write(_motor_links_enable_pin, 0);      // Linken Motor sicherheitshalber ausschalten :-) 

	// Rechter Motor
	pinMode(_motor_rechts_frwd_pin, OUTPUT);       // Rechter Motor FORWARD ist ein Ausgang.
	pinMode(_motor_rechts_back_pin, OUTPUT);       // Rechter Motor BACKWARD ist ein Ausgang.
	ledcSetup(MOTORPWM_RECHTS, 5000, 8);              // channel 2, 50 Hz, 8-bit width
	ledcAttachPin(_motor_rechts_enable_pin, MOTORPWM_RECHTS);    // Rechter Motor SPEED mit Kanal 2 verbunden
	analog_write(_motor_rechts_enable_pin, 0);      // Rechten Motor sicherheitshalber ausschalten :-) 

																									// LEDs
	pinMode(_led_frwd_pin, OUTPUT);     // LED Vorw<72>rts ist ein Ausgang
	pinMode(_led_stop_pin, OUTPUT);          // LED Stop ist ein Ausgang
	pinMode(_led_links_pin, OUTPUT);         // LED Links ist ein Ausgang
	pinMode(_led_rechts_pin, OUTPUT);        // LED Rechts ist ein Ausgang
																				// alle LEDS aus
	digitalWrite(_led_frwd_pin, LOW);
	digitalWrite(_led_stop_pin, LOW);
	digitalWrite(_led_links_pin, LOW);
	digitalWrite(_led_rechts_pin, LOW);

	// Taster
	taster_last_pressed_ms = 0;
	pinMode(_taster_pin, INPUT_PULLUP);
	attachInterrupt(digitalPinToInterrupt(_taster_pin), set_taster_state, FALLING);

	// Random
	randomSeed(analogRead(0));

	Serial.println("CodeRacer initialisiert ... ");
}

void CodeRacer::servo_einstellungen(int winkel_mitte, int winkel_links, int winkel_rechts, int schwenk_links, int schwenk_rechts)
{
	_servo_mitte = winkel_mitte;
	_servo_links = winkel_links;
	_servo_rechts = winkel_rechts;
	_schwenk_links = schwenk_links;
	_schwenk_rechts = schwenk_rechts;

	_schwenk_position = _servo_mitte;
	_schwenk_richtung = SERVO_SCHWENK_LI;
	_schwenk_millis = millis();

	Serial.println("CodeRacer Servo eingestellt ... ");

}

void CodeRacer::motor_einstellungen(int motor_links_tempo, int motor_rechts_tempo, int drehung_links_ms, int drehung_rechts_ms)
{
	_motor_links_tempo = motor_links_tempo;
	_motor_rechts_tempo = motor_rechts_tempo;
	_drehung_links_ms = drehung_links_ms;
	_drehung_rechts_ms = drehung_rechts_ms;
}

void CodeRacer::analog_write(int pin, int speed) {
	if (pin == _motor_links_enable_pin) { ledcWrite(MOTORPWM_LINKS, speed); }
	if (pin == _motor_rechts_enable_pin) { ledcWrite(MOTORPWM_RECHTS, 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);
	// Linken Motor abschalten 
	digitalWrite(_motor_links_frwd_pin, LOW);
	digitalWrite(_motor_links_back_pin, LOW);
	// Motoren beide ausschalten 
	analog_write(_motor_rechts_enable_pin, 0);
	analog_write(_motor_links_enable_pin, 0);
	// LEDs setzen
	digitalWrite(_led_frwd_pin, LOW);
	digitalWrite(_led_stop_pin, HIGH);
	digitalWrite(_led_links_pin, LOW);
	digitalWrite(_led_rechts_pin, LOW);
	delay(1000);
}


void CodeRacer::normal_tempo()
{
	//Serial.println("RACER_NORMAL_TEMPO");                            // Meldung am Monitor ausgeben
	analog_write(_motor_rechts_enable_pin, _motor_rechts_tempo);
	analog_write(_motor_links_enable_pin, _motor_links_tempo);
	_drive_forward = true;
	_drive_normal_forward = true;
}

void CodeRacer::vorwaerts()
{

	Serial.println("RACER_VORWAERTS");                            // Meldung am Monitor ausgeben

	if (false == _drive_forward) {
		// Rechten Motor auf vorw<72>rts stellen 
		digitalWrite(_motor_rechts_frwd_pin, HIGH);
		digitalWrite(_motor_rechts_back_pin, LOW);
		// Linken Motor auf vorw<72>rts stellen 
		digitalWrite(_motor_links_frwd_pin, HIGH);
		digitalWrite(_motor_links_back_pin, LOW);
		// 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::rueckwaerts()
{

	Serial.println("RACER_RUECKWAERTS");                            // Meldung am Monitor ausgeben

	if (false == _drive_forward) {
		// Rechten Motor auf r<>ckw<6B>rts stellen 
		digitalWrite(_motor_rechts_frwd_pin, LOW);
		digitalWrite(_motor_rechts_back_pin, HIGH);
		// Linken Motor auf r<>ckw<6B>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()
{
	Serial.println("RACER_LINKS");                            // Meldung am Monitor ausgeben
	_drive_forward = false;
	// LEDs setzen
	digitalWrite(_led_frwd_pin, LOW);
	digitalWrite(_led_stop_pin, LOW);
	digitalWrite(_led_links_pin, HIGH);
	digitalWrite(_led_rechts_pin, LOW);
	// Rechten Motor auf vorw<72>rts stellen 
	digitalWrite(_motor_rechts_frwd_pin, HIGH);
	digitalWrite(_motor_rechts_back_pin, LOW);
	// Linken Motor auf r<>ckw<6B>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);
	// Warten bis der RAcer sich gedreht hat
	delay(_drehung_links_ms);
	// Motoren wieder auschalten
	analog_write(_motor_rechts_enable_pin, 0);
	analog_write(_motor_links_enable_pin, 0);
}

void CodeRacer::rechts()
{
	Serial.println("RACER_RECHTS");                            // Meldung am Monitor ausgeben
	_drive_forward = false;
																															// LEDs setzen
	digitalWrite(_led_frwd_pin, LOW);
	digitalWrite(_led_stop_pin, LOW);
	digitalWrite(_led_links_pin, LOW);
	digitalWrite(_led_rechts_pin, HIGH);
	// Rechten Motor auf r<>ckw<6B>rts stellen 
	digitalWrite(_motor_rechts_frwd_pin, LOW);
	digitalWrite(_motor_rechts_back_pin, HIGH);
	// Linken Motor auf vorw<72>rts stellen 
	digitalWrite(_motor_links_frwd_pin, HIGH);
	digitalWrite(_motor_links_back_pin, LOW);
	// Motoren beide anschalten 
	analog_write(_motor_rechts_enable_pin, MOTORRE_MAX_TEMPO);
	analog_write(_motor_links_enable_pin, MOTORLI_MAX_TEMPO);
	// Warten bis der RAcer sich gedreht hat
	delay(_drehung_rechts_ms);
	// Motoren wieder auschalten
	analog_write(_motor_rechts_enable_pin, 0);
	analog_write(_motor_links_enable_pin, 0);
}

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();
	delay(1000);                                                 // Kurz warten, dass der Servo die Stellung erreicht
}

void CodeRacer::servo_schwenk()
{
	_schwenk = true;
	if (millis() - _schwenk_millis > SERVO_SCHWENK_MS) {
		_schwenk_position = _schwenk_position + _schwenk_richtung;
		if (_schwenk_position >= _schwenk_links) {
			_schwenk_position = _schwenk_links;
			_schwenk_richtung = SERVO_SCHWENK_RE;
		}
		if (_schwenk_position <= _schwenk_rechts) {
			_schwenk_position = _schwenk_rechts;
			_schwenk_richtung = SERVO_SCHWENK_LI;
		}
		_servo->write(_schwenk_position);                            // Servo auf den Winkel links drehen
		_schwenk_millis = millis();
	}
}

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<66>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 long echo_dauer_us[3] = { 0,0,0 };
	int messnr = 0;

	do{
		// Messung starten - ein kurzer Pulse "HIGH" wird zum TRIG pin des Ultraschallsensors geschickt
		pinMode(_us_echo_pin, OUTPUT);
		pinMode(_us_echo_pin, INPUT);
		digitalWrite(_us_trigger_pin, LOW);
		delayMicroseconds(2);
		digitalWrite(_us_trigger_pin, HIGH);
		delayMicroseconds(10);
		digitalWrite(_us_trigger_pin, LOW);

		// Messung der Dauer in Mikrosekundenmeasure bis das ECHO Pin vom Ultraschallsensor HIGH wird 
		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 (echo_dauer_us[messnr] > 200) {
				messnr++;
			}
		}
	} 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[1] * 0.0172;
	
	//Messwert am Monitor anzeigen
	//Serial.print("ABSTAND_MESSEN. Der Abstand in cm ist (mindestens):");
	//Serial.println(abstand_cm);

	return(abstand_cm);
}

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();
			_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();
		}
	}
		
	return(_coderracer_activ);
}

void IRAM_ATTR CodeRacer::set_taster_state() {
	if ((millis() - taster_last_pressed_ms) > 200) {
		taster_last_pressed_ms = millis();
		if (false == coderracer_activ) {
			coderracer_activ = true;
		}
		else {
			coderracer_activ = false;
		}
	}
}