Drehen mit Kompass zu ungenau / falsche werte gemessen?
Hi,
nach langer Zeit hat mich mal wieder der Bastelwahn gepackt und ich hab an meinem RP6 weiter gemacht.
Ich hab zum Geburtstag ein Kompassmodul bekommen (CMPS03).
Ich hab es an den I2C Bus angeschlossen und mit dem Programm von WarChild angesteuert. (danke für das Programm)
Nachdem ich herausgefunden habe, dass die Motoren den Kompass stören, hab ich ihn höher gesetzt. Jetzt stimmen die Werte.
Danach habe ich folgendes Programm geschrieben. Es lässt den Roboter solange drehen, bis er den gewünschten Winkel erreicht hat. Kontrolliert vom Kompass.
Code:
#include "RP6RobotBaseLib.h"
#include "kompass.c"
void adv_rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle)
{
// Calculate the Angle
uint16_t old_comp;
uint16_t comp;
uint16_t new_comp;
uint16_t upend;
uint16_t downend;
old_comp = dirCMPS03();
if(dir == RIGHT)
{
new_comp = old_comp + angle;
if(new_comp >= 360)
new_comp = new_comp - 360;
upend = new_comp + 1;
downend = new_comp - 1;
if(downend <= 0)
downend = downend + 360;
if(upend >= 360)
upend = upend - 360;
writeString_P("\t");
writeInteger(new_comp,DEC);
writeString_P("\t Upend: ");
writeInteger(upend,DEC);
writeString_P("\t downend: ");
writeInteger(downend,DEC);
writeString_P("\n");
moveAtSpeed(desired_speed, 0);
while((comp <= downend) || (comp >= upend))
{
comp = dirCMPS03();
task_RP6System();
writeString_P("Current: ");
writeInteger(comp,DEC);
writeString_P("\t Newcomp: ");
writeInteger(new_comp,DEC);
writeString_P("\t Upend: ");
writeInteger(upend,DEC);
writeString_P("\t downend: ");
writeInteger(downend,DEC);
writeString_P("\n");
}
moveAtSpeed(0, 0);
}
while(!isMovementComplete())
task_RP6System();
}
int main(void)
{
initRobotBase();
powerON();
uint16_t comp;
adv_rotate(50, RIGHT, 90);
while(true)
{
task_RP6System();
comp = dirCMPS03();
writeInteger(comp,DEC);
writeString_P("\n");
}
}
Hab mal die Fehler ausgebessert
Und noch die kompass.c
Code:
#include "RP6I2CmasterTWI.h"
#define CMPS03 0xC0
uint16_t dirCMPS03(void)
{
I2CTWI_initMaster(100);
uint8_t cmpsbuffer[2];
uint16_t direction;
I2CTWI_transmitByte(CMPS03, 2); // Ab dem zweiten Register Daten anfordern
I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); // und in dem Array speichern
direction = ((cmpsbuffer[0] * 256) + cmpsbuffer[1]) / 10;
return direction;
}
Ganz fertig ist sie noch nicht. Links drehen fehlt.
Soweit funktioniert das auch. Nur wenn ich ihn um 90° drehen lassen will nimmt er nur ca. 45°. Auf dem Kompass jedoch sind es 90°
An den upend und downend liegt es nicht.
Falls ihr euch fragt warum upend / downend und nicht new_comp.
Mein Loop ist nicht schnell genug um jeden gemessenen Wert dazustellen. Deswegen kam es oft vor, das er sich 5-6 mal im Kreisgedreht hat. Also habe ich einen Bereich definiert, in dem der Wert liegen soll.
Ich bin verwirrt
Sieht jemand einen Fehler?
Einen kenne ich, aber ich finde die Ursache nicht.
Wenn ich das Programm starte um im Terminal nachschaue. Gibt er mir den new_comp wert aus. Bei der ersten Ausgabe ist er z.B 250. Und bei der zweiten und den folgenden ist er dann aufeinmal z.B. 30. Ist aber nur manchmal.
Gruß
Spanky
