Hi, I need help in understanding why the MPU 6050 cannot display the data on serial monitor. I had tried compiling the MPU 6050 dmp code and it was successful in giving me the data. However, when I tried compiling with my sketch it couldn't work. It only work when i comment out the rest of the code in the void loop except for the digitalWrite. Thanks
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Servo.h>
//#include <SoftwareSerial.h>
//SoftwareSerial XBee(2, 3);
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;
int ch1 = 4;
int ch2 = 5;
int ch3 = 6;
int ch4 = 7;
int ch6 = 8;
int led_red_light = 13;
int trigPin = A1;
int echoPin = A2;
int roll_channel;
int pitch_channel;
int throttle_channel;
int yaw_channel;
int alt_hold_switch;
int alt_hold_switch_value;
int throttle_channel_low;
int throttle_channel_high;
long duration;
float distance;
int hovering_pulse_function();
int throttle_channel_function();
int hovering_function();
int led_light_blink();
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
bool blinkState = false;
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;
float euler[3];
float ypr[3];
uint8_t teapotPacket[14] = { '
, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
void setup()
{
Serial.begin(115200);
pinMode(ch1, INPUT);
esc1.attach(9);
pinMode(ch2, INPUT);
esc2.attach(10);
pinMode(ch3, INPUT);
esc3.attach(11);
pinMode(ch4, INPUT);
esc4.attach(12);
pinMode(ch6, INPUT);
pinMode(echoPin, INPUT);
pinMode(trigPin, OUTPUT);
pinMode(led_red_light, OUTPUT);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24;
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
mpu.initialize();
devStatus = mpu.dmpInitialize();
mpu.setXGyroOffset(0);
mpu.setYGyroOffset(-1);
mpu.setZGyroOffset(1);
mpu.setZAccelOffset(16365);
if (devStatus == 0)
{
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
}
else
{
Serial.print(devStatus);
Serial.println(F(")"));
}
pinMode(LED_PIN, OUTPUT);
}
int hovering_pulse_function()
{
alt_hold_switch = pulseIn(ch6, HIGH);
if (alt_hold_switch < 970)
{
alt_hold_switch_value = HIGH;
}
else if (alt_hold_switch > 1460)
{
alt_hold_switch_value = LOW;
}
return 0;
}
int throttle_channel_function()
{
throttle_channel_high = 1500;
throttle_channel = throttle_channel_high;
return 0;
}
int hovering_function()
{
if (alt_hold_switch_value == HIGH && distance == 100)
{
esc1.writeMicroseconds(roll_channel);
esc2.writeMicroseconds(pitch_channel);
esc3.writeMicroseconds(throttle_channel_high);
esc4.writeMicroseconds(yaw_channel);
}
return 0;
}
int led_light_blink(){
//*****************************************************************************************************************************************************
//Run throttle_channel_function(); and turn on led light when meet condition of both "distance >= 95 && distance <=105 && alt_hold_switch_value"
if (distance >= 95 && distance <=105 && alt_hold_switch_value)
{
throttle_channel_function();
digitalWrite(led_red_light, LOW);
}
//*****************************************************************************************************************************************************
//LED blink when condition is "alt_hold_switch_value == LOW && distance >= 100"
else if(alt_hold_switch_value == LOW && distance > 0)
{
digitalWrite(led_red_light, LOW);
delay(100);
digitalWrite(led_red_light, HIGH);
}
//*****************************************************************************************************************************************************
return 0;
}
void loop()
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = duration * 0.034 / 2;
roll_channel = pulseIn(ch1, HIGH);
esc1.writeMicroseconds(roll_channel);
pitch_channel = pulseIn(ch2, HIGH);
esc2.writeMicroseconds(pitch_channel);
throttle_channel = pulseIn(ch3, HIGH);
esc3.writeMicroseconds(throttle_channel);
yaw_channel = pulseIn(ch4, HIGH);
esc4.writeMicroseconds(yaw_channel);
digitalWrite(led_red_light, HIGH);
if(!dmpReady) return;
mpuIntStatus = mpu.getIntStatus();
fifoCount = mpu.getFIFOCount();
if((mpuIntStatus & 0x10) || fifoCount == 1024)
{
mpu.resetFIFO();
}
else if (mpuIntStatus & 0x02)
{
while (fifoCount < packetSize)
fifoCount = mpu.getFIFOCount();
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUR_READABLE_QUATERNION
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetEuler(euler, &q);
Serial.print("euler\t");
Serial.print(euler[0] * 180/M_PI);
Serial.print("\t");
Serial.print(euler[1] * 180/M_PI);
Serial.print("\t");
Serial.println(euler[2] * 180/M_PI);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("Phi: ");
Serial.print(ypr[1] * 18/M_PI);
Serial.print("\t theta: ");
Serial.print(" ");
Serial.print(ypr[2] * 180/M_PI);
Serial.print("\t Psi: ");
Serial.print(" ");
Serial.println(ypr[0] * 180/M_PI);
#endif
#ifdef OUTPUT_READABLE_REALACCEL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
Serial.print("areal\t");
Serial.print(aaReal.x);
Serial.print("\t");
Serial.print(aaReal.y);
Serial.print("\t");
Serial.println(aaReal.z);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
#endif
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
hovering_pulse_function();
hovering_function();
led_light_blink();
Serial.print("Channel 1 (roll) = ");
Serial.print(roll_channel);
Serial.print("\t\tChannel 2 (pitch) = ");
Serial.print(pitch_channel);
Serial.print("\t\tChannel 3 (throttle) = ");
Serial.print(throttle_channel);
Serial.print("\t\tChannel 4 (yaw) = ");
Serial.print(yaw_channel);
Serial.print("\t\tDistance: ");
Serial.println(distance);
delay(500);
}