Ok. Thank you very much for your patience with me. ![]()
#include <SoftwareSerial.h>
#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;
SoftwareSerial MyBlue(0, 1); // RX | TX
// Timers
unsigned long timer = 0;
float timeStep = 0.01;
// Pitch, Roll and Yaw values
byte pitch = 0;
byte roll = 0;
int count;
void setup()
{
Serial.begin(9600);
MyBlue.begin(9600);
Serial.println("Initialize MPU6050");
while (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH);
mpu.calibrateGyro(); //Calibrate gyroscope
digitalWrite(LED_BUILTIN, LOW);
mpu.setThreshold(3); //Set threshold/sensitivity
Serial.println("Ready to connect\nDefualt password is 1234 or 000");
}
void loop()
{
if (MyBlue.available())
{
timer = millis();
// Read normalized values
Vector norm = mpu.readNormalizeGyro();
// Calculate Pitch, Roll and Yaw
pitch = pitch + norm.YAxis * timeStep;
roll = roll + norm.XAxis * timeStep;
// Output raw
MyBlue.print(roll);
MyBlue.print("|");
MyBlue.println(pitch);
Serial.print(pitch);
Serial.print("|");
Serial.println(roll);
// Wait to full timeStep period
delay((timeStep * 1000) - (millis() - timer));
}
}
