Hello! This is proving to be a miserable fruitless battle. Can you please help? I'm reading two BNO055 sensors and two TOF sensors. One BNO055 and one TOF on same bus. The pairs of sensors need to be on different i2c buses and I have no idea what I'm doing wrong. In this example I'm only showing the TOF for testing purposes. Please save me!!! Thank you in advance!
Marc
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <VL53L1X.h>
VL53L1X leftSensor;
VL53L1X rightSensor;
/* Set the delay between fresh samples */
#define BNO055_SAMPLERATE_DELAY_MS (50)
Adafruit_BNO055 leftIMU = Adafruit_BNO055(55, 0x29, &Wire);
Adafruit_BNO055 rightIMU = Adafruit_BNO055(55, 0x29, &Wire1);
float leftPitchMeasured;
float leftPitchFilteredOld=0;
float leftPitchFilteredNew;
float rightPitchMeasured;
float rightPitchFilteredOld=0;
float rightPitchFilteredNew;
float leftRollMeasured;
float leftRollFilteredOld=0;
float leftRollFilteredNew;
float rightRollMeasured;
float rightRollFilteredOld=0;
float rightRollFilteredNew;
float leftPitch;
float leftRoll;
float rightPitch;
float rightRoll;
float leftDistance;
float rightDistance;
void setup(void)
{
Serial.begin(115200);
Serial1.begin(115200);
leftIMU.begin();
rightIMU.begin();
leftIMU.setExtCrystalUse(true); //use crystal on board not in chip
rightIMU.setExtCrystalUse(true); //use crystal on board not in chip
delay(1000);
Wire.begin();
Wire.setClock(400000);
Wire.beginTransmission(0x28);
Wire1.begin();
Wire1.setClock(400000);
Wire1.beginTransmission(0x29);
leftSensor.setTimeout(500);
rightSensor.setTimeout(500);
if (!leftSensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
if (!rightSensor.init())
{
Serial.println("Failed to detect and initialize sensor!");
while (1);
}
leftSensor.setDistanceMode(VL53L1X::Long);
leftSensor.setMeasurementTimingBudget(50000);
leftSensor.startContinuous(50);
rightSensor.setDistanceMode(VL53L1X::Long);
rightSensor.setMeasurementTimingBudget(50000);
rightSensor.startContinuous(50);
}
void loop(void)
{
leftDistance = (leftSensor.read());
rightDistance = (rightSensor.read());
imu::Vector<3> leftAcc = leftIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> rightAcc = rightIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
leftPitchMeasured=atan2(leftAcc.x()/9.8,leftAcc.z()/9.8)/2/3.14159*360; //converted to degrees
// next that value needs to be be LOW PASS FILTERED
leftPitchFilteredNew=.75*leftPitchFilteredOld+.25*leftPitchMeasured;
//roll=atan2(acc.y()/9.8, acc.z()/9.8); //result in radians
leftRollMeasured=atan2(leftAcc.y()/9.8, leftAcc.z()/9.8)/2/3.14159*360; //converted to degrees
// next that value needs to be be LOW PASS FILTERED
leftRollFilteredNew=.75*leftRollFilteredOld+.25*leftRollMeasured;
//========================================
rightPitchMeasured=atan2(rightAcc.x()/9.8,rightAcc.z()/9.8)/2/3.14159*360; //converted to degrees
// next that value needs to be be LOW PASS FILTERED
rightPitchFilteredNew=.75*rightPitchFilteredOld+.25*rightPitchMeasured;
//roll=atan2(acc.y()/9.8, acc.z()/9.8); //result in radians
rightRollMeasured=atan2(rightAcc.y()/9.8, rightAcc.z()/9.8)/2/3.14159*360; //converted to degrees
// next that value needs to be be LOW PASS FILTERED
rightRollFilteredNew=.75*rightRollFilteredOld+.25*rightRollMeasured;
//========================================
/*
if (leftPitchFilteredNew > 0)
{
leftPitchFilteredNew = leftPitchFilteredNew * -1;
}
else
{leftPitchFilteredNew = abs(leftPitchFilteredNew);
}
*/
/*
Serial.print(leftPitchFilteredNew);
Serial.print(",");
Serial.print(leftRollFilteredNew);
Serial.print(",");
Serial.println(leftDistance);
Serial1.print(leftPitchFilteredNew);
Serial1.print(",");
Serial1.print(leftRollFilteredNew);
Serial1.print(",");
Serial1.println(leftDistance);
*/
Serial.print(leftDistance);
Serial.print("========");
Serial.println(rightDistance);
leftPitchFilteredOld=leftPitchFilteredNew;
leftRollFilteredOld=leftRollFilteredNew;
delay(BNO055_SAMPLERATE_DELAY_MS);
}
