Roboter Fahrzeug mit Blynk und Odometrie steuern

Willkommen zu einem neuen Blog aus unserer Reihe über Robot Cars. Diesmal geht es darum, den zurückgelegten Weg mit Hilfe der Anzahl der Umdrehungen der Räder zu ermitteln. Dieses Verfahren nennt man Odometrie. Das kommt aus dem Griechischen und bedeutet Wegmessung.

Geometrische Grundlagen

Für das Geradeausfahren ist das ganz einfach. Wenn sich beide Räder gleich schnell drehen, ist der zurückgelegte Weg nach einer Umdrehung gleich dem Umfang des Rades also mit Durchmesser d

Die Umdrehungen können mit einer Schlitzscheibe und einer Gabellichtschranke ermittelt werden. Besitzt die Schlitzscheibe n Schlitze erhalten wir n Impulse pro Umdrehung. Um den Weg zwischen zwei Impulsen zu ermitteln, müssen wir den Umfang durch die Impulse pro Umdrehung dividieren. Wir erhalten den Streckenfaktor.

Auch die Ermittlung des Drehwinkels ist bei einem zweirädrigen Fahrzeug einfach, wenn man davon ausgeht, dass sich beide Räder gegengleich mit derselben Drehzahl drehen. In diesem Fall beschreiben beide Räder einen Kreis um den unveränderten Standpunkt mit dem Radabstand a als Durchmesser. Um das Fahrzeug um 360 Grad zu drehen, muss eine Strecke von

zurückgelegt werden. Für einen Winkel α ergibt sich eine Strecke von

Beide Räder legen dieselbe Strecke zurück, aber in entgegengesetzter Richtung. Um die Anzahl der Impulse zu erhalten, muss man durch den Streckenfaktor dividieren.

Aus dem Kehrwert ergibt sich der Winkelfaktor im Grad/Impuls

Geometrie

Benötigte Hardware

Im Gegensatz zum ersten Teil benötigen wir in diesem Fall das ESP32 Modul, da zusätzliche Eingänge für die Gabel-Lichtschranken benötigt werden. Es sollte auch besser das Chassis aus dem 3D-Drucker verwendet werden, da damit die Montage der Gabellichtschranken wesentlich erleichtert wird.

Anzahl

Bauteil

Anmerkung

1

2 Motoren mit Rädern

 

1

Rollkugel als Nachlaufrad

 

6

Schrauben M3 x 30 mit Mutter

 

2

Schrauben M3 x 12 mit Mutter

 

1

Chassis aus dem 3D-Drucker

 

1

Powerpack-Halter aus dem 3D-Drucker

 

1

D1 Board NodeMCU ESP8266MOD-12F

 

1

Set Gabellichtschranken mit Schlitzscheiben

 

2

Blechschrauben 2.2 X 6.5 mm

 

 

Damit das Motortreiber Shield mit den ESP Controllern verwendet werden kann, sind kleine Umbauarbeiten notwendig, da die ESP-Controller mit 3.3 V Versorgungsspannung arbeiten.

Vom Anschluss D7 sollte ein 10kOhm Widerstand an GND angeschlossen werden. Das ist nötig, da am Shield der Pin D7 über einen Pullup Widerstand von 10KOhm mit 5V verbunden ist.

Motortreiber ShieldDie Anschlüsse A0 bis A5, sowie die zugehörigen Versorgungsanschlüsse, sollten mit Stiftleisten bestückt werden. Damit diese Anschlüsse für Sensoren genutzt werden können, muss außerdem die Versorgungsspannung statt an 5V an 3.3V angeschlossen werden. Das geschieht einfach dadurch, dass  auf der Unterseite die Verbindungsleitung zu den Pins aufgetrennt wird. Anschließend kann man die Pins mit dem 3.3V Pin verbinden. Auch die Anschlüsse für die Versorgungsspannung sollten bestückt werden.

Zum Anschluss der beiden Motoren werden die Ausgänge M2 und M4 benutzt. Die Motoranschlüsse werden mit den entsprechenden Schraubklemmen am Motorshield verbunden. Sollte das Fahrzeug über die USB Buchse am Mikrocontroller versorgt werden, muss der Jumper (roter Kreis) entfernt und der Eingang M+ mit dem 5V Anschluss des Mikrocontrollers verbunden werden (grün eingezeichnet).

Motoren am Treiber Shield

Für die Messung der Drehzahl werden jeweils eine Schlitzscheibe auf das innere Ende der Motor-Achsen gesteckt und die Gabellichtschranken mit je einer Blechschraube an der dafür vorgesehenen Halterung festgeschraubt.

Schlitzscheiben

Nun werden VCC, GND und D0 über ein dreipoliges Kabel mit den Anschlüssen A2 und A3 am Motorshield verbunden.  Vcc mit +5V, GND mit Gnd und D0 mit A2 bzw A3.

Motorshield

Das Motorshield wird dann einfach auf das Mikrocontroller Board aufgesteckt. Weitere Verdrahtungsmaßnahmen sind nicht erforderlich.
Zur mechanischen Befestigung der Boards sind beim Chassis aus dem 3D-Drucker entsprechende Löcher vorhanden, an denen mit Distanzstücken das Mikrocontroller Board festgeschraubt werden kann. Beim Bausatz muss man eventuell geeignete Löcher bohren.

Software

Zur Fernsteuerung des Fahrzeugs soll die frei erhältliche Software Blynk verwendet werden. Blynk ist ein Kickstarter Projekt, das eine möglichst einfache Steuerung von Mikrocontrollern mit einem Smartphone erlaubt. Wesentliches Element ist die App, mit der über eine Art Baukasten eine Anwendung erstellt werden kann. Alle Informationen zu dieser Anwendung werden im Blynk Server gespeichert. Jede Anwendung erhält eine eindeutige Identifikationsnummer. Auf der Mikrocontrollerseite sorgt eine Bibliothek dafür, dass der Mikrocontroller mit dem Blynk Server kommuniziert und dass die Anwendung am Smartphone direkt die Pins des Mikrocontrollers lesen oder ansteuern kann. Dazu ist keine weitere Programmierung am Mikrocontroller erforderlich. Die einzige Bedingung ist, dass der Mikrocontroller Zugriff auf den Blynk Server haben muss. Da für die Fernsteuerung des Fahrzeugs aber keine Pins des Mikrocontrollers direkt angesteuert werden können, wird doch ein wenig Programmierung notwendig sein.

Zuerst aber muss die Blynk App am Smartphone installiert werden. Nachdem die App installiert und gestartet wurde, erscheint der Anmeldebildschirm. Damit man Blynk nutzen kann, braucht man einen Account für den Blynk-Server. Dazu sind lediglich eine E-Mail-Adresse und ein beliebiges Passwort erforderlich.

Nachdem Sie einen Account angelegt haben, erscheint eine Nachricht zum Thema Energie. Um ein Blynk-Projekt zu erstellen, braucht man für jedes Bildschirmelement eine bestimmte Menge von Energie. Mit einem neuen Account erhält man automatisch 2000 Energiepunkte, mit denen man arbeiten kann. Braucht man für ein Projekt mehr Energiepunkte, kann man solche käuflich erwerben.

Nach diesem Meldungsfenster erscheint der Startbildschirm. Hier kann nun das Robocar Projekt über den folgenden QR-Code geladen werden.

Sie sehen nun das Projektfenster. Wenn Sie auf das Mutternsymbol klicken, öffnen sich die Projekteinstellungen. In den Projekteinstellungen finden Sie den Link „Email all“. Wenn Sie darauf klicken, erhalten Sie eine E-Mail mit dem Key, den Ihr Mikrocontroller Sketch braucht, um mit dem Blynk-Server zu kommunizieren. Mit dem Dreieck-Symbol am Projektfenster oben rechts starten Sie die Anwendung.

Blync App

Mehr Informationen zu Blynk, finden Sie im Internet oder im Smarthome-Buch. Im Buch wird auch beschrieben, wie man einen eigenen privaten Blynk-Server auf einem Raspberry Pi installieren kann.

Die Steuerung erfolgt, indem man die gewünschte Strecke und den gewünschten Winkel einstellt. Durch Klicken auf die Knöpfe Vorwärts oder Rückwärts fährt das Fahrzeug die eingestellte Strecke. Durch Klicken auf die Knöpfe Links oder Rechts dreht sich das Fahrzeug um den eingestellten Winkel.

Klickt man den Button Lernen, so wird auf den Lernmodus umgeschaltet. Jeder eingegebene Befehl wird nun aufgezeichnet und kann später automatisch ausgeführt werden. Ein erneutes Klicken auf den Button Lernen beendet den Lernmodus. Jetzt können durch Klicken auf den Button Run die gespeicherten Befehle automatisch ausgeführt werden.

Am unteren Rand wird angezeigt wie viele Befehlszeilen gespeichert wurden. Es können maximal 30 Befehlszeilen gespeichert werden. Beim Umschalten auf den Lern Modus, wird der Befehlszeilen-Zähler wieder auf 0 gesetzt.

Der Sketch

Außer dem Paket für den ESP32 benötigen Sie die Blynk-Bibliothek, die über die Bibliotheksverwaltung installiert werden kann.

 #include <WiFi.h>
 #include <BlynkSimpleEsp32.h>
 
 // You should get Auth Token in the Blynk App. // Go to the Project Settings (nut icon).
 
 // BLYNK authentication key
 #define AUTH "*********************************"
 // SSID of your WLAN
 #define SSID "*******************************"
 // Passkey for your WLAN
 #define PASS "***********************************"
 // Name of private BLYNK server, or "" if you use public BLYNK server
 #define SRV "raspberrypi4"
 
 
 #define MAXCOMMANDS 30 //Anzahl der Befehlszeilen, die gespeichert werden können
 
 #define BLYNK_PRINT Serial
 
 //Bits im Schieberegister zu Motor Zuordnung
 #define M1A 2   //Motor1 A
 #define M1B 3   //Motor1 B
 #define M2A 1   //Motor2 A
 #define M2B 4   //Motor2 B
 #define M3A 5   //Motor3 A
 #define M3B 7   //Motor3 B
 #define M4A 0   //Motor4 A
 #define M4B 6   //Motor4 B
 
 //Pins für das Drehrichtungs Schieberegister
 #define SHIFT_IN 12       //Dateneingang Arduino D8
 #define SHIFT_CLK 17      //Schiebetackt Arduino D4
 #define LATCH_CLK 19     //Speichertakt Arduino D12
 #define OUT_ENABLE 14    //Mit LOW Ausgang aktivieren Arduino D7
 
 //PWM für Motoren Geschwindigkeit zwischen 0 und 1023
 #define M1PWM 23   //Pin für Motor1 Arduino D11
 #define MRIGHT 25    //Pin für Motor2 Arduino D3
 #define M3PWM 27   //Pin für Motor3 Arduino D6
 #define MLEFT 16   //Pin für Motor4 Arduino D5
 
 //Servo Anschlüsse
 #define SERVO1 5  //Pin für Servo1 D10
 #define SERVO2 13   //Pin für Servo2 D9
 
 //Motor Zuordnung
 #define DRIGHT 2
 #define DLEFT 4
 
 //Konstanten für Drehrichtung
 #define STOP 0
 #define FORWARD 1
 #define BACKWARD 2
 
 #define SPEEDLEFT 35
 #define SPEEDRIGHT 34
 
 //Aktueller Inhalt des Richtungs-Schieberegisters
 uint32_t directions = 0;
 
 typedef
 struct {
   char cmd;
   uint16_t val;
 } CMDLINE;
 
 CMDLINE commands[MAXCOMMANDS];
 uint8_t cmdCnt = 0;
 
 int32_t cntleft = 0;
 int32_t cntright = 0;
 uint16_t strecke = 0;
 uint16_t winkel = 0;
 boolean button = 0;
 float winkelfaktor, streckenfaktor;
 boolean learning = false;
 boolean program = false;
 uint8_t prgStep;
 
 //Für Interrupt Synchronisation
 portMUX_TYPE leftMux = portMUX_INITIALIZER_UNLOCKED;
 portMUX_TYPE rightMux = portMUX_INITIALIZER_UNLOCKED;
 
 
 //Interrupt Service Routine für linken Speedsensor
 void IRAM_ATTR isrLeft() {
   portENTER_CRITICAL(&leftMux);
   cntleft--;
   portEXIT_CRITICAL(&leftMux);
   if ((cntleft <= 0) && (cntright<=0)) {
     motors(0,0);
     if (program) nextStep();
  }
 }
 
 //Interrupt Service Routine für rechten Speedsensor
 void IRAM_ATTR isrRight() {
   portENTER_CRITICAL(&rightMux);
   cntright--;
   portEXIT_CRITICAL(&rightMux);
   if ((cntleft <= 0) && (cntright<=0)) {
     motors(0,0);
     if (program) nextStep();
  }
 }
 
 
 
 //Füllt das Schieberegister zur Motorsteuerung
 //mit dem Inhalt von directions
 void sendDirections() {
   uint8_t i;
   digitalWrite(LATCH_CLK, LOW);  //Speicher sperren
   digitalWrite(SHIFT_IN, LOW);   //Eingang auf 0
   for (i=0; i<8; i++) {
     digitalWrite(SHIFT_CLK, LOW);//Bit für Bit einlesen
     if (directions & bit(7-i)) {
       digitalWrite(SHIFT_IN, HIGH);
    } else {
       digitalWrite(SHIFT_IN, LOW);
    }
     digitalWrite(SHIFT_CLK, HIGH);
  }
   digitalWrite(LATCH_CLK, HIGH); //Mit der positiven Flanke speichern
 }
 
 void nextStep() {
   if (prgStep < cmdCnt) {
      switch (commands[prgStep].cmd) {
        case 'F' :
        case 'B' : drive(commands[prgStep].cmd,commands[prgStep].val);
               break;
        case 'L' :
        case 'R' : turn(commands[prgStep].cmd,commands[prgStep].val);
               break;
      }
      prgStep++;
  } else {
     program = false;
  }
 }
 
 //Die Drehrichtung für einen Motor festlegen
 void setDirection(uint8_t motor, uint8_t direction) {
   uint8_t a=0, b=0;
   //Bitnummern für den gewählten Motor bestimmen
   switch (motor) {
     case 1: a=M1A; b=M1B; break;
     case 2: a=M2A; b=M2B; break;
     case 3: a=M3A; b=M3B; break;
     case 4: a=M4A; b=M4B; break;
  }
   //zuerst beide Bits für den Motor auf 0 setzen bedeutet STOP
   directions &= ~ bit(a) & ~ bit(b);
 
   switch (direction) {
     case FORWARD: directions |= bit(a); break; //Für VORWÄRTS Bit A auf 1
     case BACKWARD: directions |= bit(b); break;//Für RÜCKWÄRTS Bit B auf 1
  }
   //Serial.printf("Directions = %x\n",directions);
   sendDirections(); //Neue Einstellung ans Schieberegister senden
 }
 
 
 //Geschwindigkeit
 int z;
 
 
 void motors(int16_t left, int16_t right) {
   //Serial.printf("Links %i, rechts %i\n",left,right);
   //Richtung
   if (left<0) {
     setDirection(DLEFT,BACKWARD);
  } else if (left == 0){
     setDirection(DLEFT,STOP);
  } else {
     setDirection(DLEFT,FORWARD);
  }
   if (right<0) {
     setDirection(DRIGHT,BACKWARD);
  } else if (right == 0){
     setDirection(DRIGHT,STOP);
  } else {
     setDirection(DRIGHT,FORWARD);
  }
   //Geschwindigkeit
   ledcWrite(DLEFT,abs(left));
   ledcWrite(DRIGHT,abs(right));
 }
 
 void drive(char direction, uint16_t val) {
   if (learning) {
     if (cmdCnt < MAXCOMMANDS) {
       commands[cmdCnt].cmd = direction;
       commands[cmdCnt].val = val;
       cmdCnt++;
    }
  } else {
     cntleft = val;
     cntright = val;
     if (direction == 'F') {
       motors(z,z);
    } else {
       motors(-z,-z);
    }
  }      
 }
 
 void turn(char direction, uint16_t val) {
   if (learning) {
     if (cmdCnt < MAXCOMMANDS) {
       commands[cmdCnt].cmd = direction;
       commands[cmdCnt].val = val;
       cmdCnt++;
    }
  } else {
     cntleft = val;
     cntright = val;
     if (direction == 'L') {
       motors(-z,z);
    } else {
       motors(z,-z);
    }
  }      
 }
 
 void setup() {
   Serial.begin(115200);
   Serial.println();
   Serial.println("Initialisierung");
   //Alle verwendeten Pins als OUTPUT setzen
   pinMode(SHIFT_IN,OUTPUT);
   pinMode(SHIFT_CLK,OUTPUT);
   pinMode(LATCH_CLK,OUTPUT);
   pinMode(OUT_ENABLE,OUTPUT);
   ledcSetup(DLEFT, 100, 10);
   ledcSetup(DRIGHT,100, 10);
   ledcAttachPin(MLEFT,DLEFT);
   ledcAttachPin(MRIGHT,DRIGHT);
   //Alle Motoren STOP
   directions = 0;
   //Odometrie Parameter berechnen
   //Streckenfaktor = Raddurchmesser * PI / Anzahl_Schlitze
   streckenfaktor = 67 * 3.14 /20; //= 10.524 mm/Impuls
   //Winkelfaktor = 360 Grad / (Achsabstand * PI) * streckenfaktor
   winkelfaktor = (360 * 67)/(130 * 20); //= 9.277 Grad/Impuls
   sendDirections();  //Ans Schieberegister senden
   digitalWrite(OUT_ENABLE,0); //Ausgänge des Schieberegisters freigeben
   z=1023;
   pinMode(SPEEDLEFT,INPUT);
   pinMode(SPEEDRIGHT,INPUT);
   cntleft=0;
   cntright=0;
   attachInterrupt(SPEEDLEFT,isrLeft,FALLING);
   attachInterrupt(SPEEDRIGHT,isrRight,FALLING);
   
 
   Serial.println("Starte BLYNK");
   #ifdef SRV
     Blynk.begin(AUTH, SSID, PASS, SRV, 8080);
   #else
     Blynk.begin(AUTH, SSID, PASS);
   #endif
   button = 0;
 }
 
 //Kleine Testschleife
 void loop() {
     Blynk.run();
 }
 
 BLYNK_WRITE(V0)
    { strecke = param[0].asInt(); }
 BLYNK_WRITE(V1)
    { winkel = param[0].asInt(); }
 BLYNK_WRITE(V2)
 { if (param[0].asInt() == 0) {
       button = false;
    } else {
         if (!button) {
             button = true;
             drive('F',strecke / streckenfaktor);
        }
    }
 }
   
 BLYNK_WRITE(V3)
 { if (param[0].asInt() == 0) {
       button = false;
    } else {
         if (!button) {
             button = true;
             drive('B',strecke /streckenfaktor);
        }
    }
 }
 BLYNK_WRITE(V4)
 { if (param[0].asInt() == 0) {
       button = false;
    } else {
       if (!button) {
         button = true;
         turn('L',winkel / winkelfaktor);
      }
    }
 }
 BLYNK_WRITE(V5)
 { if (param[0].asInt() == 0) {
     button = false;
    } else {
         if (!button) {
             button = true;
             turn('R',winkel /winkelfaktor);
        }
    }
 }
 
 BLYNK_WRITE(V6)
 {
     if (!learning) {
         prgStep=0;
         program = true;
         nextStep();
    }
 }
 
 BLYNK_WRITE(V7)
 {
     learning = (param[0].asInt() != 0);
     if (learning) cmdCnt = 0;
 }
 
 BLYNK_READ(V8)
 {
     Blynk.virtualWrite(V8,cmdCnt);
 }

Quellcode als Download

Viel Spaß mit dem Roboter Car

 

Esp-32Progetti per principianti

2 Kommentare

Gerald Lechner

Gerald Lechner

Wenn in Zeile 72 und 73 die Strecke auf 300 und der Winkel auf 90 voreingestellt werden, dann stimmen die Werte nach dem Start mit den Schiebern in der Blynk-App überein. Das Problem mit der Verbindung kommt daher, dass das Board eine relativ schlechte Wlan Empfindlichkeit hat

Walter

Walter

Sehr schönes Projekt, und einfacher als erst gedacht. Vor allem die Odometrie ist so ein Kinderspiel.
Umgesetzt mit ein ESP32 Dev Board, motordriver L289N und lokalen Blynkserver.

Ein offenes Problem ist, dass der Blynkserver keine initiale Werte gibt ( oder ich nicht weis wie ). Was bedeutet das erst die Schieber für Distanz und Winkel betätigt werden müssen, sonst ist der Wert der den man im Programm festlegt ( = 0 und dann passiert nichts ).
Der Blynkserver ist für mich sowie so eine Blackbox, wobei manchmal keine Verbindung zu Stande kommt, scheinbar ohne Ursache. Nach einigen Reboots funktioniert es dann auf einmal .

Einen Kommentar hinterlassen

Alle Kommentare werden vor der Veröffentlichung moderiert

Post di blog consigliati

  1. Installa ESP32 ora dal gestore del consiglio di amministrazione
  2. Lüftersteuerung Raspberry Pi
  3. Arduino IDE - Programmieren für Einsteiger - Teil 1
  4. ESP32 - das Multitalent
  5. OTA - Over the Air - Programmazione ESP tramite WLAN