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:
// 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(__li ne2)), (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:
[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 -32721Gestartet 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:
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 (http://www.conrad.de/ce/de/product/207369/Modelcraft-B4230-Carbon-Series-Fahrtregler-Belastbarkeit-54-A-35-A-25-A-Motorlimit-20-Turns-380er-Motoren) 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/threads/31931-Domino-Day-f%C3%BCr-den-RP6?p=301774&viewfull=1#post301774
Wenn der Mischer funktioniert wird's auch mal wieder ein Video geben ;)
Gruß
mic