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.
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
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 |
|
|
|
1 |
|
|
|
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 |
|
|
|
1 |
|
|
|
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.
Die 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).
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.
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.
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.
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.
// You should get Auth Token in the Blynk App. // Go to the Project Settings (nut icon).
// BLYNK authentication key
// SSID of your WLAN
// Passkey for your WLAN
// Name of private BLYNK server, or "" if you use public BLYNK server
//Anzahl der Befehlszeilen, die gespeichert werden können
//Bits im Schieberegister zu Motor Zuordnung
//Motor1 A
//Motor1 B
//Motor2 A
//Motor2 B
//Motor3 A
//Motor3 B
//Motor4 A
//Motor4 B
//Pins für das Drehrichtungs Schieberegister
//Dateneingang Arduino D8
//Schiebetackt Arduino D4
//Speichertakt Arduino D12
//Mit LOW Ausgang aktivieren Arduino D7
//PWM für Motoren Geschwindigkeit zwischen 0 und 1023
//Pin für Motor1 Arduino D11
//Pin für Motor2 Arduino D3
//Pin für Motor3 Arduino D6
//Pin für Motor4 Arduino D5
//Servo Anschlüsse
//Pin für Servo1 D10
//Pin für Servo2 D9
//Motor Zuordnung
//Konstanten für Drehrichtung
//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");
Blynk.begin(AUTH, SSID, PASS, SRV, 8080);
Blynk.begin(AUTH, SSID, PASS);
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);
}
2 Kommentare
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
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 .