Hello guys
How can I map Gyroscope L3G4200D values to have centimeters as result?
This is my code for my R2D2 unit, I would like to use the gyro to tell Arduino mega when the robot is stuck(no movement) but the gyro is very sensitive so it reads also vibrations from the motors pushing when the robot is stuck.
Thanks
The code part 1/2:
#include <NewPing.h>
#include <Wire.h>
#include <Encoder.h>
#define SONAR_NUM 4 // Number of sensors.
#define MAX_DISTANCE 300 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 100 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM]; // Where the ping distances are stored.
uint8_t currentSensor = 0; // Keeps track of which sensor is active.
NewPing sonar[SONAR_NUM] = { // Sensor object array.
NewPing(26, 27, MAX_DISTANCE), // FRONT SENSOR Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(28, 29, MAX_DISTANCE), // DX SENSOR
NewPing(24, 25, MAX_DISTANCE), //LX SENSOR
NewPing(22, 23, MAX_DISTANCE), // BACK MSENSOR
// NewPing(23, 24, MAX_DISTANCE),
// NewPing(25, 26, MAX_DISTANCE),
// NewPing(27, 28, MAX_DISTANCE),
// NewPing(29, 30, MAX_DISTANCE),
// NewPing(31, 32, MAX_DISTANCE),
// NewPing(34, 33, MAX_DISTANCE),
// NewPing(35, 36, MAX_DISTANCE),
// NewPing(37, 38, MAX_DISTANCE),
// NewPing(39, 40, MAX_DISTANCE),
// NewPing(50, 51, MAX_DISTANCE),
// NewPing(52, 53, MAX_DISTANCE)
};
//Motor LX;
int dir1PinA = 13;
int dir2PinA = 12;
int speedPinA = 10;
//motor RX;
int dir1PinB = 11;
int dir2PinB = 8;
int speedPinB = 9;
int spead = 255; //define the spead of motor
//--------------------------------------GYRO--------------------------
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24
int L3G4200D_Address = 105; //I2C address of the L3G4200D
int x;
int y;
int z;
//---------------------------------GYRO END--------------------------------
void setup() {
pinMode (dir1PinA, OUTPUT);
pinMode (dir2PinA, OUTPUT);
pinMode (speedPinA, OUTPUT);
pinMode (dir1PinB, OUTPUT);
pinMode (dir2PinB, OUTPUT);
pinMode (speedPinB, OUTPUT);
Wire.begin();
Serial.begin(9600);
// -------------PING---------------------
pingTimer[0] = millis() + 50; // First ping starts at 75ms, gives time for the Arduino to chill before starting.
for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
//---------------------------------GYRO------------------------
Serial.println("starting up L3G4200D");
setupL3G4200D(500); // Configure L3G4200 - 250, 500 or 2000 deg/sec
delay(500); //wait for the sensor to be ready
}
//----------------motors functions
void forward()
{
//for (int spead = 180; spead < 256; spead++)
analogWrite(speedPinA, spead);
digitalWrite (dir1PinA, LOW);// right motor
digitalWrite (dir2PinA, HIGH);
analogWrite(speedPinB, spead); // lx motor
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, HIGH);
Serial.println(spead);
}
void backward()
{
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 255);
digitalWrite (dir1PinA, HIGH);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, HIGH);
digitalWrite (dir2PinB, LOW);
delay(10);
}
void left()
{
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 0);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, HIGH);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, LOW);
delay(10);
}
void right()
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 255);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, HIGH);
delay(10);
}
void stop()
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 0);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, LOW);
}
void rightBACK()// ----------------------BACK RIGHT
{
analogWrite(speedPinA, 0);
analogWrite(speedPinB, 255);
digitalWrite (dir1PinA, LOW);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, HIGH);
digitalWrite (dir2PinB, LOW);
delay(10);
}
void leftBACK() //-----------------------BACK LEFT
{
analogWrite(speedPinA, 255);
analogWrite(speedPinB, 0);
digitalWrite (dir1PinA, HIGH);
digitalWrite (dir2PinA, LOW);
digitalWrite (dir1PinB, LOW);
digitalWrite (dir2PinB, LOW);
delay(10);
}
//-----------------end motors function------------------------
The code part 2/2:
void loop() {
for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
cm[currentSensor] = 0; // Make distance zero in case there's no ping echo for this sensor.
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
}
}
cm[0] = constrain(cm[0], 5, 300);
cm[1] = constrain(cm[1], 5, 300);
cm[2] = constrain(cm[2], 5, 300);
cm[3] = constrain(cm[3], 5, 300);
// Other code that *DOESN'T* analyze ping results can go here.
// GO forward
if (cm[0] >= 35) {
// -----------------------------------Center sensor
//stop();
forward();
Serial.println("FWD A");
Serial.println("FWD B");
}
if (cm[0] <= 35 && cm[0] != 5) {
backward();
Serial.println("BKW A");
Serial.println("BKW B");
leftBACK();
}
//---------------------------------------------RIGHT SENSOR
if (cm[1] <= 20 && cm[1] != 5) {
left();
// MOTORS TURN RIGH M
Serial.println("FWD RX A"); //RX MOTOR ON
Serial.println("STOP B"); //LX MOTOR OFF
}
if (cm[2] <= 20 && cm[2] != 5) { //---------------- LEFT SENSOR
right();
Serial.println("STOP A");// RX MOTOR OFF
Serial.println("FWD B");// LX MOTOR ON
}
if (cm[3] <= 15 && cm[3] != 5) { // -------------------BACK SENSOR
forward();
Serial.println("STOP A");
Serial.println("STOP A");
Serial.println("STOP B");
Serial.println("STOP B");
right();
}
//----------------------STUCK front RIGHT
if (cm[0] <=10 && cm[1] <=10 && cm[0] !=5 && cm[1] !=5) {
rightBACK();
Serial.println("STUCK front right");
}
//------------------------STUCK front LEFT
if (cm[0] <=10 && cm[2] <=10 && cm[0] !=5 && cm[2] !=5) {
leftBACK();
Serial.println("STUCK front LEFT");
}
//------------------------STUCK BACK LEFT
if (cm[3] <=10 && cm[2] <=10 && cm[3] !=5 && cm[2] !=5) {
left();
Serial.println("STUCK BACK LEFT");
}
//------------------------STUCK BACK RICHT
if (cm[3] <=10 && cm[2] <=10 && cm[3] !=5 && cm[2] !=5) {
right();
Serial.println("STUCK BACK RIGHT");
}
if (cm[0] <10 && cm[1] <10 && cm[0] !=5 && cm[1] !=5 && cm[2] <10 && cm[3] <10 && cm[2] !=5 && cm [3] != 5) {
leftBACK();
Serial.println("STUCK ALL BACK SIDE");
}
//---------------------------------------GYRO--------------------------------
getGyroValues(); // This will update x, y, and z with new values
Serial.print("X:");
Serial.print(x);
Serial.print(" Y:");
Serial.print(y);
Serial.print(" Z:");
Serial.println(z);
delay(100); //Just here to slow down the serial to make it more readable
}
void echoCheck() { // If ping received, set the sensor distance to array.
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
void oneSensorCycle() { // Sensor ping cycle complete, do something with the results.
// The following code would be replaced with your code that does something with the ping results.
for (uint8_t i = 0; i < SONAR_NUM; i++) {
Serial.print(i);
Serial.print("=");
Serial.print(cm[i]);
Serial.print("cm ");
Serial.println();
}
}
//--------------------------------------GYRO VOID-----------------------
void getGyroValues(){
byte xMSB = readRegister(L3G4200D_Address, 0x29);
byte xLSB = readRegister(L3G4200D_Address, 0x28);
x = ((xMSB << 8) | xLSB);
byte yMSB = readRegister(L3G4200D_Address, 0x2B);
byte yLSB = readRegister(L3G4200D_Address, 0x2A);
y = ((yMSB << 8) | yLSB);
byte zMSB = readRegister(L3G4200D_Address, 0x2D);
byte zLSB = readRegister(L3G4200D_Address, 0x2C);
z = ((zMSB << 8) | zLSB);
}
int setupL3G4200D(int scale){
//From Jim Lindblom of Sparkfun's code
// Enable x, y, z and turn off power down:
writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
// If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
//writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
// Configure CTRL_REG3 to generate data ready interrupt on INT2
// No interrupts used on INT1, if you'd like to configure INT1
// or INT2 otherwise, consult the datasheet:
// writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
// CTRL_REG4 controls the full-scale range, among other things:
if(scale == 250){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
}else if(scale == 500){
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
}else{
writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
}
// CTRL_REG5 controls high-pass filtering of outputs, use it
// if you'd like:
writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}
void writeRegister(int deviceAddress, byte address, byte val) {
Wire.beginTransmission(deviceAddress); // start transmission to device
Wire.write(address); // send register address
Wire.write(val); // send value to write
Wire.endTransmission(); // end transmission
}
int readRegister(int deviceAddress, byte address){
int v;
Wire.beginTransmission(deviceAddress);
Wire.write(address); // register to read
Wire.endTransmission();
Wire.requestFrom(deviceAddress, 1); // read a byte
while(!Wire.available()) {
// waiting
}
v = Wire.read();
return v;
}
How can I map Gyroscope L3G4200D values to have centimeters as result?
That's a really good question.
What's the geometry that maps radians per second to metres?
I guess tangent
but it is the code that gives me problem
I need to map gyro values to be less sensitive that's why I would transform in cm so I can control it better.
what do you think?
I think I don't understand your reasoning.
If you need to get rid of noise from the gyro, try a rolling average.