Time of flight sensor program stops working with FreeRTOS

dexterbinary · May 11, 2024, 9:58am

Hey all,

Hopefully I have setup this question correctly. I have been trying to implement the Arduino FreeRTOS into my robotics project but have run into some issues. The purpose of my code is to read the distance from 4 time of flight sensors on a robot and output them onto the serial monitor (see working code). I tried to implement the FreeRTOS into my code but now I dont get any serial outputs, i.e my serial monitor is blank.

So far I've tried working up from scratch with the code (starting from a blank working FreeRTOS project and slowly adding my code bit by bit) to see what breaks it, however what occurs is at a certain point of adding code my serial monitor displays the first string I print cut of and repeating (i.e Serial.println("Serial Started") is my first serial print and my monitor outputs Serial StSerialStSerialSt etc)

Any assistance is greatly appreciated, I am rather new to Arduino and the forums so any help on formatting my posts better is also welcome.

WORKING CODE WITHOUT RTOS:

/*
SHT = Shutdown Pin
*/

#include "Adafruit_VL53L0X.h"

#define FL_TOF_ADRESS 0x2A
#define FR_TOF_ADRESS 0x2D
#define BL_TOF_ADRESS 0x2C
#define BR_TOF_ADRESS 0x19

#define SHT_FL_TOF 10
#define SHT_FR_TOF 9
#define SHT_BL_TOF 5
#define SHT_BR_TOF 7

Adafruit_VL53L0X frontLeftTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X frontRightTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X backLeftTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X backRightTOF = Adafruit_VL53L0X();

VL53L0X_RangingMeasurementData_t distFL;
VL53L0X_RangingMeasurementData_t distFR;
VL53L0X_RangingMeasurementData_t distBL;
VL53L0X_RangingMeasurementData_t distBR;

void setRangingSensorAdresses() {
  // all reset
    digitalWrite(SHT_FL_TOF, LOW);    
    digitalWrite(SHT_FR_TOF, LOW);
    digitalWrite(SHT_BL_TOF, LOW);
    digitalWrite(SHT_BR_TOF, LOW);
    delay(10);
  //

  // all unreset
    digitalWrite(SHT_FL_TOF, HIGH);
    digitalWrite(SHT_FR_TOF, HIGH);
    digitalWrite(SHT_BL_TOF, HIGH);
    digitalWrite(SHT_BR_TOF, HIGH);
    delay(10);
  //

  //Front Left TOF sensor intitialisation
    digitalWrite(SHT_FL_TOF, HIGH);
    digitalWrite(SHT_FR_TOF, LOW);
    digitalWrite(SHT_BL_TOF, LOW);
    digitalWrite(SHT_BR_TOF, LOW);

    if(!frontLeftTOF.begin(FL_TOF_ADRESS)) {
      Serial.println(F("Failed to boot first VL53L0X"));
      while(1);
    }
    delay(10);
  //

  //Front Right TOF sensor intitialisation
    digitalWrite(SHT_FR_TOF, HIGH);
    delay(10);
    if(!frontRightTOF.begin(FR_TOF_ADRESS)) {
      Serial.println(F("Failed to boot second VL53L0X"));
      while(1);
    }
  //

  //Back Left TOF sensor intitialisation
    digitalWrite(SHT_BL_TOF, HIGH);
    delay(10);
    if(!backLeftTOF.begin(BL_TOF_ADRESS)) {
      Serial.println(F("Failed to boot third VL53L0X"));
      while(1);
    }
  //

  //Back Right TOF sensor intitialisation
    digitalWrite(SHT_BR_TOF, HIGH);
    delay(10);
    if(!backRightTOF.begin(BR_TOF_ADRESS)) {
      Serial.println(F("Failed to boot fourth VL53L0X"));
      while(1);
    }
  //
}

void readSensors() {
  
  frontLeftTOF.rangingTest(&distFL, false); // pass in 'true' to get debug data printout!
  frontRightTOF.rangingTest(&distFR, false);
  backLeftTOF.rangingTest(&distBL, false); 
  backRightTOF.rangingTest(&distBR, false); 

  // print sensor one reading
  Serial.print(F("FL: "));
  if(distFL.RangeStatus != 4) {     // if not out of range
    Serial.print(distFL.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.print(F(" "));

  // print sensor two reading
  Serial.print(F("FR: "));
  if(distFR.RangeStatus != 4) {
    Serial.print(distFR.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }

  Serial.print(F(" "));
  // print sensor two reading
  Serial.print(F("BL: "));
  if(distBL.RangeStatus != 4) {
    Serial.print(distBL.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }

  Serial.print(F(" "));
  // print sensor two reading
  Serial.print(F("BR: "));
  if(distBR.RangeStatus != 4) {
    Serial.print(distBR.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }  
  
  Serial.println();
}

void setup() {
//Serial
  Serial.begin(115200);
  while (! Serial) { delay(1); }
  delay(100);
  Serial.println(F("Serial initialised"));
//

//TOF Sensor intitialisation
  pinMode(SHT_FL_TOF, OUTPUT);
  pinMode(SHT_FR_TOF, OUTPUT);
  pinMode(SHT_BL_TOF, OUTPUT);
  pinMode(SHT_BR_TOF, OUTPUT);

  Serial.println(F("Shutdown pins initialised"));

  digitalWrite(SHT_FL_TOF, LOW);
  digitalWrite(SHT_FR_TOF, LOW);
  digitalWrite(SHT_BL_TOF, LOW);
  digitalWrite(SHT_BR_TOF, LOW);

  Serial.println(F("Shutdown pins Activated"));

  setRangingSensorAdresses();
//
}

void loop() {
  readSensors();
  delay(10);
}

NOT WORKING CODE WITH RTOS:

/*
SHT = Shutdown Pin
*/
#include <Arduino_FreeRTOS.h>
#include "Adafruit_VL53L0X.h"

#define FL_TOF_ADRESS 0x2A
#define FR_TOF_ADRESS 0x2D
#define BL_TOF_ADRESS 0x2C
#define BR_TOF_ADRESS 0x19

#define SHT_FL_TOF 10
#define SHT_FR_TOF 9
#define SHT_BL_TOF 5
#define SHT_BR_TOF 7

Adafruit_VL53L0X frontLeftTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X frontRightTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X backLeftTOF = Adafruit_VL53L0X();
Adafruit_VL53L0X backRightTOF = Adafruit_VL53L0X();

VL53L0X_RangingMeasurementData_t distFL;
VL53L0X_RangingMeasurementData_t distFR;
VL53L0X_RangingMeasurementData_t distBL;
VL53L0X_RangingMeasurementData_t distBR;

void setup() {

  Serial.begin(115200);
  delay(100);
  Serial.println("Serial initialised");

  pinMode(SHT_FL_TOF, OUTPUT);
  pinMode(SHT_FR_TOF, OUTPUT);
  pinMode(SHT_BL_TOF, OUTPUT);
  pinMode(SHT_BR_TOF, OUTPUT);

  Serial.println(F("Shutdown pins initialised"));

  digitalWrite(SHT_FL_TOF, LOW);
  digitalWrite(SHT_FR_TOF, LOW);
  digitalWrite(SHT_BL_TOF, LOW);
  digitalWrite(SHT_BR_TOF, LOW);

  Serial.println(F("Shutdown pins Activated"));

  setRangingSensorAdresses();

  xTaskCreate(sensing, "Sensing", 256, NULL, 1, NULL);
  xTaskCreate(myLoop, "Loop", 256, NULL, 2, NULL);
  delay(200);
  vTaskStartScheduler();
}

void setRangingSensorAdresses() {
  // all reset
    digitalWrite(SHT_FL_TOF, LOW);    
    digitalWrite(SHT_FR_TOF, LOW);
    digitalWrite(SHT_BL_TOF, LOW);
    digitalWrite(SHT_BR_TOF, LOW);
    delay(10);
  //

  // all unreset
    digitalWrite(SHT_FL_TOF, HIGH);
    digitalWrite(SHT_FR_TOF, HIGH);
    digitalWrite(SHT_BL_TOF, HIGH);
    digitalWrite(SHT_BR_TOF, HIGH);
    delay(10);
  //

  //Front Left TOF sensor intitialisation
    digitalWrite(SHT_FL_TOF, HIGH);
    digitalWrite(SHT_FR_TOF, LOW);
    digitalWrite(SHT_BL_TOF, LOW);
    digitalWrite(SHT_BR_TOF, LOW);

    if(!frontLeftTOF.begin(FL_TOF_ADRESS)) {
      Serial.println(F("Failed to boot first VL53L0X"));
      while(1);
    }
    delay(10);
  //

  //Front Right TOF sensor intitialisation
    digitalWrite(SHT_FR_TOF, HIGH);
    delay(10);
    if(!frontRightTOF.begin(FR_TOF_ADRESS)) {
      Serial.println(F("Failed to boot second VL53L0X"));
      while(1);
    }
  //

  //Back Left TOF sensor intitialisation
    digitalWrite(SHT_BL_TOF, HIGH);
    delay(10);
    if(!backLeftTOF.begin(BL_TOF_ADRESS)) {
      Serial.println(F("Failed to boot third VL53L0X"));
      while(1);
    }
  //

  //Back Right TOF sensor intitialisation
    digitalWrite(SHT_BR_TOF, HIGH);
    delay(10);
    if(!backRightTOF.begin(BR_TOF_ADRESS)) {
      Serial.println(F("Failed to boot fourth VL53L0X"));
      while(1);
    }
  //
}

void readSensors() {
  
  frontLeftTOF.rangingTest(&distFL, false); // pass in 'true' to get debug data printout!
  frontRightTOF.rangingTest(&distFR, false);
  backLeftTOF.rangingTest(&distBL, false); 
  backRightTOF.rangingTest(&distBR, false); 

  // print sensor one reading
  Serial.print(F("FL: "));
  if(distFL.RangeStatus != 4) {     // if not out of range
    Serial.print(distFL.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }
  
  Serial.print(F(" "));

  // print sensor two reading
  Serial.print(F("FR: "));
  if(distFR.RangeStatus != 4) {
    Serial.print(distFR.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }

  Serial.print(F(" "));
  // print sensor two reading
  Serial.print(F("BL: "));
  if(distBL.RangeStatus != 4) {
    Serial.print(distBL.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }

  Serial.print(F(" "));
  // print sensor two reading
  Serial.print(F("BR: "));
  if(distBR.RangeStatus != 4) {
    Serial.print(distBR.RangeMilliMeter);
  } else {
    Serial.print(F("Out of range"));
  }  
  
  Serial.println();
}

void sensing(void *pvParameters) {
  for(;;) {
    readSensors();
    delay(150);
  }
}

void myLoop(void *pvParameters) {
  for(;;){
  delay(10);
  Serial.println(F("Test"));
  }
}

void loop(){
  for(;;){

  }
}

J-M-L · May 11, 2024, 11:55am

Serial is a shared object between your tasks. You need to use it within a critical section

github.com

feilipu/Arduino_FreeRTOS_Library/blob/19e621e9385f0a864d13ca332e0c7bbd034b3c24/examples/AnalogRead_DigitalRead/AnalogRead_DigitalRead.ino#L4-L7


      
          // Declare a mutex Semaphore Handle which we will use to manage the Serial Port.
          // It will be used to ensure only one Task is accessing this resource at any time.
          SemaphoreHandle_t xSerialSemaphore;

see how they do that in AnalogRead_DigitalRead.ino

system · November 7, 2024, 11:56am

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Arduino Rtos problem General Guidance	10	367	June 20, 2024
Arduino UNO R4 - FreeRTOS Time Slicing Software	9	213	February 26, 2026
Arduino UNO R4 - "core" >= 1.3.1 - FreeRTOS™ Software	10	320	September 24, 2025
MPU9250 Library and FreeRTOS Conflict: Sensor Readings Fail in Task but Work in Loop Sensors	7	114	January 20, 2025
Porting FreeRTOS to Arduino Mega2560 – Anyone Tried? Programming	10	244	February 11, 2026

Time of flight sensor program stops working with FreeRTOS

Related topics