bool Gyro_init(void)
{
Wire.begin();
mpu.initialize();
if (mpu.testConnection() == false) {
return false;
}
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
mpu.setXGyroOffset(X_GYRO_OFFSET);
mpu.setYGyroOffset(Y_GYRO_OFFSET);
mpu.setZGyroOffset(Z_GYRO_OFFSET);
mpu.setXAccelOffset(X_ACCEL_OFFSET);
mpu.setYAccelOffset(Y_ACCEL_OFFSET);
mpu.setZAccelOffset(Z_ACCEL_OFFSET);
mpu.setDMPEnabled(true);
dmpReady = true;
attachInterrupt(0, dmp_data_ready, RISING);
mpuIntStatus = mpu.getIntStatus();
packetSize = mpu.dmpGetFIFOPacketSize();
return true;
}
else {
return false;
}
}
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#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
// initialize serial communication
Serial.begin(38400);
while (!Serial);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
send_status(MPU_INITIALIZE, STATUS_OK);
// verify connection
send_status(MPU_CONNECTION, mpu.testConnection() ? STATUS_OK : STATUS_FAIL);
// load and configure the DMP
// 0 = DMP OK
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
ua_dev_status = mpu.dmpInitialize();
send_status(DMP_INITIALIZE, ua_dev_status);
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(120);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-185);
mpu.setZAccelOffset(1688); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (ua_dev_status == 0)
{
// turn on the DMP, now that it's ready
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
attachPinChangeInterrupt(INTERRUPT_PIN, dmpDataReady, RISING);
ua_mpu_interrupt_status = mpu.getIntStatus();
send_status(DMP_INTERRUPT, ua_mpu_interrupt_status);
b_dmp_ready = true;
// get expected DMP packet size for later comparison
uh_packet_size = mpu.dmpGetFIFOPacketSize();
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
void calibrate_accel(){
xoff = accelgyro.getXAccelOffset();
yoff = accelgyro.getYAccelOffset();
zoff = accelgyro.getZAccelOffset();
byte i=0;
while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
accelgyro.getAcceleration(&ax, &ay, &az);
if (count == SAMPLE_COUNT){
xoff += int(axm/-6);
yoff += int(aym/-6);
zoff += int((azm+16384)/-6);
accelgyro.setXAccelOffset(xoff);
accelgyro.setYAccelOffset(yoff);
accelgyro.setZAccelOffset(zoff);
#ifdef CAL_DEBUG
Serial.print(axm); Serial.print(" ");
Serial.print(aym); Serial.print(" ");
Serial.println(azm);
Serial.print(xoff); Serial.print(" ");
Serial.print(yoff); Serial.print(" ");
Serial.println(zoff);
Serial.println("*********************");
#endif
count = 0;
i++; //iteration++
#ifdef CAL_DEBUG
Serial.print(".");
#endif
}
else{
axm = (axm*count + ax)/(count+1.0);
aym = (aym*count + ay)/(count+1.0);
azm = (azm*count + az)/(count+1.0);
count++;
}
}
#ifdef CAL_DEBUG
Serial.println(" Done.");
#endif
}
void setup() {
mpu.initialize();
// trace_printf(mpu.testConnection() ? ("MPU6050 connection successful\n") : ("MPU6050 connection failed\n"));
// load and configure the DMP
// trace_printf("Initializing DMP...\n");
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
// trace_printf("Enabling DMP...\n");
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
// trace_printf("DMP ready! Waiting for first interrupt...\n");
// trace_printf("System is running!\n");
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)
// trace_printf("DMP Initialization failed (code \n");
// trace_printf("%d\n", devStatus);
}
}
void setup() {
Spark.variable("quaternionW", &quaternionW, DOUBLE);
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
//TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial.available()) ; // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device
Serial.println("Initializing I2C devices...");
mpu.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// wait for ready
Serial.println("\nSend any character to begin DMP programming and demo: ");
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println("Initializing DMP...");
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println("Enabling DMP...");
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println("Enabling interrupt detection (Arduino external interrupt 0)...");
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println("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("DMP Initialization failed (code ");
Serial.print(devStatus);
Serial.println(")");
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
void imu_init()
{
uint8_t count = 10;
// initialize device
#ifdef __BOARD_YUN__
Console.println(F("Initializing I2C devices..."));
#else
Serial.println(F("Initializing I2C devices..."));
#endif
mpu.initialize();
// verify connection
#ifdef __BOARD_YUN__
Console.println(F("Testing device connections..."));
Console.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
#else
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
#endif
/*
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
*/
// load and configure the DMP
#ifdef __BOARD_YUN__
Console.println(F("Initializing DMP..."));
#else
Serial.println(F("Initializing DMP..."));
#endif
do {
devStatus = mpu.dmpInitialize();
// Set some offset to the MEMS
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
// make sure it worked (returns 0 if so)
if (devStatus == 0)
{
count = 10;
// turn on the DMP, now that it's ready
#ifdef __BOARD_YUN__
Console.println(F("Enabling DMP..."));
#else
Serial.println(F("Enabling DMP..."));
#endif
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
#ifdef __BOARD_YUN__
Console.println(F("DMP ready! Waiting for first interrupt..."));
#else
Serial.println(F("DMP ready! Waiting for first interrupt..."));
#endif
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
return;
}
else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
#ifdef __BOARD_YUN__
Console.print(F("DMP Initialization failed (code"));
Console.print(devStatus);
Console.println(F(")"));
// New attempt message
Console.println(F("Trying again"));
#else
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
// New attempt message
Serial.println(F("Trying again"));
#endif
}
}
while (--count);
// configure LED for output
pinMode(SOL_LED, OUTPUT);
// Check if the configuration has failed
// if (!count)
{
#ifdef __BOARD_YUN__
Console.println(F("DMP initializaion failed"));
#else
Serial.println(F("DMP initialization failed"));
#endif
while (true)
{
// Locks in infinite loop
digitalWrite(SOL_LED, HIGH);
delay(300);
digitalWrite(SOL_LED, LOW);
delay(300);
}
}
}
