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ß!
Zitieren
Lesezeichen