3 imu with RTIMULib

//  This file is part of RTIMULib-Arduino
//
//  Copyright (c) 2014-2015, richards-tech
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy of
//  this software and associated documentation files (the "Software"), to deal in
//  the Software without restriction, including without limitation the rights to use,
//  copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
//  Software, and to permit persons to whom the Software is furnished to do so,
//  subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//  INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//  PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//  OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include <Wire.h>
#include "I2Cdev.h"
#include "RTIMUSettings.h"
#include "RTIMU.h"
#include "RTFusionRTQF.h"
#include "CalLib.h"
#include <EEPROM.h>


RTIMU *imu;                                           // the IMU object
RTIMU *imu2;
RTIMU *imu3;

RTFusionRTQF fusion;                                  // the fusion object
RTFusionRTQF fusion2;
RTFusionRTQF fusion3;

RTIMUSettings settings;                               // the settings object




//  DISPLAY_INTERVAL sets the rate at which results are displayed

#define DISPLAY_INTERVAL  30                         // interval between pose displays

//  SERIAL_PORT_SPEED defines the speed to use for the debug serial port

#define  SERIAL_PORT_SPEED  9600

int EN = 53;
int S0 = 47;
int S1 = 49;
int S2 = 50;

void out1()
{
  digitalWrite (EN,LOW);
  digitalWrite (S0,LOW);
  digitalWrite (S1,LOW);
  digitalWrite (S2,LOW);
  }

void out2()
{
  digitalWrite (EN,LOW);
  digitalWrite (S0,LOW);
  digitalWrite (S1,HIGH);
  digitalWrite (S2,LOW);
  }

void out3()
{
  digitalWrite (EN,LOW);
  digitalWrite (S0,LOW);
  digitalWrite (S1,LOW);
  digitalWrite (S2,HIGH);
  }

unsigned long lastDisplay;
unsigned long lastDisplay2;
unsigned long lastDisplay3;

unsigned long lastRate;
unsigned long lastRate2;
unsigned long lastRate3;

int sampleCount;
int sampleCount2;
int sampleCount3;


void setup()
{
 out3();
 out2();
 out1();


  pinMode (EN,OUTPUT);
  pinMode (S0,OUTPUT);
  pinMode (S1,OUTPUT);
  pinMode (S2,OUTPUT);

    int errcode;

    Serial.begin(SERIAL_PORT_SPEED);
    Wire.begin();

 ///////// inizializzazione imu1  /////////////////

   out1();

   imu = RTIMU::createIMU(&settings);                        // create the imu object
//
//
   Serial.print("ArduinoIMU starting using the first device "); Serial.println(imu->IMUName());
  if ((errcode = imu->IMUInit()) < 0) {
       Serial.print("Failed to init IMU: "); Serial.println(errcode);
  }
//
    if (imu->getCalibrationValid())
       Serial.println("1: Using compass calibration");
    else
       Serial.println("1: No valid compass calibration data");
//
    lastDisplay = lastRate = millis();
    sampleCount = 0;
//
//    // Slerp power controls the fusion and can be between 0 and 1
//    // 0 means that only gyros are used, 1 means that only accels/compass are used
//    // In-between gives the fusion mix.
//
    fusion.setSlerpPower(0.02);
//
//    // use of sensors in the fusion algorithm can be controlled here
//    // change any of these to false to disable that sensor
//
    fusion.setGyroEnable(true);
    fusion.setAccelEnable(true);
    fusion.setCompassEnable(true);

  ///////// inizializzazione imu2  /////////////////

  out2();

    imu2 = RTIMU::createIMU(&settings);                        // create the imu object

    Serial.print("ArduinoIMU starting using the second device "); Serial.println(imu2->IMUName());
    if ((errcode = imu2->IMUInit2()) < 0) {
        Serial.print("Failed to init the second IMU: "); Serial.println(errcode);
    }

    if (imu2->getCalibrationValid())
        Serial.println("2: Using compass calibration");
    else
        Serial.println("2: No valid compass calibration data");

    lastDisplay2 = lastRate2 = millis();
    sampleCount2 = 0;
    // Slerp power controls the fusion and can be between 0 and 1
    // 0 means that only gyros are used, 1 means that only accels/compass are used
    // In-between gives the fusion mix.

    fusion2.setSlerpPower(0.02);

    // use of sensors in the fusion algorithm can be controlled here
    // change any of these to false to disable that sensor

    fusion2.setGyroEnable(true);
    fusion2.setAccelEnable(true);
    fusion2.setCompassEnable(true);


  ///////// inizializzazione imu3  /////////////////

 out3();
//
   imu3 = RTIMU::createIMU(&settings);                        // create the imu object
//
    Serial.print("ArduinoIMU starting using the third device "); Serial.println(imu3->IMUName());
//
  if ((errcode = imu3->IMUInit3()) < 0) {
      Serial.print("Failed to init IMU: "); Serial.println(errcode);
  }
//
if (imu3->getCalibrationValid())
      Serial.println("3: Using compass calibration");
    else
       Serial.println("3: No valid compass calibration data");

    lastDisplay3 = lastRate3 = millis();
   sampleCount3 = 0;
//    // Slerp power controls the fusion and can be between 0 and 1
//    // 0 means that only gyros are used, 1 means that only accels/compass are used
//    // In-between gives the fusion mix.
//
    fusion3.setSlerpPower(0.02);
//
//    // use of sensors in the fusion algorithm can be controlled here
//    // change any of these to false to disable that sensor
//
   fusion3.setGyroEnable(true);
    fusion3.setAccelEnable(true);
    fusion3.setCompassEnable(true);

}

void loop()
{
  out1();

    unsigned long now = millis();
    unsigned long now2 = millis();
    unsigned long now3 = millis();
    unsigned long delta;
    unsigned long delta2;
    unsigned long delta3;
    int loopCount = 1;
    int loopCount2 = 1;
    int loopCount3 = 1;

      out1();


    while (imu->IMURead()) {   // Serial.println("ciao ciao");                        // get the latest data if ready yet
        // this flushes remaining data in case we are falling behind
        if (++loopCount >= 3)
            continue;


       fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());

        sampleCount++;
        if ((delta = now - lastRate) >= 1000) {
  /*          Serial.print("Sample rate: "); Serial.print(sampleCount);
            if (imu->IMUGyroBiasValid())
                Serial.println(", gyro bias valid");
            else
                Serial.println(", calculating gyro bias");*/

            sampleCount = 0;
            lastRate = now;
        }
        if ((now - lastDisplay) >= DISPLAY_INTERVAL) {

            lastDisplay = now;
//          RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
            RTMath::display("Accel1:", (RTVector3&)imu->getAccel());              // accel data
//          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
            RTMath::displayRollPitchYaw("Pose1:", (RTVector3&)fusion.getFusionPose()); // fused output
           Serial.println();

        }
    }

    out2();


   while (imu2->IMURead()) {  //Serial.println("ciao ciao");                    // get the latest data if ready yet
        // this flushes remaining data in case we are falling behind

       if (++loopCount2 >= 3)
           continue;

        fusion2.newIMUData(imu2->getGyro(), imu2->getAccel(), imu2->getCompass(), imu2->getTimestamp());

        sampleCount2++;
        if ((delta2 = now2 - lastRate2) >= 1000) {
  /*          Serial.print("Sample rate: "); Serial.print(sampleCount);
            if (imu->IMUGyroBiasValid())
                Serial.println(", gyro bias valid");
            else
                Serial.println(", calculating gyro bias");*/

            sampleCount2 = 0;
            lastRate2 = now2;
        }
        if ((now2 - lastDisplay2) >= DISPLAY_INTERVAL) {
            lastDisplay2 = now2;
//          RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
          RTMath::display("Accel2:", (RTVector3&)imu2->getAccel());              // accel data
//          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
          RTMath::displayRollPitchYaw("Pose2:", (RTVector3&)fusion2.getFusionPose()); // fused output
           Serial.println();


        }
   }

     out3();


   while (imu3->IMURead()) {   //  Serial.println("ciao ciao");                 // get the latest data if ready yet
       // this flushes remaining data in case we are falling behind
      if (++loopCount3 >= 10)
           continue;
      fusion3.newIMUData(imu3->getGyro(), imu3->getAccel(), imu3->getCompass(), imu3->getTimestamp());
        sampleCount3++;
       if ((delta3 = now3 - lastRate3) >= 1000) {
 /*          Serial.print("Sample rate: "); Serial.print(sampleCount);
          if (imu->IMUGyroBiasValid())
               Serial.println(", gyro bias valid");
          else
               Serial.println(", calculating gyro bias");*/

          sampleCount3 = 0;
           lastRate3 = now3;
        }
       if ((now3 - lastDisplay3) >= DISPLAY_INTERVAL) {
            lastDisplay3 = now3;
         RTMath::display("Gyro:", (RTVector3&)imu->getGyro());                // gyro data
          RTMath::display("Accel3:", (RTVector3&)imu3->getAccel());              // accel data
          RTMath::display("Mag:", (RTVector3&)imu->getCompass());              // compass data
           RTMath::displayRollPitchYaw("Pose3:", (RTVector3&)fusion3.getFusionPose()); // fused output
         Serial.println();
        }
   }


}