-         

Ergebnis 1 bis 2 von 2

Thema: RP6 WIFI und Wireless IMU (für Android)

  1. #1
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8

    RP6 WIFI und Wireless IMU (für Android)

    Anzeige

    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

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

  2. #2
    Moderator Robotik Visionär Avatar von radbruch
    Registriert seit
    27.12.2006
    Ort
    Stuttgart
    Alter
    54
    Beiträge
    5.781
    Blog-Einträge
    8
    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
    // http://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:
    http://www.roboternetz.de/community/...l=1#post301774

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

    Gruß

    mic

    Atmel’s products are not intended, authorized, or warranted for use
    as components in applications intended to support or sustain life!

Ähnliche Themen

  1. RP6 Verbindung zwischen Android App und RP6 via Wifi
    Von Dennis Stolp im Forum Robby RP6
    Antworten: 4
    Letzter Beitrag: 05.03.2014, 11:31
  2. RP6 - M256 WIFI Modul
    Von markus788 im Forum Robby RP6
    Antworten: 1
    Letzter Beitrag: 26.05.2013, 17:45
  3. RP6 M256 WIFI: Clock Library
    Von Dirk im Forum Robby RP6
    Antworten: 0
    Letzter Beitrag: 31.07.2012, 22:55
  4. Antworten: 3
    Letzter Beitrag: 28.07.2012, 19:43
  5. [ERLEDIGT] Kostenlose RP6 M256 WIFI Module für Testanwender!
    Von SlyD im Forum Robby RP6
    Antworten: 10
    Letzter Beitrag: 02.07.2012, 16:24

Stichworte

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •