Dirk
27.07.2013, 18: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:
/*
* ************************************************** **************************
* 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_transmissio nError);
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ß!
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_transmissio nError);
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ß!