I think this worked but it still freezes sometimes at the beginning.
Let me upload my whole code for better analysis.
#include "Adafruit_VL53L0X.h"
#include <FreeRTOS.h>
#include <task.h>
#include "semphr.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LOX1_ADDRESS 0x30
#define LOX2_ADDRESS 0x31
#define SHT_LOX1 6
#define SHT_LOX2 9
#define TIMER_INTERRUPT_DEBUG 1
#define _TIMERINTERRUPT_LOGLEVEL_ 4
#include "RPi_Pico_TimerInterrupt.h"
uint8_t sayac = 0;
RPI_PICO_Timer ITimer1(1);
#define TIMER1_INTERVAL_MS 1000
bool TimerHandler1(struct repeating_timer *t) {
(void)t;
static bool toggle1 = false;
#if (TIMER_INTERRUPT_DEBUG > 0)
//timer interrupt toggles pin outputPin1
sayac++;
#endif
return true;
}
Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();
VL53L0X_RangingMeasurementData_t measure1;
VL53L0X_RangingMeasurementData_t measure2;
MPU6050 mpu;
MPU6050 mpu2(0x69);
#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
#define INTERRUPT_PIN2 3
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
bool blinkState = false;
bool dmpReady = false;
bool dmp2Ready = false;
uint8_t mpuIntStatus;
uint8_t mpu2IntStatus;
uint8_t devStatus;
uint8_t dev2Status;
uint16_t packetSize;
uint16_t packetSize2;
uint8_t fifoBuffer[64];
uint8_t fifoBuffer2[64];
// orientation/motion vars
Quaternion q, q2;
VectorInt16 aa, aa2;
VectorInt16 aaReal, aaReal2;
VectorInt16 aaWorld, aaWorld2;
VectorFloat gravity, gravity2;
float euler[3], euler2[3];
float ypr[3], ypr2[3];
volatile bool mpuInterrupt = false;
volatile bool mpu2Interrupt = false;
void dmpDataReady() {
mpuInterrupt = true;
}
void dmpDataReady2() {
mpu2Interrupt = true;
}
static SemaphoreHandle_t mutex;
void setup() {
mutex = xSemaphoreCreateMutex();
xTaskCreate(tof, "TOF", 1024, nullptr, 1, nullptr);
xTaskCreate(blink, "BLINK", 128, nullptr, 1, nullptr);
xTaskCreate(mputask, "MPUTASK", 1024, nullptr, 2, nullptr);
pinMode(SHT_LOX1, OUTPUT);
pinMode(SHT_LOX2, OUTPUT);
Serial1.println(F("Shutdown pins inited..."));
if (ITimer1.attachInterruptInterval(TIMER1_INTERVAL_MS * 1000, TimerHandler1)) {
Serial1.print(F("Starting ITimer1 OK, millis() = "));
Serial1.println(millis());
#if (TIMER_INTERRUPT_DEBUG > 1)
Serial1.print(F("OutputPin1 = "));
Serial1.print(outputPin1);
#endif
} else
Serial1.println(F("Can't set ITimer1. Select another freq. or timer"));
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
}
void blink(void *param) {
(void)param;
pinMode(LED_BUILTIN, OUTPUT);
while (1) {
digitalWrite(LED_BUILTIN, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
digitalWrite(LED_BUILTIN, HIGH);
vTaskDelay(100 / portTICK_PERIOD_MS);
}
}
void read_dual_sensors() {
lox1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
lox2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
// print sensor one reading
Serial1.print(F("TOF1: "));
if (measure1.RangeStatus != 4) { // if not out of range
Serial1.print(measure1.RangeMilliMeter);
} else {
Serial1.print(F("Out of range"));
}
Serial1.print(F(" "));
// print sensor two reading
Serial1.print(F("TOF2: "));
if (measure2.RangeStatus != 4) {
Serial1.print(measure2.RangeMilliMeter);
} else {
Serial1.print(F("Out of range"));
}
Serial1.println();
}
void init() {
setID();
Serial1.println("initlendim");
}
void oku() {
if ((Serial1.available() && Serial1.read())) {
int h;
while (h = Serial1.available() > 0) {
while (h--) Serial1.read();
}
sayac = 0;
init();
}
if (sayac <= 20) {
vTaskDelay(10 / portTICK_PERIOD_MS);
read_dual_sensors();
}
if (sayac > 20) {
lox1.stopMeasurement();
lox2.stopMeasurement();
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
}
}
void setID() {
// all reset
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(10 / portTICK_PERIOD_MS);
// all unreset
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, HIGH);
vTaskDelay(10 / portTICK_PERIOD_MS);
// activating LOX1 and resetting LOX2
digitalWrite(SHT_LOX1, HIGH);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
// initing LOX1
if (!lox1.begin(LOX1_ADDRESS, 0, &Wire)) {
Serial1.println(F("Failed to boot first VL53L0X"));
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
setID();
}
vTaskDelay(10 / portTICK_PERIOD_MS);
// activating LOX2
digitalWrite(SHT_LOX2, HIGH);
vTaskDelay(100 / portTICK_PERIOD_MS);
//initing LOX2
if (!lox2.begin(LOX2_ADDRESS, 0, &Wire)) {
Serial1.println(F("Failed to boot second VL53L0X"));
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(100 / portTICK_PERIOD_MS);
setID();
}
vTaskDelay(10 / portTICK_PERIOD_MS);
}
void tof(void *param) {
(void)param;
Wire.setSDA(4);
Wire.setSCL(5);
Wire.begin();
Serial1.begin(115200);
digitalWrite(SHT_LOX1, LOW);
digitalWrite(SHT_LOX2, LOW);
vTaskDelay(50 / portTICK_PERIOD_MS);
Serial1.println(F("Both in reset mode...(pins are low)"));
Serial1.println(F("Starting..."));
setID();
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) { // mutex kilidini almaya çalış
oku();
vTaskDelay(100 / portTICK_PERIOD_MS);
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void mputask(void *param) {
(void)param;
Serial1.println(F("Initializing I2C devices..."));
mpu.initialize();
mpu2.initialize();
pinMode(INTERRUPT_PIN, INPUT);
pinMode(INTERRUPT_PIN2, INPUT);
Serial1.println(F("Testing device connections..."));
Serial1.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial1.println(mpu2.testConnection() ? F("MPU6050 2 connection successful") : F("MPU6050 2 connection failed"));
Serial1.println(F("\nSend any character to begin DMP programming and demo: "));
//while (Serial1.available() && Serial1.read())
// ;
//while (!Serial1.available())
// ;
//while (Serial1.available() && Serial1.read())
// ;
Serial1.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
dev2Status = mpu2.dmpInitialize();
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
mpu2.setXGyroOffset(220);
mpu2.setYGyroOffset(76);
mpu2.setZGyroOffset(-85);
mpu2.setZAccelOffset(1788);
if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
Serial1.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
Serial1.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial1.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial1.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
Serial1.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial1.print(F("DMP Initialization failed (code "));
Serial1.print(devStatus);
Serial1.println(F(")"));
}
if (dev2Status == 0) {
mpu2.CalibrateAccel(6);
mpu2.CalibrateGyro(6);
mpu2.PrintActiveOffsets();
Serial1.println(F("Enabling DMP..."));
mpu2.setDMPEnabled(true);
Serial1.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial1.print(digitalPinToInterrupt(INTERRUPT_PIN2));
Serial1.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN2), dmpDataReady2, RISING);
mpu2IntStatus = mpu2.getIntStatus();
Serial1.println(F("DMP ready! Waiting for first interrupt..."));
dmp2Ready = true;
packetSize2 = mpu2.dmpGetFIFOPacketSize();
} else {
Serial1.print(F("DMP Initialization failed (code "));
Serial1.print(dev2Status);
Serial1.println(F(")"));
}
while (1) {
if (xSemaphoreTake(mutex, portMAX_DELAY) == pdTRUE) {
vTaskDelay(500 / portTICK_PERIOD_MS);
// if programming failed, don't try to do anything
if (!dmpReady) return;
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial1.print("MPU1\t");
Serial1.print(ypr[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr[2] * 180 / M_PI);
#endif
}
/////////////////////////////////////////////////////////////////////////// 2. MPU veri alımı ////////////////////////////////////////////////////////////////////////////
Serial1.print("\r");
if (!dmp2Ready) return;
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL
mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
mpu2.dmpGetGravity(&gravity2, &q2);
mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
Serial1.print("MPU2\t");
Serial1.print(ypr2[0] * 180 / M_PI);
Serial1.print("\t");
Serial1.print(ypr2[1] * 180 / M_PI);
Serial1.print("\t");
Serial1.println(ypr2[2] * 180 / M_PI);
#endif
}
xSemaphoreGive(mutex); // mutex kilidini serbest bırak
}
}
}
void loop() {
// put your main code here, to run repeatedly:
}