library/Arduino/libraries/CodeRacer/CodeRacer.cpp

589 lines
18 KiB
C++

// 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;
}
}
}