RP6 WIFI und Wireless IMU (für Android)

Hallo

Endlich habe ich mal wieder Zeit und Muse für ein neues Projekt. Es soll ein Vehikel mit der M256-Platine als Steuereinheit werden. Im Prinzip wäre es fahrbereit, nun fehlt noch eine Handsteuerung. WIFI-Terminal mit Tastensteuerung ist nicht so der Bringer und Drahtlos sollte es schon sein. Und was drängt sich da auf? Das Smartphon. Nach kurzem googlen wurde ich fündig:

Wireless IMU für Android
(Dank an Jan Zwiener)

Die Android-App möchte nur WIFI-Rechte und sendet Sensordaten (Accelerometer, Gyroscope und Magnetometer) per UDP ins Netzwerk. Einstellen muss man die IP des m256 und den Port (meist 2000 :) Der Empfang der Daten mit dem RP6-WIFI ist angenehm einfach:

Code:

---

        uint8_t c, n=0, debug=1;

        enter_cmd_mode_WIFI();
        issueCMD_WIFI("set ip protocol 3","AOK"); // TCP and UDP empfangen
        leave_cmd_mode_WIFI();

        while(true)
        {
                if(getBufferLength_WIFI()) // Daten empfangen?
                {
                        c=readChar_WIFI(); // Zeichen einlesen...
                        //writeCharLCD(c);
                        writeChar_WIFI(c); // ...und zum WIFI-Terminal senden
                        if(debug) n++;
                        setStopwatch1(0);
                        startStopwatch1(); // Timeout starten
                }
                if((getStopwatch1() > 30) || (n >= 96)) // Timeout ist Zeilenende oder 96 Zeichen bei drei Sensoren
                {
                        stopStopwatch1();
                        setStopwatch1(0);
                        writeString_P_WIFI("\n\r");
                        if(debug)
                        {
                                writeChar_WIFI('*');
                                writeInteger_WIFI(n,10);
                                n=0;
                                writeString_P_WIFI("*\n\r");
                        }
                }
        }

---

Ich zeige hier mit Absicht keine detailierte Auswertung der Daten, denn das kann ich im Moment selbst noch nicht und ich würde gerne sehen, wie ihr das umsetzt. Nett ist, dass die Daten parallel zur Terminalverbindung empfangen werden. Viel Spass damit. :)

Gruß

mic

Hallo

Nach ein paar (vielen) Versuchen habe ich nun eine zufriedenstellende Funktion zusammengebastellt. Vorerst noch ohne LCD, denn die Ausgabe bremst schon heftig, vor allem ohne Busy-Abfrage. Das größte Problem war das Erkennen vom Start der eigentlichen Daten. Wenn man den Datenstream vom Handy anschaut fällt auf, dass nur die Sensornummer zwischen zwei Kommas gesendet wird. Deshalb ist der Einleseablauf nun folgender:

1.: zwei aufeinanderfolgende Kommas suchen, eine Stelle vor dem zweiten Komma befindet sich die Sensornummer.
2.: nachfolgende Zeichen Einlesen und in einem String zwischenspeichern.
3.: dabei auf den Punkt achten
4.: den Punkt nicht speichern, so werden aus Gleitpunkt Integer.
5.: nach dem Punkt noch drei weitere Zeichen (die Nachkommastellen) speichern. (wenn's hier einen 16bit-Überlauf geben sollte, kann man auch nur zwei Stellen einlesen)
6.: mit atoi einen Integerwert erzeugen und in der entsprechenden Variablen für Sensor und Achse abspeichern.
7.: Bereitmachen für's Einlesen des den nächsten Sensors

Der Code sieht so aus:

Code:

---

// m256 und wireles imu  (Daten empfangen und Motor ansteuern)                  mic 13.09.2014

// Daten per UDP vom Handy empfangen
// https://www.roboternetz.de/community/threads/65626-RP6-WIFI-und-Wireless-IMU-%28f%C3%BCr-Android%29
// Zusatz im Makefile: COMMON_FLAGS += -std=gnu99

#include "RP6M256Lib.h"
#include "RP6M256uart.h" // für die serielle Kommunikation mit dem WIFI-Chip

// LCD 20x4
void setCursorPosLCD_20x4(uint8_t line, uint8_t pos);
void _showScreenLCD_P_20x4(const char *line1, const char *line2, const char *line3, const char *line4);
#define showScreenLCD_20x4(__line1,__line2, __line3,__line4); \
({_showScreenLCD_P_20x4((PSTR(__line1)),(PSTR(__line2)), (PSTR(__line3)),(PSTR(__line4)));})

int main(void)
{
        initRP6M256();
        initLCD();

        // Timer1 für 16MHz-ATMega256 Servoansteuerung 20ms Mitte=3000
        # define OC1A                                PB5
        # define OC1B                                PB6
        DDRB |= (1<<OC1A) | (1<<OC1B);                // PWM-Pins sind Ausgang...
        PORTB &= ~((1<<OC1A) | (1<<OC1B));            // ...und low
          TCCR1A = 0;
          TCCR1B = (0<<CS12)|(1<<CS11)|(0<<CS10);        // prescaler /8
        TCCR1A |= (1<<WGM11)|(0<<WGM10);              // Mode 14: FastPWM mit ICR1 als Top
        TCCR1B |= (1<<WGM13)|(1<<WGM12);
        TCCR1A |= (1<<COM1A1)|(0<<COM1A0);              // OCR1A set on Botton, reset on Match
        TCCR1A |= (1<<COM1B1)|(0<<COM1B0);              // OCR1B set on Botton, reset on Match
        TCNT1 = 0;                                // Timer1 zurücksetzen;
  ICR1 = 40000;                            // 16MHz/8/20mS für 20ms-Zyklus
        OCR1A = 3000;                            // Match und OC1A auf Low nach 1,5ms
        OCR1B = 3000;                            // Match und OC1B auf Low nach 1,5ms

        // Make sure WLAN Module Packet size / buffer size and flush timer is OK
        enter_cmd_mode_WIFI();
        issueCMD_WIFI("set comm size 1024","AOK");
        issueCMD_WIFI("set comm time 10","AOK");
        leave_cmd_mode_WIFI();

        // LCD-Backlight
        // DDRB |= (1<<PB6); // LCD-Backlight an OC1B
        // PORTB &= ~(1<<PB6);
        DDRE |= (1<<PE4); // LCD-Backlight an OC3B
        PORTE &= ~(1<<PE4);

        uint16_t pwm_l=3000, pwm_r=3000;

        clearLCD();
        setLEDs(0);
        //uint16_t lipo;

        uint8_t c=0, last_c, punkt=0, komma=0;
        uint8_t  s_nr=0, s_achse=0, s_stelle=0;
        char imu_temp[11]="0000000000";
        int ax=0, ay=0, az=0, gx, gy, gz, mx, my, mz;

        uint8_t debug=0; // debug: 1=Daten direkt 2=Integerwerte 3=beides
        uint8_t motor_an=1;

        enter_cmd_mode_WIFI();
        issueCMD_WIFI("set ip protocol 3","AOK"); // TCP and UDP empfangen
        leave_cmd_mode_WIFI();

        clearReceptionBuffer_WIFI();
        if(debug & 2) startStopwatch1();
        while(true)
        {
                if(motor_an==1)
                {
                        OCR1A=pwm_l-ay/25-ax/25; // GAS!!! (Hier müssen die RP6-Besitzer die Motorbefehle einsetzen...
                        OCR1B=pwm_r-ay/25+ax/25; // ...die sie zum RP6 senden mit I2C wollen
                }

                if(getBufferLength_WIFI()) // Daten empfangen?
                {
                        last_c=c;
                        c=readChar_WIFI(); // Zeichen einlesen...
                        if((c=='.') && (komma==1)) komma=0; // Punkt folgt auf Komma -> neuer Versuch (2b)
                        if((c=='.') && (komma==2) && (punkt==0)) punkt=1;  // Punkt beim Einlesen eines Wertes (3)
                        if((c==',') && (komma==1)) // zwei aufeinanderfolgende Kommas erkannt (2a)
                        {
                                komma=2;
                                s_nr=last_c; // die Sensornummer steht eine Stelle vor der aktuellen Lesestelle des Datenstreams
                                s_stelle=0; // der String wird ab der ersten Stelle zusammengebastelt
                                if(debug & 1) writeChar_WIFI(s_nr);
                        }
                        if((c==',') && (komma==0)) komma=1; // erstes Komma gefunden (1)
                        if((komma==2) && (c!='.') && (c!=',')) // Werte werden eingelesen (als Integer ohne Punkt ;)
                        {
                          imu_temp[s_stelle]=c;
                          if(debug & 1) writeChar_WIFI(c);
                          s_stelle++;
                          if(punkt) punkt++; // nach dem Dezimalpunkt werden die Nachkommastellen gezählt
                          if(punkt==4) // Anzahl der Stellen nach dem Punkt+1(4=default, 3 für 16bit-Integer ohne Überlauf)
                                {
                                        imu_temp[s_stelle]=0; // String Endekennung anfügen
                                        if(s_nr=='3') switch(s_achse) //Accelerometer
                                        {
                                          case 0:ax=atoi(imu_temp); break;
                                          case 1:ay=atoi(imu_temp); break;
                                          case 2:az=atoi(imu_temp); break;
                                        }
                                        if(s_nr=='4') switch(s_achse) // Gyroscope
                                        {
                                          case 0:gx=atoi(imu_temp); break;
                                          case 1:gy=atoi(imu_temp); break;
                                          case 2:gz=atoi(imu_temp); break;
                                        }
                                        if(s_nr=='5') switch(s_achse) // Magnetometer
                                        {
                                          case 0:mx=atoi(imu_temp); break;
                                          case 1:my=atoi(imu_temp); break;
                                          case 2:mz=atoi(imu_temp); break;
                                        }
                                        punkt=0;
                                        s_achse++;
                                        s_stelle=0;
                                        if(s_achse==3)
                                        {
                                          s_achse=0;
                                          s_stelle=0;
                                                komma=0;
                                                punkt=0;
                                                if(debug & 1) writeChar_WIFI('\n');
                                        }
                                }
                        }
                }
                if((debug & 2) && (komma==0) && (getStopwatch1() > 500))
                {
                    setStopwatch1(0);
                        writeString_P_WIFI("ACC: ");
                        writeInteger_WIFI(ax,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(ay,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(az,10);
                        writeString_P_WIFI("\nGyr: ");
                        writeInteger_WIFI(gx,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(gy,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(gz,10);
                        writeString_P_WIFI("\nMag: ");
                        writeInteger_WIFI(mx,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(my,10);
                  writeChar_WIFI(' ');
                  writeInteger_WIFI(mz,10);
                  writeString_P_WIFI("\n\n");
                }
        }
}

// LDC 20x4
void setCursorPosLCD_20x4(uint8_t line, uint8_t pos)
{
        pos |= 128;
        if(line==1) pos += 0x40;
        else if(line==2) pos += 20;
        else if(line==3) pos += 0x40+20;
        writeLCDCommand(pos);
}
void _showScreenLCD_P_20x4(const char *line1, const char *line2, const char *line3, const char *line4)
{
        clearLCD();
        writeNStringLCD_P(line1);
        setCursorPosLCD_20x4(1, 0);
        writeNStringLCD_P(line2);
        setCursorPosLCD_20x4(2, 0);
        writeNStringLCD_P(line3);
        setCursorPosLCD_20x4(3, 0);
        writeNStringLCD_P(line4);
}

---

Die Ausgabe am WIFI-Terminal mit debug=2 (mit Booten und Übertragen des Programms) sieht so aus:

Code:

---

[WIFIBOOT]
[ACK]
RP6:
  ÿÿ  g[B][B][B][B][B][B][B][B][B][B][B][B][B][B][
[READY]
[ACK]
ACC: 0 0 0
Gyr: 289 -4864 -17424
Mag: 257 207 7680

ACC: 0 0 0
Gyr: 289 -4864 -17424
Mag: 257 207 7680

ACC: 0 0 0
Gyr: 289 -4864 -17424
Mag: 257 207 7680

ACC: -459 192 9960
Gyr: -136 26 27
Mag: -17755 -1717 -32616

ACC: -612 -1991 9233
Gyr: -443 166 -219
Mag: -18101 -1874 -32519

ACC: -1110 -3447 8964
Gyr: 48 -12 -145
Mag: -17887 351 -32691

ACC: 2452 1993 11225
Gyr: -195 -241 98
Mag: -18508 4037 32172

ACC: 1073 920 8237
Gyr: -354 552 -70
Mag: -20120 -2671 -32026

ACC: -1149 805 9462
Gyr: -90 395 -54
Mag: -21661 -11509 -30152

ACC: -957 -38 10727
Gyr: -162 -188 133
Mag: -19166 -11965 -31825

ACC: -612 -344 10995
Gyr: -563 -44 -210
Mag: -15962 -9876 30139

ACC: -612 -1225 9846
Gyr: -243 100 61
Mag: -16008 -8220 29133

ACC: -1034 -2796 9539
Gyr: -269 29 48
Mag: -16218 -3800 27900

ACC: -1187 -3639 9309
Gyr: -268 40 0
Mag: -15258 449 27561

ACC: -1493 -4864 8926
Gyr: -394 35 37
Mag: -13936 4435 27411

ACC: -1110 -6167 7547
Gyr: -226 -73 16
Mag: -12848 10332 28129

ACC: -919 -7354 7356
Gyr: -143 1 -9
Mag: -12766 15214 29521

ACC: -306 -6397 8160
Gyr: 769 -201 -147
Mag: -13756 21369 -32721

---

Gestartet wird mit "ACC: 0 0 0", so fährt man erst los, wenn man mit dem Handy die Sensordaten sendet. Nun fehlt noch ein guter Mischer, denn das ist nicht so optimal:

Code:

---

                        OCR1A=pwm_l-ay/25-ax/25; // GAS!!! (Hier müssen die RP6-Besitzer die Motorbefehle einsetzen...
                        OCR1B=pwm_r-ay/25+ax/25; // ...die sie zum RP6 senden mit I2C wollen

---

Da ich zwei Fahrtregler aus dem Modelbau verwende, muss ich nur Servosignale erzeugen. Bei echten RP6-Fahrwerken (oder anderen PWM-gesteuerten Fahrwerken) muss man sich um die Umwandlung der Signale selbst kümmern:
https://www.roboternetz.de/community...l=1#post301774

Wenn der Mischer funktioniert wird's auch mal wieder ein Video geben ;)

Gruß

mic