Hello! I'm trying to send the angles value from a MPU6050 with nRF24L01. I'm using two Arduino UNO and Jeff Rowberg's MPU libraries. The program is working but I have this issues:
The receiver gets data very slowly because the transmitter is not always sending data (it shows "data can't be sent"). And sometimes it stops sending at all, even if I press reset button.
The biggest problem is that the programs works for a while and then It stops. If I press reset button it starts to work again but the problem is still there.
NOTE: the Arduino IDE shows "few memory available, it may cause stability problems" and I don't know how to fix it.
//TRANSMITTER
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define CE_PIN 9
#define CSN_PIN 10
byte direccion[5] ={'c','a','n','a','l'};
RF24 radio(CE_PIN, CSN_PIN);
MPU6050 mpu;
#define INTERRUPT_PIN 2 //Definir el pin de interrupciones
#define LED_PIN 13
bool blinkState = false;
//Variables de control y estado del MPU
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[1024];
//Variables de orientación y movimiento
Quaternion q; // [w, x, y, z]
VectorInt16 aa; // [x, y, z]
VectorInt16 aaReal; // [x, y, z]
VectorInt16 aaWorld; // [x, y, z]
VectorFloat gravity; // [x, y, z]
float ypr[3]; // [yaw, pitch, roll]
float angulos[3];
float yaw;
float pitch;
float roll;
volatile bool mpuInterrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
radio.begin();
radio.openWritingPipe(direccion);
//Iniciar comunicación con el bus I2C
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
// Iniciar MPU6050
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// Iniciar DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(219);
mpu.setYGyroOffset(75);
mpu.setZGyroOffset(-19); //85
mpu.setZAccelOffset(1688);
// Activar DMP
if (devStatus == 0) {
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// Activar interrupcion
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
radio.stopListening();
}
void loop() {
if (!dmpReady) return;
// Ejecutar mientras no hay interrupcion
while (!mpuInterrupt && fifoCount < packetSize) {
// AQUI EL RESTO DEL CODIGO DE TU PROGRRAMA
}
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// Obtener datos del FIFO
fifoCount = mpu.getFIFOCount();
// Controlar overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
}
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.resetFIFO();
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// Mostrar Yaw, Pitch, Roll
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
yaw=-ypr[0] * 180/M_PI;
pitch=ypr[1] * 180/M_PI;
roll=ypr[2] * 180/M_PI;
angulos[0]=yaw;
angulos[1]=pitch;
angulos[2]=roll;
bool ok = radio.write(&angulos, sizeof(angulos));
if(ok){
Serial.print(angulos[0]);
Serial.print(angulos[1]);
Serial.println(angulos[2]);
}
else{
Serial.println("no se ha podido enviar");
}
delay(100);
}}
//RECEIVER
#include <Adafruit_NeoPixel.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#define CE_PIN 9
#define CSN_PIN 10
int PIN=6;
int TInter=50;
byte direccion[5] ={'c','a','n','a','l'};
RF24 radio(CE_PIN, CSN_PIN);
int NUMPIXELS=40;
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
float datos[3];
float yaw;
float pitch;
float roll;
void POSICION ()
{
for (int fo1=28;fo1<40;fo1++){
pixels.setPixelColor(fo1,0,0,0);
pixels.show();
}
for (int fo2=0;fo2<12;fo2++){
pixels.setPixelColor(fo2,0,0,0);
pixels.show();
}
for (int p1=12;p1<28;p1++){
pixels.setPixelColor(p1,100,0,0);
pixels.show();
}
delay (200);
for (int p2=12;p2<28;p2++){
pixels.setPixelColor(p2,0,0,0);
pixels.show();
}
delay (140);
}
void FRENO(){
for (int f=0;f<41;f++){
pixels.setPixelColor(f,255,1,0);
pixels.show();
}}
void IZQUIERDA(){
for (int doff=28;doff<40;doff++){
pixels.setPixelColor(doff,0,0,0);
pixels.show();
}
for (int piz=12;piz<28;piz++){
pixels.setPixelColor(piz,100,0,0);
pixels.show();
}
for (int i=7;i>=-4;i--){
int i1=i+1;
int i2=i+2;
int i3=i+3;
int i4=i+4;
pixels.setPixelColor(i,245,70,0);
pixels.setPixelColor(i1,245,70,0);
pixels.setPixelColor(i2,245,70,0);
pixels.setPixelColor(i3,245,70,0);
pixels.setPixelColor(i4,0,0,0);
pixels.show();
delay (TInter);
}}
void DERECHA(){
for (int io=0;io<12;io++){
pixels.setPixelColor(io,0,0,0);
pixels.show();
}
for (int pdcha=12;pdcha<28;pdcha++){
pixels.setPixelColor(pdcha,100,0,0);
pixels.show();
}
for (int d=32;d<=43;d++){
int d1=d-1;
int d2=d-2;
int d3=d-3;
int d4=d-4;
pixels.setPixelColor(d,245,70,0);
pixels.setPixelColor(d1,245,70,0);
pixels.setPixelColor(d2,245,70,0);
pixels.setPixelColor(d3,245,70,0);
pixels.setPixelColor(d4,0,0,0);
pixels.show();
delay (TInter);
}}
void setup() {
pixels.begin();
pixels.show();
radio.begin();
radio.setPALevel(RF24_PA_LOW);
Serial.begin(9600);
radio.openReadingPipe(1, direccion);
radio.startListening();
}
void loop() {
uint8_t numero_canal;
if ( radio.available() ){
radio.read(datos,sizeof(datos));
Serial.print("yaw= " );
Serial.print(datos[0]);
Serial.print(" pitch= " );
Serial.print(datos[1]);
Serial.print(" roll= " );
Serial.println(datos[2]);
yaw=datos[0];
pitch=datos[1];
roll=datos[2];
if(yaw>80 && pitch<10 && roll<10){
IZQUIERDA();
}
if(yaw<10 && pitch<10 && roll>70){
DERECHA();
}
if(yaw<80 && roll<70){
POSICION();
} }
else{
Serial.println("No hay datos de radio disponibles");
POSICION();
}
delay(100);
}
I got a screenshot of the serial monitor.
Hope someone can help. Thanks!
