Wie kann ich mit i2c-Sensoren für Nibo2 kommunizieren?

[Klausi49]

Hallo zusammen,

ich bin hier neu im Forum, weil ich dringend eure Hilfe benötige!!!

Ich besitze einen Nibo2-Roboter (ja, den gibt es immer noch!) und habe schon recht viel mit ihm angestellt und durch ihn gelernt.
Jetzt wage ich den nächsten Schritt:
Ich kaufte mir einen GY-271 DA5883 Kompassmodul - 3-Achse Magnetometer Sensor und baute diesen in meinen Nibo2 ein.
So weit, so gut. Doch es gelingt mir beim besten Willen nicht, mit diesem über das i2c-Protokoll zu kommunizieren.
Mir stehen aus der Nibo2-Bibliothek die folgenden Befehle zur Verfügung:

i2cmaster.h-Dateireferenz

Routinen zur Kommunikation ueber den I2C Bus.
#include <stdint.h>

Makrodefinitionen
#define I2C_TX(ADDR) ((ADDR)<<1)

#define I2C_RX(ADDR) (((ADDR)<<1)+1)

Aufzählungen
enum { I2C_BUSY, I2C_IDLE, I2C_SUCCESS, I2C_ERROR =0x10 }

Funktionen
void i2c_init (void)

uint8_t i2c_start_transmission (void)

uint8_t i2c_status (void)

uint8_t i2c_wait_transmission (void)

Variablen
volatile uint8_t i2c_size

uint8_t i2c_buf [I2C_BUF_SIZE]

volatile uint8_t i2c_last_TWSR

volatile uint8_t i2c_pos


Daher meine Bitte: Wer kann mir mit einem konkreten Code-Beispiel helfen, mittels i2c die Daten aus dem Sensor in den Roboter zu bekommen?

Vielen Dank schon im Voraus!
Gruß
Klaus

[HaWe]

hallo!
ich würde den Nibo2 in der Arduino-IDE programmieren, da hast du sehr viele praktische Libs zur Verfügung mit einfacher Ansteuerung (ebenfalls in C/C++):
https://www.roboter.cc/index.php?option=com_kunena&view=topic&catid=13&id=2333&Itemid=20

wenn du diese verwendest und Fragen hast, stell ggf. nochmal die Frage neu hier im Arduino-Unterforum!

[Klausi49]

Hallo HaWe,
vielen Dank für deine schnelle Antwort.
Ich habe bisher immer das AVRStudio4 verwendet und komme gut klar damit.
Wenn ich nun weiter mit der Arduino-IDE programmiere, muss ich ja wohl alle meine Codes wieder neu erstellen, oder?!?
Ich will doch den Gyro-Sensor in ein bestehendes Projekt einbinden.
Gibt es nicht die Möglichkeit, die i2c-Routinen, die die Nibo2-Bibliothek mir bietet, zu nutzen?
Gruß
Klaus

[021aet04]

Wenn du auf die Arduino IDE umsteigst, musst du die Programme neu schreiben oder zumindest anpassen. Ich weiß nicht ob man C Programme importieren kann.

Die Library kenne ich nicht, aber du könntest versuchen auf eine andere I2C Library umsteigen. Eine häufig verwendete Library ist die von Peter Fleury.
die "i2cmaster" Library von hier http://www.peterfleury.epizy.com/avr-software.html#libs

Wichtig ist das du Pullup Widerstände zwischen VCC (+5V) und SCL bzw VCC und SDA hast. Entweder du nimmst externe Widerstände oder schaltest die internen Pullup Widerstände ein.
Ohne Widerstände funktioniert der Bus nicht.

Zusätzlich ist noch der Ablauf wichtig.
Die Unterscheidung zwischen Lesen und Schreiben wird mit dem niederwertigsten Bit bei der Adresse angegeben. Eine "0" bedeutet das du schreiben willst, eine "1" bedeutet lesen.
https://www.roboter-bausatz.de/media/pdf/ca/fc/5a/QMC5883L-Datasheet-1-0.pdf
Wenn das dieses IC ist hast du die I2C Grundadresse "0D" => 0001101
Somit hast du als Schreibadresse 00011010 => 1A
und die Leseadresse 00011011 => 1B
Hier der Ablauf in Pseudocode.

Beim Schreiben:

I2C Start
I2C Schreibadresse
I2C Register (im Datenblatt des ICs nachschauen)
I2C Daten (Daten die du ins Register schreiben willst, z.B. Einstellungen)
I2C Stopp

Beim Lesen:

I2C Start
I2C Schreibadresse
I2C Register (Register aus dem du lesen willst)
I2C Start (bzw repeated start)
I2C Leseadresse
Daten in Variable lesen mit ACK => wenn weitere Daten gelesen werden sollen, Registeradresse wird automatisch um 1 erhöht
Daten in Variable lesen mit NACK => wenn keine weiteren Daten gelesen werden sollen
I2C Stopp

Du hast oben Makrodefinitionen angegeben (aus der Headerdatei). Dort steht z.B. "#define I2C_TX(ADDR) ((ADDR)<<1)".
In diesem Fall müsstest du bei "ADDR" die "0D" angeben und nicht "1A" bzw "1B", weil dieses Makro das Linksschieben übernimmt.

MfG Hannes

[Klausi49]

Hallo Hannes,
toll, dass du dich um mein Problem bemühst! Danke.
Ja, das istder IC, den ich gekauft habe, und auch von diesem Shop.
Das mit dem Links-Schieben wusste ich nicht. Bin eben ein Neuling auf diesem Gebiet.
Ich habe folgenden Beispielsketch für Arduino Kompassmodul GY-282 HMC5983 gefunden:

#include <Wire.h> //I2C Arduino Library
#define addr 0x1E //I2C Address for The HMC5883
void setup(){
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(addr); //start talking
Wire.write(0x02); // Set the Register
Wire.write(0x00); // Tell the HMC5883 to Continuously Measure
Wire.endTransmission();
}
void loop()
{
int x,y,z; //triple axis data
//Tell the HMC what regist to begin writing data into
Wire.beginTransmission(addr);
Wire.write(0x03); //start with register 3.
Wire.endTransmission();
//Read the data.. 2 bytes for each axis.. 6 total bytes
Wire.requestFrom(addr, 6);
if(6<=Wire.available()){
x = Wire.read()<<8; //MSB x
x |= Wire.read(); //LSB x
z = Wire.read()<<8; //MSB z
z |= Wire.read(); //LSB z
y = Wire.read()<<8; //MSB y
y |= Wire.read(); //LSB y
}
// Show Values
Serial.print("X Value: ");
Serial.println(x);
Serial.print("Y Value: ");
Serial.println(y);
Serial.print("Z Value: ");
Serial.println(z);
Serial.println();
delay(500);
}


Könnte man diesen Code nicht so umschreiben,
dass er mit den Befehlen der Nibo2-Bibliothek (wie im 1. Post beschrieben)
ausführbar ist?!?
Würdest du mir dabei bitte auch behilflich sein?
Gruß
Klaus

[021aet04]

Das ist ein Code der für die Arduino IDE, der ist so nicht kompatibel mit dem AVR Studio.
Dort wird die "wire.h" verwendet, die ist Teil der Arduino IDE.

Der Code, den du gepostet hast, liest die Werte der 3 Achsen aus und sendet es mittels UART an die USB Schnittstelle (über UART/USB Wandler).
Die Adresse ist auch eine andere, warum weiß ich aber nicht.

Die Nibo Library kenne ich überhaupt nicht. Die Lib die ich gepostet habe habe ich zu Testzwecken schon verwendet, habe aber noch nicht viel gemacht.

Ich würde die von mir gepostete Library herunterladen und diese verwenden. Die ist relativ leicht zu bedienen.

Wenn du Code postest, bitte in Code Tags posten. Entweder unter "Erweitert" und dann das #-Symbol oder du schreibst den Code zwischen [ code] und [ /code] (du musst die Leerzeichen entfernen).

Beim Code selbst kann ich dir aber helfen, programmiere zwar mittlerweile mit dem aktuellen AVR Studio, habe aber mit der Version 4 angefangen.

MfG Hannes

[Klausi49]

Hallo Hannes,
zum letzten Mal heute Abend:
Habe die Library von Peter Fleury heruntergeladen und entpackt.
Doch wohin nun mit den einzelnen Dateien? Habe in die Anleitung geschaut,
aber das sieht ja nun wieder ganz anders aus als in der Nibo-Bibliothek oder bei Arduino.
Da werde ich mir wohl oder übel noch die letzten Haare raufen müssen...
Aber ich will das Ganze auf meine alten Tage unbedingt noch lernen,
dann kann ich mein Wissen auch noch an meinen Enkel weitergeben,
der genau wie ich roboterbesessen ist.
Gruß
Klaus

[021aet04]

Du benötigst nur 2 Dateien, einmal die "i2cmaster.h" und die "twimaster.c".

Diese beiden Dateien würde ich in den Projektordner kopieren (in der die C-Datei mit der "main" liegt).

Wenn du dann das Projekt öffnest gibt es im Projektbaum (der Abschnitt mit den Ordnern "Source Files", "Header Files",...) den Ordner mit den verwendeten Source Files, dort musst du die C-Datei hinzufügen. Rechte Maustaste auf den Ordner "Source Files" und dann "Add existing Source File(s)". Jetzt wählst du die "twimaster.c" aus.
In dem Ordner muss jetzt die C-Datei mit der "main" sein und die "twimaster.c" (eventuell noch mehrere C-Dateien).

Anschließend musst du noch die Headerdatei integrieren. Es gibt 2 verschiedene Arten Headerdateien zu integrieren.
1) die Datei wird in den Symbolen < und > geschrieben (z.B. #include <avr\interrupt.h>) => die Dateien sind in einem Ordner die dem AVR Studio bekannt sind, wie die Standartmäßigen Headerdateien.
2) die Datei wird zwischen 2 "-Symbolen geschrieben (z.B. #include "i2cmaster.h") => die Dateien liegen im Projektordner

Wenn du das Projekt dann übersetzt sollte unter "External Dependencies" die Headerdatei (in dem Fall die i2cmaster.h) zu finden sein.

Wenn du soweit bist kannst du die einzelnen Funktionen der Library nutzen.
Am Programmanfang musst du 1x den TWI initialisieren (damit die Bitrate richtig eingestellt ist). Hier würde ich auch die Einstellung für das IC machen.
In der Loop kannst du dann die Werte auslesen.

Ich hoffe das hilft dir weiter.

MfG Hannes

[Klausi49]

Hallo Hannes,
auf ein Neues!!!
Ich habe deine Hinweise beachtet und die beiden Dateien eingebunden. Dann habe ich mein kleines Projekt geschrieben. Es wurde fehlerfrei compiliert. Allerdings liest der Nibo2 nicht die Werte des Gyros.
Ich dachte mir, vielleicht liegt es daran, dass die vorher vorhandenen i2c-Routinen für den eingebauten Abstandsmesser nds3 nicht mehr vorhanden sind. Habe also folglich die alten i2c-Dateien wieder eingebunden (wie es vorher war) und die neuen i2c-Dateien namenlich angepasst (i2c__master.h und die Routine i2c__init), damit nichts kollidiert.
Es wird fehlerfrei compiliert, aber in der Textausgabe zum Schluss kommt nichts an. Wenn ich den Zugriff auf den Sensor auskommentiere (siehe Code), dann wird die Textausgabe (stellvertretend mit x=5, y=6, z=7) geschrieben.
Nun bin ich mit meinem Latein wieder mal am Ende und bitte dich um Hilfe.



/* Nibo2-Programm Test_Gyro_I2C, erstellt von Klaus Beutel im Februar 2020 */
/********************** Bibliotheken einbinden ************************************************** ****/
#include <math.h>
#include <stdio.h> // Standardein- und -ausgabefunktionen
#include <avr/interrupt.h> // Interrupts ermöglichen
#include <stdlib.h> // notwendig u.a. für die abs-Funktion
#include <nibo/niboconfig.h> // Konfiguration der Nibo-Bibliothek
#include <nibo/display.h> // Ansprechen eines Displays
#include <nibo/gfx.h> // Ansteuerung eines LC-Grafik-Displays
#include <nibo/spi.h> // Kommunikation über SPI.
#include <nibo/copro.h> // Ansprechen des Coprozessors
#include <nibo/delay.h> // Warteschleifen
#include <nibo/iodefs.h> // Zuordnung der physikalischen Pins zu symbolischen Namen
#include <nibo/bot.h> // allgemeine Funktionen des Roboters
#include "i2c__master.h" // Kommunikation ueber den I2C Bus
#include <nibo/pwm.h> // Erzeugung der PWM-Signale und der Systemzeit.
#include <nibo/leds.h> // Ansteuerung der LEDs
#include <nibo/i2cmaster.h> // Kommunikation ueber den I2C Bus
#include <nibo/nds3.h> // Kommunikation mit dem NDS3 Modul

/************************* Konstante definieren ************************************************** ***/
#define gyroaddr 0x0D // i2c-Adresse für HMC5883

/************************* Globale Variable definieren **********************************************/
char text[22] = " "; // Text-Ausgabe-Buffer
uint16_t x,y,z; // triple axis data

/************************* Hauptprogramm ************************************************** **********/
int main()
{
/********************** Initialisierung ************************************************** ***********/
sei(); // Interrupts global erlauben
bot_init(); // Controller reseten
leds_init(); // LEDs initialisieren
pwm_init(); // PWM-Kanäle initialisieren
display_init(2); // Display initialisieren
spi_init(); // SPI initialisieren
gfx_init(); // Graphikdisplay initialisieren
i2c_init(); // i2c-Bus initialisieren
nds3_init();
i2c__init();

/********************* Lokale Variable definieren ************************************************** */

/************************************************** **************************************************/
nds3_move(82); // nds3 auf Null-Position
leds_set_displaylight(1024); // Display-Hintergrundsbeleuchtung einschalten
gfx_set_proportional(0); // Schrift auf normal setzen
gfx_move(0, 50);
gfx_print_text("S3: Start");

while (PIND&(1<<4)) // warten auf Betätigung von S3 zum Starten
{
delay(250); // alle grünen LEDs blinken
PORTC=0xFF;
delay(250);
PORTC=0x00;
}

gfx_fill(0x00); // Display leeren
gfx_move(0, 0);
gfx_print_text(" x y z");

/* i2c_start(gyroaddr+I2C_WRITE); // set device address and write mode
i2c_write(0x09); // write address = 9
i2c_write(0x1D); // Define OSR = 512, Full Scale Range = 8 Gauss, ODR = 200Hz, set continuous measurement mode)
i2c_write(0x0A); // write address = 10
i2c_write(0x40); // The I2C data pointer will automatically roll, 6.Bit = 1 setzen
i2c_write(0x0B); // write address = 11
i2c_write(0x01); // Define Set/Reset period
i2c_stop(); // set stop conditon = release bus
*/
while (1) // Hauptschleife
{
/* i2c_start_wait(gyroaddr+I2C_WRITE); // set device address and write mode
i2c_write(0x00); // write address = 0
i2c_rep_start(gyroaddr+I2C_READ); // set device address and read mode
x = i2c_readAck()<<8; // read one byte from address 0 MSB x
x |= i2c_readAck(); // " " " " " 1 LSB x
y = i2c_readAck()<<8; // " " " " " 2 MSB y
y |= i2c_readAck(); // " " " " " 3 LSB y
z = i2c_readAck()<<8; // " " " " " 4 MSB z
z |= i2c_readNak(); // " " " " " 5 LSB z
i2c_stop(); // set stop condition = release bus
*/ x=5;
y=6;
z=7;
gfx_move(0, 10);
sprintf(text,"-> %04d %04d %04d", x, y, z);
gfx_print_text(text);

}
return 0;
}


Gruß
Klaus

P.S.: Die Adressen habe ich aus dem Datenblatt des IC entnommen. Vielleicht liegt ja da der Fehler?!?

[021aet04]

Man sollte nur eine Lib verwenden und nicht 2 oder noch mehr. Es könnte sein das sich die beiden Libs beeinflussen. Z.B. Lib 1 stellt in der Init die I2C Frequenz auf 100kHz und Lib 2 stellt es dann auf 400kHz.

Das Problem ist das ich nicht herausgefunden habe welche Adresse das NDS3 hat. Dadurch benötigst du die originale I2C Lib.

Das Problem das du aber mit dem Sensor hast ist ein anderes, du hast die falsche Adresse.

Die Grundadresse ist die 0D, das ist aber nicht Bit 0-Bit 6 sondern Bit 1-Bit 7.

Du musst die Adresse zuerst um 1 nach links schieben und dann 0 oder 1 an Bit 0 hinzufügen (vergleiche es mit der Makrofunktion im 1ten Post). Das geht entweder mit einer Addition oder einer Oder Verknüpfung. Achte auf die Adresse die ich im Post 4 geschrieben habe.
Ich empfehle dir die Rechnung mit dem Windowsrechner zu prüfen, dort gibt es einen Punkt "Programmierer".

Was mir auch noch aufgefallen ist, die Bytes sind verdreht.
Im Arduinocode und auch in deinem steht im 1ten Byte das MSB und im 2ten Byte das LSB, laut Datenblatt ist aber im 1ten Byte das LSB und im 2ten Byte das MSB.
Um das kann man sich aber dann später kümmern, wichtig ist aber das du erst einmal Daten bekommst.

MfG Hannes

[Klausi49]

Hallo Hannes,
große Freude - ich kann den Sensor lesen!!! Habe deine Ratschläge beachtet, funktioniert so gut. Dass das MSB und das LSB im Datenblatt des Sensors in der umgekehrten Rehenfolge steht und nicht wie bei dem Arduino-Scetch-Beispiel, war mir gar nicht aufgefallen. Also nochmals vielen Dank für deine Hilfe!
So, nun gehts weiter, nun muss ich mir etwas einfallen lassen, wie ich aus den Sensorwerten die Gradzahl des Winkels bekomme.
Mein Vorhaben ist, wenn der Nibo2 im Labyrinth eine 90°-Drehung nach links machen soll, dann soll er das so lange machen, bis der Kompass-Sensor sagt, dass er die 90° erreicht hat. Ohne den Sensor, nur über die Umdrehungszahl der Motoren, wird alles zu ungenau, weil der Nibo2 auf dem Untergrund auch rutscht.
Übrigens bin ich auch deiner Meinung, dass es ungünstig ist, den i2c-Bus über mehrere Libs anzusprechen, die sich stören könnten.
Ich denke mir, es müsste doch möglich sein, die Routinen aus der i2cmaster.h-Datei, die du mir empfohlen hast, mit in die originale i2cmaster.h-Datei des Nibo2 zu übernehmen. Beim Syntax dürften die sich doch eigentlich nicht stören, so dass der nds3 weiter funktioniert und ich aber meine zusätzlichen Sensoren mit den neuen Routinen anspreche. Der Befehl i2c_init() wird dann natürlich nur einmal gegeben.
Was hältst du von meiner Idee? Oder hast du einen besseren Vorschlag?
Ich wünsche dir noch einen schönen Abend.
Gruß
Klaus

[021aet04]

Schön das es jetzt funktioniert.

Ich würde mich auf eine Lib beschränken.

Entweder versuchen den Kompass mit der Nibo Lib oder den NDS3 (und andere Komponenten falls vorhanden) mit der Lib von Peter Fleury.

Die Adresse des NDS3 könntest du versuchen mit dem Simulator herauszufinden.
Du startest den Simulator mit einem Programm das den NDS3 anspricht (z.B. Entfernung auslesen). Diesen Programmteil lässt du dann Schritt für Schritt abarbeiten. Die Initialisierung kannst du überspringen, indem du vor dem Kommando klickst (rechte Maustaste) und dann wählst du "Run to Cursor".
Wenn der gelbe Pfeil dort angezeigt wird, drückst du F11 (Step Into) und schaust was in das TWI Datenregister geschrieben wird.
Wenn du die vermeintliche Adresse hast könntest du es mit der Peter Fleury Lib testen, die einzelnen Kommandos und das Protokoll sind hier zu finden http://www.nibo-roboter.de/wiki/NDS3
Du könntest dann eine eigene Library schreiben, die diese Funktionen enthält. Also eine eigene "NDS3.h" erstellen.

Ich würde die Peter Fleury Lib nehmen, da du diese auch für andere Projekte (also wenn du den Nibo nicht verwendest) verwenden kannst. Die Nibo Lib ist speziell auf den Nibo zugeschneidert und ist dadurch nur schwer für andere Dinge zu gebrauchen.

Ich wünsche dir ebenfalls einen schönen Abend
MfG Hannes

[Klausi49]

Hallo Hannes,
will mich mal wieder melden. Ich war nicht untätig, habe den Kompass-Sensor nun in mein eigentliches Nibo2-Programm eingebaut.
Und - es funktioniert wirklich, dass der Roboter, wenn er um 90° nach links drehen soll, da auch tut. Und das mit deutlich besserer Genauigkeit als vorher, wo ich sozusagen nur die Umdrehungszahl der Räder als Maß hatte, ob der Roboter nun gerutscht ist oder nicht, also ob er seine Zielrichtung erreicht hat oder nicht.
Jetzt kommt aber ein ABER - aber komischerweise klappt das 2 bis 3 mal gut im Labyrinth, und plötzlich hört er nach 90° nicht auf, macht mehrere volle 360°-Drehungen, um dann anschließend wieder ordentlich zu reagieren.
Ich weiß beim besten Willen nicht, woran das liegt!?! Ob das Probem am Lesen der Sensordaten über den i2c-Bus liegt?
Schau bitte nochmals auf meine Routine, die in der while-Hauptschleife liegt:


/********************** Registereinstellungen für Kompass-Sensor ************************************/
void kompass_init()
{
i2c_start(KOMPASS_ADR+I2C_WRITE); // set device address and write mode
i2c_write(0x09); // write address = 9
i2c_write(0x1D); // Define OSR = 512, Full Scale Range = 8 Gauss, ODR = 200Hz, set continuous measurement mode)
i2c_write(0x0A); // write address = 10
i2c_write(0x40); // The I2C data pointer will automatically roll, 6.Bit = 1 setzen
i2c_write(0x0B); // write address = 11
i2c_write(0x01); // Define Set/Reset period
i2c_stop(); // set stop conditon = release bus
}

/********************** Kommpasswerte lesen ************************************************** *******/
int16_t kompass_lesen()
{
int16_t winkel;
i2c_start_wait(KOMPASS_ADR+I2C_WRITE); // set device address and write mode
i2c_write(0x00); // write Register = 0 = welcher Wert aus welchem Register soll zuerst gelesen werden
i2c_rep_start(KOMPASS_ADR+I2C_READ); // set device address and read mode
x = i2c_readAck(); // read one byte from address 0 LSB x
x |= i2c_readAck()<<8; // " " " " " 1 MSB x (um 8 Bit nach links verschieben und addieren)
y = i2c_readAck(); // " " " " " 2 LSB y
y |= i2c_readAck()<<8; // " " " " " 3 MSB y (um 8 Bit nach links verschieben und addieren)
z = i2c_readAck(); // " " " " " 4 LSB z
z |= i2c_readNak()<<8; // " " " " " 5 MSB z (um 8 Bit nach links verschieben und addieren)
i2c_stop(); // set stop condition = release bus
winkel = atan2 (-y, x) * 180 / PI;
if (winkel < 0)
{
winkel += 360;
}
deklination = (3 + (33 / 60)) / (180 / PI); // Deklination für Gernrode (+3° 33')
winkel += deklination;
return winkel;
}

Im Datenblatt des Sensors lese ich:

 Check status register 06H[0] ,”1” means ready.
 Read data register 00H ~ 05H.

Vielleicht muss ich ja das Register 06H checken, ob das 0. Bit gesetzt ist?!?
Kannst du mir erklären, wie ich dies in meiner Routine realisieren kann?
Oder hast du evtl. noch eine andere Idee?

Viele Grüße aus dem Harz sendet
Klaus

[021aet04]

Ich würde zuerst das Statusregister lesen. Dieses hat 3 Bits (Bit 0 - Bit 2). Bit 0 bedeutet neue Daten vorhanden. Das 2te Bit ist auch sehr wichtig, es sagt dir ob eine oder mehrere Achsen überlastet sind (magnetische Feld zu groß). Das 3te Bit ist nicht so wichtig, es sagt nur das du einen Messwert übersprungen hast (nicht ausgelesen).

Ich würde also das Statusbyte lesen und nur wenn Daten vorhanden sind (Bit 0 = 1) und du keine Achse überlastet hast (Bit 1 = 0) die Sensordaten auslesen. Wenn Bit 0 = 0 oder Bit 1 = 1 einfach die Messwerte ignorieren.


Was mir auch noch auffällt. Du lässt deinen Ort mit in die Berechnung einfließen, warum? Du benötigst keine Absoluten Winkel sondern nur einen relativen zum vorhergehenden.

MfG Hannes

[Klausi49]

Hallo Hannes,
irgendwie habe ich mir das alles einfacher vorgestellt mit meinem Kompass-Sensor:
Wenn eine Drehung durchgeführt werden soll, wird zuerst der augenblickliche Startwinkel ermittelt, dann der gewünschte Drehwinkel berücksichtigt und dann der Zielwinkel berechnet. Während der Drehung wird ständig der aktuelle Winkel ermittelt, und wenn der gleich dem Zielwinkel ist, wird die Drehung beendet.
Soweit, so gut - in der Theorie. Praktisch klappt das mal, mal nicht. Ich fand heraus, dass beim Ermitteln des aktuellen Winkels immer mal wieder ein Wert ausgelassen wrd. Und wenn das zufällig der gewünschte Zielwinkel ist, habe ich eben Pech!
Hier ist mein Code:


/********************** Kommpasswerte lesen ************************************************** *******/
void kompass_lesen()
{
int8_t status;
i2c_start_wait(KOMPASS_ADR+I2C_WRITE); // set device address and write mode
i2c_write(0x06); // Statusregister 06H setzen
i2c_rep_start(KOMPASS_ADR+I2C_READ); // set device address and read mode
do{ // warten, bis Statusregister Bit0 = 1 ist, dann Winkeldaten lesen
delay(20);
status = i2c_readNak() && 1; // Statusregister lesen
}while (status == 0);
i2c_start_wait(KOMPASS_ADR+I2C_WRITE); // set device address and write mode
i2c_write(0x00); // write Register = 0 = welcher Wert aus welchem Register soll zuerst gelesen werden
i2c_rep_start(KOMPASS_ADR+I2C_READ); // set device address and read mode
x = i2c_readAck(); // read one byte from address 0 LSB x
x |= i2c_readAck()<<8; // " " " " " 1 MSB x (um 8 Bit nach links verschieben und addieren)
y = i2c_readAck(); // " " " " " 2 LSB y
y |= i2c_readNak()<<8; // " " " " " 3 MSB y (um 8 Bit nach links verschieben und addieren)
i2c_stop(); // set stop condition = release bus
winkel_aktuell = atan2 (-y, x) * 180 / PI;
if (winkel_aktuell < 0) // Winkelangabe in den Bereich 0° bis 360° bringen
{
winkel_aktuell += 360;
}
}

/************************* Soll-Drehwinkelberechnung ************************************************/
void drehwinkel_soll(int16_t winkel)
{
kompass_lesen(); // winkel_aktuell gibt die augenblickliche Fahrtrichtung an
winkel_start = winkel_aktuell; // winkel_start berechnen
winkel_ziel = winkel_start + winkel; // winkel_ziel berechnen
if (winkel_ziel < 0) // Winkelwert in Bereich 0° bis 360° bringen
{
winkel_ziel += 360;
}
else if (winkel_ziel > 360)
{
winkel_ziel -= 360;
}
}

Ich habe es auch ohne delay(20) in der kompass_lesen()-Routine versucht, bringt aber auch nichts.
Mache ich etwas verkehrt? Hast du wieder einen Tipp für mich, vielleicht auch Erfahrungswerte?
Bin gespannt.
Gruß
Klaus

[021aet04]

Deine Kompass Lesefunktion funktioniert glaube ich nicht so wie du willst. Speziell die do/while Schleife.

Vergleiche mein I2C Lese Ablauf von Post 4 und deinen Ablauf. Du ließt den Status aus (nach dem delay(20)), verwendest aber ein NACK, danach sollte aber ein I2C-Stopp kommen. Ein NACK bedeutet das keine weiteren Daten gelesen werden, du ließt aber danach (nach 20ms) noch ein Byte.

Ich würde es eher so machen:

I2C Start (Adresse + schreiben)
I2C schreibe (0x06)
I2C rep Start (Adresse + lesen)
status = I2C Lese NACK
I2C Stopp

wenn Status OK (Daten vorhanden und keine Fehler)
{
X, Y und Z Daten auslesen
}



Was du auch noch machen kannst ist eine Hysterese einbauen, also nicht auf exakt 90° drehen, sondern wenn der Bereich zwischen z.B. 89 und 91° ist, ist er OK.

Eine weitere Funktion wäre die Geschwindigkeit der Motoren im Bereich um den Zielwinkel zu reduzieren (z.B. Winkelabweichung > 45° => PWM=100%, Winkelabweichung > 25° => PWM = 50%,...)

Beim Messen könntest du auch versuchen die Motoren abzuschalten. Also Motoren stoppen, Messwert auslesen, Ziel berechnen, Motoren wieder einschalten, delay

MfG Hannes