MultiIO & RP6M256 - GYRO-Demo

**Dirk** · 27.07.2013, 19:49

Hallo MultiIO-Fans,

in der Demo für die Orientation-Lib haben wir den GYRO L3GD20 auf der minIMU-9 v2 etwas vernachlässigt, bzw. nur die Werte der 3 Achsen ausgegeben.

Hier noch eine kurze Demo, die die Winkelgeschwindigkeiten, mit denen der RP6 sich bewegt oder bewegt wird, auf allen 3 Achsen auf dem LCD darstellt.

Datei: RP6M256_MultiIO_05_GYRO_01.c:

Code:

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 MultiIO
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * In this example we show a fifth test for the MultiIO Project Board.
 * It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU).
 *
 * The following sensors are used:
 * - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
 *   chipset
 * - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
 *   2-axis magnetometer
 * - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
 *   the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer 
 *
 * ############################################################################
 * The Robot does NOT move in this example! You can simply put it on a table
 * next to your PC and you should connect it to the PC via the USB Interface!
 * You should also connect to it via WIFI.
 * ############################################################################
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h"                 // The RP6 M256 Library. 
                                        // Always needs to be included!
#include "RP6I2CmasterTWI.h"            // Include the I2C-Bus Master Library
#include "RP6M256uart1.h"                // The RP6 M256 UART1 Library
#include <math.h>

/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Orientation library":
// (This is the library for accessing certain sensors connected to the
//  MultiIO Project Board!)

#include "RP6M256_OrientationLib.h"

/*****************************************************************************/
// Defines:

//#define CALIBRATION

/*****************************************************************************/
// Variables:

double xdpsg = 0.0;
double ydpsg = 0.0;
double zdpsg = 0.0;
double last_xg = 0.0;
double last_yg = 0.0;
double last_zg = 0.0;

/*****************************************************************************/

#define CTRL_REG5_FIFO_EN            0b01000000    // FIFO on
#define FIFO_CTRL_REG_BYPASS        0b00000000    // Bypass mode (default)
#define FIFO_CTRL_REG_FIFO            0b00100000    // FIFO mode
#define FIFO_CTRL_REG_STREAM        0b01000000    // Stream mode
#define FIFO_CTRL_REG_STREAM2FIFO    0b01100000    // Stream-to-FIFO mode
#define FIFO_CTRL_REG_BYPASS2STREAM    0b10000000    // Bypass-to-Stream mode

/**
 * Call this once in order to use the 3D-Gyro
 * with the FIFO in Stream mode.
 *
 * Hint: Interrupts or FIFO watermark infos are
 *       NOT used here!
 *
 */
void L3GD20_initStreamMode(void)
{
    // Choose Stream mode:
    I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM);
    mSleep(10);
    // Activate FIFO:
    I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN);
    mSleep(10);
}

#define DPS_PER_DIGIT                0.00875        // @ +-250 dps (default)
//#define DPS_PER_DIGIT                0.0175        // @ +-500 dps
//#define DPS_PER_DIGIT                0.07        // @ +-2000 dps

/**
 * This is the L3GD20 gyroscope task.
 *
 * The task stores the averaged dps values in
 * the global double variables xdpsg, ydpsg,
 * zdpsg.
 *
 * ATTENTION: Stopwatch 7 is used for the GYRO
 *            task! Please do not use this
 *            stopwatch elsewhere in your
 *            program!
 *
 * Hint: You must calibrate the gyro carefully
 *       before using this task!
 *
 */
void task_GYRO(void)
{
    uint8_t readBuf[6];

    if(getStopwatch7() > 100) // 100ms
    {
        I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80));
        I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read  X-/Y-/Z-axis
        // xb = y:
        x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L];
        // yb = -x:
        y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L];
        y_axisg *= -1;
        // zb = z:
        z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L];
        normalizeL3GD20();
        xg *= DPS_PER_DIGIT;
        yg *= DPS_PER_DIGIT;
        zg *= DPS_PER_DIGIT;
        xdpsg = (last_xg + xg) / 2;
        ydpsg = (last_yg + yg) / 2;
        zdpsg = (last_zg + zg) / 2;
        last_xg = xg;
        last_yg = yg;
        last_zg = zg;
        setStopwatch7(0);
    }
}

/*****************************************************************************/

/**
 * Write a floating point number to the LCD.
 *
 * Example:
 *
 *            // Write a floating point number to the LCD (no exponent):
 *            writeDoubleLCD(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
    dtostrf(number, width, prec, &buffer[0]);
    writeStringLCD(&buffer[0]);
}

/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 *
 */
void I2C_transmissionError(uint8_t errorState)
{
    writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
    writeInteger_WIFI(errorState, HEX);
    writeChar_WIFI('\n');
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
    initRP6M256();    // Always call this first! The Processor will not work
                      // correctly otherwise. 

    initLCD(); // Initialize the LC-Display (LCD)
               // Always call this before using the LCD!

    setLEDs(0b1111);
    mSleep(500);
    setLEDs(0b0000);

    writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n"); 

    // IMPORTANT:
    I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
                            // with 100kHz SCL Frequency


    // Register the event handler:
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

    setLEDs(0b1111);

    // Write a text message to the LCD:
    showScreenLCD("################", "################");
    mSleep(1500);
    showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
    mSleep(2500); 
    showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO");
    mSleep(2500);
    clearLCD();

    setLEDs(0b0000);

    // ---------------------------------------

    clearLCD();
    startStopwatch1();
    startStopwatch7();                            // Used for GYRO task!

    // IMPORTANT:
    orientation_init();                            // Orientation init!

    L3GD20_initStreamMode();                    // Gyro Stream mode init!

    while(true) 
    {
#ifndef CALIBRATION
        task_I2CTWI();
        task_GYRO();
#endif
        task_I2CTWI();
        if(getStopwatch1() > 500) // 500ms
        {
#ifdef CALIBRATION
            // GYRO calibration part:
            readL3GD20();                        // Get sensor values
            //normalizeL3GD20();
            setCursorPosLCD(0, 0);        // line 1
            writeStringLCD_P("X");
            writeIntegerLCD(x_axisg, DEC);
            writeStringLCD_P("   ");
            setCursorPosLCD(0, 8);        // line 1 pos 9
            writeStringLCD_P("Y");
            writeIntegerLCD(y_axisg, DEC);
            writeStringLCD_P("   ");
            setCursorPosLCD(1, 0);        // line 2
            writeStringLCD_P("Z");
            writeIntegerLCD(z_axisg, DEC);
            writeStringLCD_P("    ");
#else
            // Display part:
            setCursorPosLCD(0, 0);        // line 1
            writeStringLCD_P("X");
            writeDoubleLCD(xdpsg, 6, 1);
            writeStringLCD_P("   ");
            setCursorPosLCD(0, 8);        // line 1 pos 9
            writeStringLCD_P("Y");
            writeDoubleLCD(ydpsg, 6, 1);
            writeStringLCD_P("   ");
            setCursorPosLCD(1, 0);        // line 2
            writeStringLCD_P("Z");
            writeDoubleLCD(zdpsg, 6, 1);
            writeStringLCD_P("   ");
#endif
            setStopwatch1(0);
        }

    }

    return 0;
}

Bedingungen:
- Orientierungs-Lib und RP6M256_MultiIO.h im Programmverzeichnis
- In der RP6M256_Orientation.h NUR IMU_9D aktivieren und GET_TEMP auskommentieren
- Den GYRO sorgfältig kalibrieren, d.h. die Definitionen OFFSET_X/Y/Z in der RP6M256_Orientation.h so anpassen, dass die Sensor-Ausgabewerte (xg, yg, zg) bei nicht bewegtem RP6 um den Nullpunkt schwanken

Viel Spaß!

**fabqu** · 28.07.2013, 13:29

Nicht schlecht

**Dirk** · 28.07.2013, 16:09

Hi Fabian!
Danke!

Es geht noch weiter mit einer 2. GYRO Demo:
Die GYRO Demo 1 weiter oben in diesem Thread ist ja ganz schön. Man kann die Drehgeschwindigkeit messen.
Leider hilft das nicht dabei, eine Winkelabweichung zu korrigieren.

Was heißt das? Nehmen wir an, der RP6 soll um 90° nach rechts drehen und dann wieder um 90° nach links. Dabei sollte er NATÜRLICH in der Ausgangsstellung wieder ankommen.
Mit der Odometrie gelingt das nicht gut. Man könnte das aber z.B. auch mit dem Magnetfeldsensor hinbekommen,- aber nur da, wo keine fremden Magnetfelder stören.

Der GYRO kann das am besten. Allerdings braucht man dann nicht die Winkelgeschwindigkeit, die der GYRO mißt, sondern den Winkel pro Zeiteinheit, den man integriert (aufsummiert), solange eine (Dreh-)Bewegung anliegt.

Hier also die 2. GYRO Demo, die die kumulierten Drehwinkel auf dem LCD anzeigt.
In Ruhelage des RP6 sollten die Werte bei 0 bleiben oder nur ganz langsam über die Zeit "wegdriften". Dieses Driften ist einerseits eine (schlechte) Gyro-Eigenschaft, die hoch temperaturabhängig ist und sich nicht beeinflussen läßt.
Andererseits: Wenn die Werte schnell (innerhalb weniger Sekunden) wegdriften, dann ist die Kalibrierung nicht sorgfältig genug gemacht worden. Wenn man das nicht besser hinkriegt, kann man auch den Wert von OFFSET_ZERO (aktuell 75) weiter erhöhen. Besser ist aber IMMER die gute Kalibrierung, weil durch die Erhöhung von OFFSET_ZERO die "Empfindlichkeit" für sehr langsame Drehungen geringer wird.

Datei: RP6M256_MultiIO_05_GYRO_02.c:

Code:

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 MultiIO
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * In this example we show a fifth test for the MultiIO Project Board.
 * It tests the "Orientation Sensors" (GPS module, 2D-Compass and/or 9D-IMU).
 *
 * The following sensors are used:
 * - NL-552ETTL GPS module (ELV No. 94241) with the u-blox5 UBX-G5000-BT GPS
 *   chipset
 * - HDMM01 compass module (Pollin No. 810164) with the MEMSIC MMC2120MG
 *   2-axis magnetometer
 * - MinIMU-9 v2 IMU module (Pololu No. 1268) with the L3GD20 3-axis gyro and
 *   the LSM303DLHC 3-axis accelerometer and 3-axis magnetometer 
 *
 * ############################################################################
 * The Robot does NOT move in this example! You can simply put it on a table
 * next to your PC and you should connect it to the PC via the USB Interface!
 * You should also connect to it via WIFI.
 * ############################################################################
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h"                 // The RP6 M256 Library. 
                                        // Always needs to be included!
#include "RP6I2CmasterTWI.h"            // Include the I2C-Bus Master Library
#include "RP6M256uart1.h"                // The RP6 M256 UART1 Library
#include <math.h>

/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Orientation library":
// (This is the library for accessing certain sensors connected to the
//  MultiIO Project Board!)

#include "RP6M256_OrientationLib.h"

/*****************************************************************************/
// Defines:

//#define CALIBRATION

/*****************************************************************************/
// Variables:

double xdg = 0.0;
double ydg = 0.0;
double zdg = 0.0;

/*****************************************************************************/

#define CTRL_REG5_FIFO_EN            0b01000000    // FIFO on
#define FIFO_CTRL_REG_BYPASS        0b00000000    // Bypass mode (default)
#define FIFO_CTRL_REG_FIFO            0b00100000    // FIFO mode
#define FIFO_CTRL_REG_STREAM        0b01000000    // Stream mode
#define FIFO_CTRL_REG_STREAM2FIFO    0b01100000    // Stream-to-FIFO mode
#define FIFO_CTRL_REG_BYPASS2STREAM    0b10000000    // Bypass-to-Stream mode

/**
 * Call this once in order to use the 3D-Gyro
 * with the FIFO in Stream mode.
 *
 * Hint: Interrupts or FIFO watermark infos are
 *       NOT used here!
 *
 */
void L3GD20_initStreamMode(void)
{
    // Choose Stream mode:
    I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, FIFO_CTRL_REG, FIFO_CTRL_REG_STREAM);
    mSleep(10);
    // Activate FIFO:
    I2CTWI_transmit2Bytes(I2C_MULTIIO_L3GD20_ADR, CTRL_REG5, CTRL_REG5_FIFO_EN);
    mSleep(10);
}

#define DPS_PER_DIGIT                0.00875        // @ +-250 dps (default)
//#define DPS_PER_DIGIT                0.0175        // @ +-500 dps
//#define DPS_PER_DIGIT                0.07        // @ +-2000 dps
#define OFFSET_ZERO                    75            // Zero offset (of raw values)
#define D_CORR_FACTOR                1.065        // Corrigation factor
#define GYRO_INTERVAL                50            // Measuring interval [ms]

/**
 * This is the L3GD20 gyroscope task.
 *
 * The task stores the cumulated angle values in
 * the global double variables xdg, ydg, zdg.
 *
 * ATTENTION: Stopwatch 7 is used for the GYRO
 *            task! Please do not use this
 *            stopwatch elsewhere in your
 *            program!
 *
 * Hint: You must calibrate the gyro carefully
 *       before using this task!
 *
 */
void task_GYRO(void)
{
    uint8_t readBuf[6];

    if(getStopwatch7() > GYRO_INTERVAL) // GYRO_INTERVAL [ms]
    {
        I2CTWI_transmitByte(I2C_MULTIIO_L3GD20_ADR, (OUT_X_L | 0x80));
        I2CTWI_readBytes(I2C_MULTIIO_L3GD20_ADR, readBuf, 6); // Read  X-/Y-/Z-axis
        // xb = y:
        x_axisg = (readBuf[OUT_Y_H - OUT_X_L] << 8) + readBuf[OUT_Y_L - OUT_X_L];
        // yb = -x:
        y_axisg = (readBuf[OUT_X_H - OUT_X_L] << 8) + readBuf[OUT_X_L - OUT_X_L];
        y_axisg *= -1;
        // zb = z:
        z_axisg = (readBuf[OUT_Z_H - OUT_X_L] << 8) + readBuf[OUT_Z_L - OUT_X_L];
        normalizeL3GD20();
        xg *= DPS_PER_DIGIT;                    // Calculate dps
        yg *= DPS_PER_DIGIT;
        zg *= DPS_PER_DIGIT;
        xg /= ((double) 1000 / GYRO_INTERVAL);    // Angle per interval
        yg /= ((double) 1000 / GYRO_INTERVAL);
        zg /= ((double) 1000 / GYRO_INTERVAL);
        if (abs(x_axisg) > OFFSET_ZERO)            // Don't cumulate 0 dps values
            xdg += (xg * D_CORR_FACTOR);        // Cumulated angle values [°]
        if (abs(y_axisg) > OFFSET_ZERO)
            ydg += (yg * D_CORR_FACTOR);
        if (abs(z_axisg) > OFFSET_ZERO)
            zdg += (zg * D_CORR_FACTOR);
        setStopwatch7(0);
    }
}

/**
 * This function resets the cumulated angle
 * values [°] stored in the global double
 * variables xdg, ydg, zdg by the L3GD20
 * gyroscope task.
 *
 */
void resetGYRO(void)
{
    xdg = 0.0;
    ydg = 0.0;
    zdg = 0.0;
}

/*****************************************************************************/

/**
 * Write a floating point number to the LCD.
 *
 * Example:
 *
 *            // Write a floating point number to the LCD (no exponent):
 *            writeDoubleLCD(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
    dtostrf(number, width, prec, &buffer[0]);
    writeStringLCD(&buffer[0]);
}

/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 *
 */
void I2C_transmissionError(uint8_t errorState)
{
    writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
    writeInteger_WIFI(errorState, HEX);
    writeChar_WIFI('\n');
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
    initRP6M256();    // Always call this first! The Processor will not work
                      // correctly otherwise. 

    initLCD(); // Initialize the LC-Display (LCD)
               // Always call this before using the LCD!

    setLEDs(0b1111);
    mSleep(500);
    setLEDs(0b0000);

    writeString_P_WIFI("\n\nRP6M256 Multi IO Selftest 5 GYRO!\n"); 

    // IMPORTANT:
    I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
                            // with 100kHz SCL Frequency


    // Register the event handler:
    I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

    setLEDs(0b1111);

    // Write a text message to the LCD:
    showScreenLCD("################", "################");
    mSleep(1500);
    showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
    mSleep(2500); 
    showScreenLCD("RP6M256 Multi IO", "Selftest 5 GYRO");
    mSleep(2500);
    clearLCD();

    setLEDs(0b0000);

    // ---------------------------------------

    clearLCD();
    startStopwatch1();
    startStopwatch7();                            // Used for GYRO task!

    // IMPORTANT:
    orientation_init();                            // Orientation init!

    L3GD20_initStreamMode();                    // Gyro Stream mode init!
    resetGYRO();                                // Reset GYRO!

    while(true) 
    {
#ifndef CALIBRATION
        task_I2CTWI();
        task_GYRO();
#endif
        task_I2CTWI();
        if(getStopwatch1() > 500) // 500ms
        {
#ifdef CALIBRATION
            // GYRO calibration part:
            readL3GD20();                        // Get sensor values
            //normalizeL3GD20();
            setCursorPosLCD(0, 0);        // line 1
            writeStringLCD_P("X");
            writeIntegerLCD(x_axisg, DEC);
            writeStringLCD_P("   ");
            setCursorPosLCD(0, 8);        // line 1 pos 9
            writeStringLCD_P("Y");
            writeIntegerLCD(y_axisg, DEC);
            writeStringLCD_P("   ");
            setCursorPosLCD(1, 0);        // line 2
            writeStringLCD_P("Z");
            writeIntegerLCD(z_axisg, DEC);
            writeStringLCD_P("    ");
#else
            // Display part:
            setCursorPosLCD(0, 0);        // line 1
            writeStringLCD_P("X");
            writeDoubleLCD(xdg, 6, 1);
            writeStringLCD_P("   ");
            setCursorPosLCD(0, 8);        // line 1 pos 9
            writeStringLCD_P("Y");
            writeDoubleLCD(ydg, 6, 1);
            writeStringLCD_P("   ");
            setCursorPosLCD(1, 0);        // line 2
            writeStringLCD_P("Z");
            writeDoubleLCD(zdg, 6, 1);
            writeStringLCD_P("   ");
#endif
            setStopwatch1(0);
        }

    }

    return 0;
}

Bedingungen:
- Orientierungs-Lib und RP6M256_MultiIO.h im Programmverzeichnis
- In der RP6M256_Orientation.h NUR IMU_9D aktivieren und GET_TEMP auskommentieren
- Den GYRO sorgfältig kalibrieren, d.h. die Definitionen OFFSET_X/Y/Z in der RP6M256_Orientation.h so anpassen, dass die Sensor-Ausgabewerte (xg, yg, zg) bei nicht bewegtem RP6 um den Nullpunkt schwanken

Viel Spaß!

**inka** · 11.08.2013, 10:21

Hi Dirk,

im moment bin ich noch im urlaub, aber ab und zu mal auch online...

meinst Du ich könnte so in einer woche die m32 version testen? Ich weiss nicht, ob Du zum umprogrammieren umbauen musst, evtl. gehts auch so, dass ich teste?

**Dirk** · 11.08.2013, 11:10

Hi inka,

die M32-Versionen sind schon fertig. Wenn du wieder zuhause bist, kann ich sie dir schicken.
Schönen Rest-Urlaub noch!

**inka** · 11.08.2013, 11:24

Hi Dirk,

danke für die wünsche und die files - freue mich schon drauf...

ende nächster woche melde ich mich von zuhause aus...

**inka** · 17.08.2013, 18:45

Hi Dirk,

danke für die M32-files,es liess mir doch keine ruhe noch ein paar tage zu warten:

bei der demo_1 werden im LCD x/y/z werte angezeigt, die sich im sekundenrhythmus verändern und um 0 im der größenordnung von 0,1 bis 0,2 schwanken. Daraus würde ich schliessen, dass die kalibrationswerte stimmen...

bei der demo_2 wird nur für x/y/z unverändert 0,0 angezeigt...

sonst passiert bei beiden demos nichts, auch wenn der RP6 gedreht wird...

hätte ich an den dateien noch etwas verändern sollen?

**Dirk** · 17.08.2013, 19:19

Hi inka,

bei der 1. Demo sollte die Winkelgeschwindigkeit angezeigt werden. Wenn du den RP6 also in der Hand hältst und rechts herum drehst, müßte die Z-Anzeige einen positiven Wert [Grad/Sekunde] zeigen. Nach links gedreht wäre der Wert negativ. Die Höhe des Werts hängt ab von der Geschwindigkeit, in der du die Drehung machst: Drehst du also in einer Sekunde um 90°, müßte der Wert auch maximal 90 anzeigen. Natürlich geht der wieder auf 0 zurück, wenn der RP6 nicht mehr bewegt wird. Ein Wert <> 0 wird also nur WÄHREND der Drehung angezeigt.

Bei der 2. Demo wird der Winkel angezeigt, um den du drehst. Wenn du also den RP6 ruhig stehen läßt, müßten alle Werte bei 0 bleiben, wenn die Kalibrierung ok ist. Drehst du den RP6 dann z.B. um 90° nach rechts, müßte die Z-Achse wieder 90 anzeigen, wobei der Wert nicht genau zu erreichen ist, kann also auch 87 oder 95° zeigen. Der Wert bleibt auch erhalten, während der RP6 nicht mehr bewegt wird. Wenn du also den RP6 nicht mehr anfasst, bleibt die Z-Achse bei 90° stehen. Drehst du jetzt wieder auf die Ursprungsposition zurück (also 90° nach links), wird der Z-Achsen Wert ungefähr wieder bei 0° liegen.

Das funktioniert so bei beiden Demos auf allen Achsen, also bei der Neigung nach rechts/links (Roll) und Nase rauf/runter (Pitch).

An den Demos muss nichts verändert werden.

**inka** · 18.08.2013, 11:56

hi Dirk,

nach dem kompilieren mit make (code:blocks hat wohl immer noch ne macke) habe ich bei demo 1 folgende werte:

x -31,7
y -3,2
z -9,3

diese verändern sich in der ersten stelle hinterm koma, reagieren aber nicht auf drehung (weder nach links, noch nach rechts)

bei demo 2 driften die werte sehr schnell weg, reagieren nicht auf eine drehung...

das hängt wohl mit der kalibrierung zusammen, ich werde dann in diesem thread weitermachen...

**Dirk** · 18.08.2013, 13:56

Hi inka,

ja, da spricht alles dafür, dass die Kalibrierung noch nicht ok ist.

Übrigens:
Du kannst die Kalibrierung des Gyro auch gut mit diesen Demos machen:
Wenn du in der Demo die "//" vor #define CALIBRATION wegmachst, kannst du mit der Demo die Rohwerte ausgeben.
Also so (z.B. Demo GYRO_02,- die ist am besten geeignet!):
1. In RP6Control_Orientation.h:
a) #define IMU_9D aktivieren, die beiden anderen Definitionen (GPS, COMPASS_2D) auskommentieren
b) #define GET_TEMP auskommentieren: //#define GET_TEMP
c) OFFSET_X, OFFSET_Y, OFFSET_Z auf 0 setzen
d) Datei speichern

2. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!)

3. In Ruhelage des RP6 die Werte beobachten: Schwanken sie z.B. bei X zwischen +20 und +140, also im Mittel um den Wert +80, dann in OFFSET_X -80 eintragen. Das so für alle 3 Achsen machen. Noch ein Beispiel: Schwanken die Werte bei Y zwischen -50 und +30, also im Mittel um -10, dann in OFFSET_Y +10 eintragen. Am Ende RP6Control_Orientation.h speichern.

4. In der Demo die "//" vor normalizeL3GD20(); wegmachen

5. Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!)

6. Ausgabewerte ansehen: Sie müßten jetzt um den Nullpunkt schwanken,- jedenfalls viel besser um 0 gemittelt sein. Gibt es da noch konstante Abweichungen, dann mit Punkt 3. weitermachen, dabei die neuen Offsets nur zu den vorhandenen addieren, also nicht bei 0 wieder anfangen. (Dabei bleibt die Zeile normalizeL3GD20(); immer mit drin!) Das so lange machen, bis du zufrieden bist, dann Kalibrierung ENDE!!!

7. Jetzt KANN es noch mit der Demo GYRO_02 ans "Feintuning" gehen:
a) In der Demo die "//" vor #define CALIBRATION wieder hinmachen (Messbetrieb, keine Kalibrierung mehr!).
b) Demo neu kompilieren (.st und .o der Orientation-Lib und alle Hilfsdateien der Demo VORHER LÖSCHEN!)
c) Idealerweise bleiben in Ruhelage des RP6 jetzt alle Achsen dauerhaft bei 0.0, dann Feintuning ENDE!!!
d) Ist das nicht der Fall und wandert eine Anzeige langsam immer weiter in den negativen oder positiven Bereich, kann man in der RP6Control_Orientation.h die OFFSET_X, _Y, _Z Werte vorsichtig korrigieren: Ich würde um max. +- 5 korrigieren, die Datei speichern und bei 7.b) weiter machen.

Noch ein Hinweis für ganz Exakte: Man kann jetzt noch den Wert OFFSET_ZERO der Demo kleiner machen (z.B. auf 60 oder 50 reduzieren). Dann driften die schön kalibrierten Werte evtl. wieder weg und man kann bei 7.b) weiter machen. Ende des Feintuning, wenn man das mehr als 2 Jahre gemacht hat oder wegen Vernachlässigung seiner sonstigen Aufgaben in der Psychiatrie liegt