also, das programm tut "im großen und ganzen" in den bereichen die mir wichtig waren was es soll. Ich habe den ganzen code hier noch einmal kopiert, weil einzelne abschnitte sicher nicht so übersichtlich wären, es ist sicher noch eine ganze menge "müll" drinnen, auch viel zu viele "mSleeps", aber ich wollte mitlesen auf dem display...
Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_OrientationLib.h"
//#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#define I2C_RP6_BASE_ADR 10
uint8_t ch;
char item[12];
char dir[3];
int32_t new_dir; //neue richtung
int32_t old_dir; //gemessene richtung
int32_t temp; //berechnung korrektur richtung
int16_t dev, rot, i;//berechnung rotation
char rdir;
/***********************calculate_dir**********************/
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?
{
normalizeHDMM01(); // daten werden "normalisiert"
heading2dm = headingHDMM01(); // berechnung der headingwerte
}
}
/*******************berechnung der richtungswerte***********/
void richtung_berechnen(void)
{
calculateDir(dir, heading2dm); //berechnung der richtung (N,S,W,E)
}
/*************korrektur richtung*****************************/
void korrekrur_richtung(void)
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
{
dev = new_dir - old_dir;
rot = dev;
if (abs(dev) > 180)
{
{
if (dev < 0)
{
rot = 360 + dev;
}
else
{
rot = -360 + dev;
}
}
}
}
/*******************werte schreiben************************/
void werte_schreiben(void)
{
setCursorPosLCD(0, 0);
writeStringLCD_P("HEA ODR NDR ROT");
old_dir = heading2dm;
setCursorPosLCD(1, 0);
writeIntegerLengthLCD(heading2dm, DEC, 3);
setCursorPosLCD(1, 4);
writeIntegerLengthLCD(old_dir, DEC, 3);
setCursorPosLCD(1, 8);
writeIntegerLengthLCD(new_dir, DEC, 3);
korrekrur_richtung();
if (rot<0)
{
setCursorPosLCD(1, 12);
writeStringLCD_P("-");
}
else
{
setCursorPosLCD(1, 12);
writeStringLCD_P(" ");
}
setCursorPosLCD(1, 13);
writeIntegerLengthLCD(abs(rot), DEC, 3);
}
/**********************test rdir**************************/
void test_rdir(void)
{
if (rot < 0)
rdir = LEFT;
else rdir = RIGHT;
}
/**********************hauptprogramm*********************/
int main(void)
{
initRP6Control();
multiio_init();
initLCD();
orientation_init();
setLEDs(0b1111);
mSleep(500);
setLEDs(0b0000);
I2CTWI_initMaster(100);
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();
/********************funktion der buttons*********************/
if(key)
{
switch(key)
{
case 1://richtung NORD
setLEDs(0b0001); //LED 1 leuchtet
while(true)
{
{
new_dir = 360;
sensorwerte_holen();
werte_schreiben();
mSleep(1000);
korrekrur_richtung();
test_rdir();
werte_schreiben();
mSleep(1000);
rotate(60, rdir, ((abs(rot)/2)), 0);
werte_schreiben();
mSleep(1000);
// if ((rot < 2) || (rot > -2)) break;
// if (rot == 0) break;
for (i= 0; i<3; i++);
{
sensorwerte_holen();
werte_schreiben();
mSleep(1000);
korrekrur_richtung();
test_rdir();
werte_schreiben();
mSleep(1000);
}
uint8_t key = getMultiIOPressedButtonNumber();
if (key)
{
break;
}
}
}
break;
case 2:// richtung OST
setLEDs(0b0010);
break;
case 3:// richtung SÜD
setLEDs(0b0100);
break;
case 4:// richtung WEST
setLEDs(0b1000);
break;
}
}
}
return 0;
}
erste frage:
der RP6 arbeitet sich durch den halbierten "rotate-satz" recht langsam an sein ziel, hier die nordrichtung, heran, tänzelt dann so in meistens immer kleineren schritten hin und her auf die "0" hin, die drehungen fallen aber auch manchmal völlig unerwartet größer aus. So werden aus 3° plötzlich wieder 7° - und ich weiss nicht warum. Liegt das nur an der ungenauigkeit des getriebes?
zweite frage:
er schafft auch schon mal die "0" zu erreichen, die erscheint dann auch im display, statt aber - wie ihm das "break" (jetzt auskomentiert) eigentlich befehlen sollte - sofort stehen zu bleiben, dreht er noch um 7, manchmal aber um bis zu 9° nach rechts. Und das ist verifizierbar...
Da bin ich auch noch am rätseln welche wariable mir dort einen streich spielt...
ansonsten muss ich wieder die arbeit an der platine und der lib loben, an mein "gebastle" in der vergangenheit darf ich gar nicht denken. Sauber...
Zitieren
Lesezeichen