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:
Zitieren
Lesezeichen