MultiIO & RP6M256 - GYRO-Demo

[Dirk]

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:

/* 
 * ****************************************************************************
 * 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ß!