Wie geht denn das?
In einem vorherigen Post habe ich bereits den Anschluss des USB-Host-Shields an einen Arduino Mega beschrieben. Ziel war es einen XBOX360-Controller zum Steuern des Roboters zu verwenden. Die Usb-Host-Library bringt einige Module für die Kommunikation mit verschiedenen USB-Geräten mit. Darunter auch eine Library für die XBOX-Controller. Die ausgelesenen Steuerdaten eines Steuerkreuzes werden dann mit einem APC220-Funkmodul an den Roboter geschickt und dort an den Motorcontroller weitergereicht.
Details bitte
Die Host-Library bietet die Möglichkeit sämtliche Steuerkreuze und Buttons des Controllers auszulesen. Die beiden analogen Steuerkreuze geben hierbei einen „signed 16Bit Integer“ für die X- & Y-Achsen aus. Das bedeutet das der mögliche Wert je Achse zwischen −32.768 und 32.767 liegen kann. Liegt das Steuerkreuz in Ruhezustand ist der Wert 0. Für meine Demo beschränke ich mich auf das Steuerkreuz welches sich unten-rechts auch dem Controller befindet. Die anderen Funktionen wie Buttons etc. werden hier nicht gebraucht.
Hier ein einfacher Beispielcode zum Auslesen des rechten Steuerkreuzes:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 |
// USB Library einbinden #include <XBOXUSB.h> USB Usb; // Instanz erstellen XBOXUSB Xbox(&Usb); // Xbox-Controller einbinden void setup() { Serial.begin(115200); Serial.println("USB-Host-Demo"); Usb.Init(); // Usb anwerfen } void loop() { Usb.Task(); // USB-Datenverarbeitung if(Xbox.Xbox360Connected) { // X-Achse auslesen int xWert = Xbox.getAnalogHat(RightHatX); // Y-Achse auslesen int yWert = Xbox.getAnalogHat(RightHatY); Serial.print("X:"); Serial.print(xWert); Serial.print(" - Y:"); Serial.print(yWert); Serial.println(); } } |
Nochmal ein wichtiger Hinweis: Ich müsst unbedingt(!) eine Externe Stromversorgung an den Arduino anschliessen. Der USB-Port alleine kann das ganze nicht mit Strom versorgen. Ihr bekommt dann einfach keine Daten. Ich habe mich da dumm und dusselig gesucht bis ich das Problem erkannt hatte.
Wenn ihr alles richtig angeschlossen und das Sketch auf den Arduino geladen habt, könnt ihr die Serielle Konsole öffnen. Es sollten dann die Werte des Steuerkreuzes ausgegeben werden
Wie auf dem Screenshot zu sehen ist haben wir nun Werte – und damit auch gleich das erste Problem: die Werte dort Oben bekomme ich wenn ich den Stick loslasse. Eigentlich erwartet man das beide Werte wieder auf 0 gehen, das passiert allerdings nicht da die Steuerkreuze ziemlich empflindlich sind. Ein leichtes antippen reicht um die Werte zu verändern…
Um das zu verhindern und einen sauberen Nullpunkt zu bekommen setzt man einen DEAD CENTER, also einen Bereich rund um den Nullpunkt welcher immer 0 ausgibt wenn sich die Werte in diesem Bereich befinden. Ich habe für mich einen Bereich von 7000 genommen. Das bedeutet: Wenn der Wert einer Achse zwischen -7000 und 7000 liegt gebe ich als Wert 0 aus.
Erweiterter Code mit „Dead Center“:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 |
void loop() { // ... // X-Achse auslesen int xWert = deadCenter(Xbox.getAnalogHat(RightHatX)); // Y-Achse auslesen int yWert = deadCenter(Xbox.getAnalogHat(RightHatY)); // ... } int deadCenter(int wert) { // Wenn Wert größer 0 aber kleiner 7000 if (wert > 0 && wert < 7000) { return 0; // Wenn Wert kleiner 0 aber größer -7000 } else if (wert < 0 && wert > -7000) { return 0; } return wert; } |
Nun habe ich ordentliche Werte. Weiter gehts 🙂
Die Funkübertragung
Um mit den Werten etwas anfangen zu können müssen die irgendwie zum Roboter übertragen werden. Ein Kabel ist keine Option. Bleibt also Funk.
Ich hatte noch ein Paar APC220-Funkmodule rumliegen. Die Module senden serielle Daten auf 434MHz und sind ziemlich leicht an einen Arduino anzubinden das sie über TTL-UART mit 5V angeprochen werden.
Überlegungen…
Wie bekommt man nun zwei variable Datenwerte (X- & Y-Achse) über die serielle Schnittstelle und die Funkmodule zum Roboter? Und wie erkennt man ob die Daten auch valide sind? Und, viel wichtiger: Wie sichere ich den Robot so ab das er bei falschen Daten (z.b. Funkstörungen) keinen Amoklauf veranstaltet?
Teil 1: Die Übertragung
Zum steuern des Roboters benötigt man zwei Werte: Drive & Turn. Der erste gibt an wie schnell und in welche Richtung der Roboter fährt, der zweite ob er sich nach links oder rechts drehen soll. Der benutze Sabertooth Motorcontroller benutzt hierfür jeweils Werte von -127 bis +127. Das sind 8 Datenbits (1 Byte). Wir haben allerdings Werte zwischen −32.768 und +32.767, also 16 Datenbits (2 Bytes). Für die Übertragung der beiden Achsen benötigen wir also insgesamt 32 Datenbits (4 Bytes).
Da die Funkstrecke möglichst Störungsfrei auch über größere Entfernungen funktioniert, habe ich die Funkmodule auf eine Baudrate von 9600 Bits pro Sekunde eingestellt. Theoretisch können also 9600 / 8 = 1200 Byte pro Sekunde übertragen werden. Das wir 2 Werte á 2 Byte übertragen, könnten(!) also 9600 / (8 Byte * (2 Pakete * 2 Byte)) = 300 Pakete pro Sekunde gesendet werden.
Theoretisch alles schön, praktisch blödsinn.
- Wozu 16 Bit Werte senden wenn der Motorcontroller damit garnichts anfangen kann
- Was ist mit der Fehlersicherheit? Nur ein falsches Bit und der Roboter fährt mit voller Kraft gegen die Hauswand.
- Wie erkennt man in welcher Reihenfolge die Daten ankommen? Kommt Drive immer vor Turn an?
Ok, meine Entscheidung ist gefallen: Wir basteln ein kleines Übertragunsprotokoll um die 3 Punkte zu umschiffen. Pro Datenpaket (Steueranweisung) wird ein 5-Byte Datenpaket übertragen.
Es beinhaltet einen 2 Byte Prefix zum erkennen des Paketes auf Empfängerseite, jeweils 1 Byte für die X- und die Y-Achse und eine einfache Checksumme um fehlerhafte Datenpakete zu erkennen.
| Beispielpaket | |||||
| Byte | 1 | 2 | 3 | 4 | 5 |
| 0xAA | 0xFF | 0x10 | 0x20 | 0xA7 | |
- Byte 1+2: Prefix (immer 0xAA & 0xFF)
- Byte 3: X-Achse (zwischen 0 und 254)
- Byte 4: Y-Achse (zwischen 0 und 254)
- Byte 5: Checksumme (XOR aus Byte 3+4)
Hier der komplette Sendercode:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 |
// USB Library einbinden #include <XBOXUSB.h> USB Usb; // Instanz erstellen XBOXUSB Xbox(&Usb); // Xbox-Controller einbinden void setup() { Serial.begin(115200); Serial.println("USB-Host-Demo"); Serial1.begin(9600); // Serial1 für das APC220 Modul Usb.Init(); // Usb anwerfen } void loop() { Usb.Task(); // USB-Datenverarbeitung if(Xbox.Xbox360Connected) { // X-Achse auslesen int xWert = deadCenter(Xbox.getAnalogHat(RightHatX)); // Y-Achse auslesen int yWert = deadCenter(Xbox.getAnalogHat(RightHatY)); sendRcData(xWert, yWert); } } uint8_t oldRCx; uint8_t oldRCy; void sendRcData(int xWert, int yWert) { // Schauen ob sich die Werte geändert haben, // wenn nicht brauchen wir nicht senden if ((xWert == oldRCx) && (yWert == oldRCy)) return; oldRCx = xWert; oldRCy = yWert; uint8_t rcpacket[5]; // Datenpaket (5 Werte zu 8 Bit); uint8_t xorvalue = 0; // Variable für die Checksumme // 16 Bit Werte in 8 Bit umwandeln int RCx = map(xWert, -32768, 32767, 0, 255); int RCy = map(yWert, -32768, 32767, 0, 255); // Checksumme bilden xorvalue ^= RCx; xorvalue ^= RCy; // Datenpaket zusammensetzen rcpacket[0]=0xFF; rcpacket[1]=0xAA; rcpacket[2]=RCx; rcpacket[3]=RCy; rcpacket[4]=xorvalue; // Und senden :) Serial1.write(rcpacket,5); Serial1.flush(); } int deadCenter(int wert) { if (wert > 0 && wert < 7000) { return 0; } else if (wert < 0 && wert > -7000) { return 0; } return wert; } |
Teil 2: Der Empfänger
Der Empfängercode macht eigentlich nich viel. Ankommende Daten vom Funkmodul annehmen, Datenpaket einlesen und nach validierung die Daten an den Motorcontroller senden.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 |
// Library für den Sabertooh Controller einbinden #include <Sabertooth.h> // Controller liegt an Serial2 Sabertooth ST(128, Serial2); // The Sabertooth is on address 128. We'll name its object ST. void setup() { Serial.begin(115200); Serial.println("ROBOT EMPFÄNGER"); Serial1.begin(9600); // Serial1 für das APC220 Modul Serial2.begin(9600); // Serial2 für den Motorcontroller // Motorcontroller Setup ST.setDeadband(5); // To change your Sabertooth back to its default, call ST.setRamping(0) ST.setRamping(0); ST.drive(0); // The Sabertooth won't act on mixed mode packet serial commands until ST.turn(0); // it has received power levels for BOTH throttle and turning, since it // mixes the two together to get diff-drive power levels for both motors. } int state = 0; uint8_t xorvalue = 0; int XValue; int YValue; void loop() { while(Serial3.available() > 0) { uint8_t ch = Serial3.read(); if (ch == 0xFF && state == 0) { xorvalue = 0; state = 1; } else if (ch == 0xAA && state == 1) { state = 2; } else { switch(state) { case 2: xorvalue ^= ch; state = 3; XValue = (ch - 127); break; case 3: xorvalue ^= ch; state = 4; YValue = (ch - 127); break; case 8: state = 0; if (xorvalue == ch) { // War das Datenpaket valide? ST.drive(XValue); // The Sabertooth won't act on mixed mode packet serial commands until ST.turn(YValue); // it has received power levels for BOTH throttle and turning, since it } else { Serial.println("Datenpaket Fehlerhaft"); } break; } } } } |
Das war es auch schon.
Bei Fragen: Fragen!!
Einfach einen Kommentar eintragen und ich versuche zu Antworten 🙂