Ich habe mal ein paar kleinere Änderungen gemacht. Ich habe das NICHT geprüft, weil ich die M32 nicht an die MultiIO angeschlossen habe:
Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_OrientationLib.h"
//#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_MultiIOLib.h"
#define I2C_RP6_BASE_ADR 10
uint8_t ch;
char item[12];
char dir[3];
//uint16_t new_dir; //neue richtung
//uint16_t old_dir; //gemessene richtung
//uint16_t temp; //berechnung korrektur richtung
int16_t new_dir; //neue richtung
int16_t old_dir; //gemessene richtung
int16_t temp; //berechnung korrektur richtung
/*
void I2C_requestedDataReady(uint8_t dataRequestID) //macht was?
{
checkRP6Status(dataRequestID);
}
*/
void calculateDir(char* dir, uint16_t heading) //setzt headingwerte grob in himmelsrichtungen um
{
dir[1] = ' ';
dir[2] = '\0';
if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';}
if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';}
if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';}
if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';}
}
void I2C_transmissionError(uint8_t errorState) //gibt I2C fehlermeldungen über LCD aus
{
clearLCD();
writeStringLCD_P("I2C ERROR -->");
setCursorPosLCD(1, 0); // line 2
writeStringLCD_P("TWI STATE: 0x");
writeIntegerLCD(errorState, HEX);
}
/*******************sensorwerte holen*************************/
void sensorwerte_holen(void)
{
intreg2dm = readHDMM01(); // holt sersorwerte
if(!intreg2dm) { //daten gültig? <=== KLAMMER AUF!!!
normalizeHDMM01(); // daten werden "normalisiert"
heading2dm = headingHDMM01(); // berechnung der headingwerte
} // <=== KLAMMER ZU!!!
}
/******************heading auf LCD schreiben**************/
void heading_ausgeben(void)
{
setCursorPosLCD(0, 0); //headingwerte werden auf LCD geschrieben
writeStringLCD_P("Heading ");
writeIntegerLCD(heading2dm, DEC);
}
/*******************berechnung der richtungswerte***********/
void richtung_berechnen(void)
{
calculateDir(dir, heading2dm); //berechnung der richtung (N,S,W,E)
}
/**************ausgabe der richtungswerte*******************/
void richtung_ausgeben(void)
{
setCursorPosLCD(1, 0);
writeStringLCD_P("direction ");
writeStringLCD(dir); //ausgabe der richtung
}
int main(void)
{
initRP6Control();
multiio_init();
initLCD();
orientation_init();
//COMPASS_2D_init();
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
I2CTWI_initMaster(100);
//I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);//macht was?
I2CTWI_setTransmissionErrorHandler(I2C_transmissionError); //aktiviert I2C fehlermeldungen
showScreenLCD(" RP6Control M32", " gyro_quadrat");
mSleep(2500);
clearLCD();
while(true)
{
/*****************anzeige gedrückter buttons****************/
clearLCD();
pressedMultiIOButtonNumber = getMultiIOPressedButtonNumber();
setCursorPosLCD(0, 0);
writeStringLCD("Button: ");
writeIntegerLCD(pressedMultiIOButtonNumber, DEC);
mSleep(500);
// uint8_t key = getMultiIOPressedButtonNumber();
uint8_t key = pressedMultiIOButtonNumber;
/********************funktion der buttons*********************/
if(key)
{
switch(key)
{
case 1://richtung NORD
setLEDs(0b0001); //LED 1 leuchtet
{
new_dir = 257;
sensorwerte_holen();
heading_ausgeben();
old_dir = heading2dm;
setCursorPosLCD(1, 0);
writeStringLCD_P("dir ");
writeIntegerLCD(old_dir, DEC);
writeStringLCD_P(" ");
writeIntegerLCD(new_dir, DEC);
mSleep(1000);
}
// move(150, FWD, DIST_MM(500), true); //fährt 50cm
mSleep(1000);
// rotate(50, 3, 90, 1); //dreht um 90°
clearLCD();
break;
case 2:
setLEDs(0b0010);
break;
case 3:
setLEDs(0b0100);
break;
case 4:
setLEDs(0b1000);
break;
}
}
}
return 0;
}
Schau dir mal die Änderungen an:
1. RP6Control_I2CMasterLib nicht eingebunden
2. Variablen new_dir, old_dir und temp als int16_t
3. I2C_requestedDataReady weg
4. In sensorwerte_holen() KLAMMERN ergänzt
5. COMPASS_2D_init() weg (wird in der Lib aufgerufen)
6. I2CTWI_setRequestedDataReadyHandler() weg
7. getMultiIOPressedButtonNumber() nicht 2x aufgerufen, weil die Funktion nur EINMAL die gedrückte Taste ausgibt
Du must gucken, ob das jetzt ok ist. Ein allg. Problem ist sicher, dass die Ausgabe nur 1x erfolgt, nachdem du gedrückt hast. Vielleicht änderst du das z.B. noch so, dass die Ausgabe erfolgt, SOLANGE keine andere Taste als die 1 gedrückt wurde.
Um die Rotation zu errechnen, um von old_dir zu new_dir zu kommen, habe ich jetzt auch eine passende Formel:
Code:
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
int16_t dev, rot;
dev = new_dir - old_dir;
rot = dev;
if (abs(dev) > 180) {
if (dev < 0) {
rot = 360 + dev;
}
else {
rot = -360 + dev;
}
}
Der Drehwinkel zu new_dir ist danach in rot, wobei positive Werte für Rechtsdrehung stehen und negative für Linksdrehung.
Du must also nur den Absolutwert von rot an die rotate-Funktion übergeben und je nach Vorzeichen links oder rechts drehen lassen.
Teste das mal!
Zitieren
Zitat von Dirk
Lesezeichen