// 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 Ü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ä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ärts stellen digitalWrite(_motor_rechts_frwd_pin, HIGH); digitalWrite(_motor_rechts_back_pin, LOW); // Linken Motor auf vorwä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ä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() { 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ärts stellen digitalWrite(_motor_rechts_frwd_pin, HIGH); digitalWrite(_motor_rechts_back_pin, LOW); // 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); // 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ärts stellen digitalWrite(_motor_rechts_frwd_pin, LOW); digitalWrite(_motor_rechts_back_pin, HIGH); // Linken Motor auf vorwä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ä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; } } }