FreeRTOS acting strange inside C++ Wrappers

Hello everyone,

 I am trying to build simple radar with servo motor and ultrasonic sensor. When I do not use C++ wrappers code works just fine.



[code language="cpp"]
#include <Servo.h>
#include <Arduino_FreeRTOS.h>
#include <Radar.h>
#include <UltrasonicSensor.h>

#define trigPin 52
#define echoPin 53
#define servoPin 10

Servo servoMotor;
UltrasonicSensor ultrasonicSensor(trigPin, echoPin);
Radar radar(&ultrasonicSensor, &servoMotor);

void ultrasonicSensorThread(void* pvParameters)
{
  uint8_t distance;
  
  while (true)
  {
    ultrasonicSensor.read(&distance);
    Serial.print("Distance: ");
    Serial.println(distance);
  }
}

void servoThread(void* pvParameters)
{
  uint8_t angle;
  
  while (true)
  {
    for ( angle = 0; angle < 180; ++angle )
    {
      servoMotor.write(angle);
      vTaskDelay(20 / portTICK_PERIOD_MS);
    }
    
    for ( angle = 180; angle > 0; --angle )
    {
      servoMotor.write(angle);
      vTaskDelay(20 / portTICK_PERIOD_MS);
    }
  }
}


void setup()
{
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servoMotor.attach(servoPin);

  xTaskCreate(
  servoThread
  ,  (const portCHAR *)"Servo"   // A name just for humans
  ,  128  // Stack size
  ,  NULL
  ,  2  // priority
  ,  NULL );
  
  xTaskCreate(
  ultrasonicSensorThread
  ,  (const portCHAR *)"Ultrasonic"   // A name just for humans
  ,  128  // Stack size
  ,  NULL
  ,  2  // priority
  ,  NULL );

  vTaskStartScheduler();
}

void loop() 
{
    
}

When I try to use the same code inside cpp wrappers, my servo doesn’t work propery (moving only clockwise) and also ultrasonic detects objects with delay.

#include <Servo.h>
#include <Arduino_FreeRTOS.h>
#include <Radar.h>
#include <UltrasonicSensor.h>

#define trigPin 52
#define echoPin 53
#define servoPin 10

Servo servoMotor;
UltrasonicSensor ultrasonicSensor(trigPin, echoPin);
Radar radar(&ultrasonicSensor, &servoMotor);

void radarThread(void* pvParameters)
{
  uint8_t distance;
  uint8_t angle;
  
  while (true)
  {
    radar.read(&distance, &angle);
    Serial.print("Distance: ");
    Serial.println(distance);
    Serial.print("Angle: ");
    Serial.println(angle);
  }
}

void setup()
{
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  servoMotor.attach(servoPin);

  xTaskCreate(
  radarThread
  ,  (const portCHAR *)"Raar"   // A name just for humans
  ,  128  // Stack size
  ,  NULL
  ,  2  // priority
  ,  NULL );
  
  vTaskStartScheduler();
}

void loop() 
{
    
}

Here is the Radar class


#ifndef RADAR_H
#define RADAR_H

#include <stdint.h>

class Servo;
class UltrasonicSensor;

class Radar
{
private:
UltrasonicSensor* ultrasonicSensor;
Servo* servo;
static Radar* sInstance;
uint8_t objectDistance;
uint8_t objectAngle;

public:
Radar(UltrasonicSensor* ultrasonicSensor, Servo* servo);
static int8_t read(uint8_t* distance, uint8_t* angleInDegres);

private:
static void ultrasonicThread(void* parameter);
static void servoThread(void* parameter);
};

#endif

/*
 * Radar.cpp
 *
 * Created: 7/30/2017 2:06:20 PM
 *  Author: fairenough
 */ 

#include <util/delay.h>
#include "Radar.h"
#include <Arduino_FreeRTOS.h>
#include <Servo.h>
#include "UltrasonicSensor.h"

Radar* Radar::sInstance = nullptr;

Radar::Radar(UltrasonicSensor* ultrasonicSensor, Servo* servo)
{
	this->ultrasonicSensor = ultrasonicSensor;
	this->servo = servo;
	
	this->objectAngle = 0;
	this->objectDistance = 0;
	
	Radar::sInstance = this;
		
	xTaskCreate(
	Radar::ultrasonicThread
	,  NULL  // A name just for humans
	,  128  // Stack size
	,  NULL
	,  2  // priority
	,  NULL );
	
	xTaskCreate(
	Radar::servoThread
	,  NULL  // A name just for humans
	,  128  // Stack size
	,  NULL
	,  2  // priority
	,  NULL );
}

int8_t Radar::read(uint8_t* distance, uint8_t* angleInDegres)
{	
	*angleInDegres = Radar::sInstance->objectAngle;	
	*distance = Radar::sInstance->objectDistance;
	
	return 0;
}

void Radar::ultrasonicThread(void* parameter)
{
	while (true)
	{
		Radar::sInstance->ultrasonicSensor->read( &(Radar::sInstance->objectDistance) );
	}
}

void Radar::servoThread(void* parameter)
{
	int8_t angle;
	
	while (true)
	{
		for ( angle = 0; angle < 180; ++angle )
		{
		  Radar::sInstance->servo->write(angle);
		  Radar::sInstance->objectAngle = angle;
		  vTaskDelay(20 / portTICK_PERIOD_MS);
		}
		
		for ( angle = 180; angle > 0; --angle )
		{
		  Radar::sInstance->servo->write(angle);
		  Radar::sInstance->objectAngle = angle;
		  vTaskDelay(20 / portTICK_PERIOD_MS);
		}
	}
}

Do you actually need to use the RTOS library ?

UKHeliBob:
Do you actually need to use the RTOS library ?

Yes sir, this is actually just a part of the program, I have like 40 classes inside my project this like 1% of it. I need multitasking for the following reasons:

  1. I am reading data from 6 sensors
  2. I am using encoding data that is being received from the sensors
  3. I am sending encoded data via bluetooth back to my C# windows app
  4. I am receviving encoded data via bluetooth and decoding it
  5. I am then using MVC design pattern to control eletrical vehicle speed, direction etc...

So you see there are a lot of things going on here. I am sure I forgot to mention something.

Best regards!

You can do multi-tasking without FreeRTOS. Have a look at the demo Several Things at a Time

...R

I need multitasking for the following reasons:
...
So you see there are a lot of things going on here.

Nope. You could easily do all of that without the added complexity of an RTOS. You can save a lot of program space, some RAM and MCU time, and a lot of frustration by not using an RTOS.

And frankly, if you can't format your post, there's little chance that you will be able to choreograph an embedded RTOS. Like Robin2 suggested, take a look at the General Design section of the Useful Links page.

Robin2:
You can do multi-tasking without FreeRTOS. Have a look at the demo Several Things at a Time

...R

Yes I know about that trick using millis, but some of AVR uCs I use do not support arduino.
I do not know why i cannot put code insode blocks like in other forums.

Code in a code block ?

Serial.println("Easy Peasy");

Select the code then click the code block icon which you obviously found.

UKHeliBob:
Code in a code block ?

Serial.println("Easy Peasy");

Select the code then click the code block icon which you obviously found.

I didn’t use the icon but I formatted text using blocks, that is what I was referring to.

This forum doesn't support parameters for the code-tag and weirdly / annoyingly doesn't format C/C++...

You can format it yourself by typing in the [­code] and ­[­/code] tags yourself if you want

EDIT: I fixed it, it was my copy pasting …

One more question, before I used static class pointer to access class member fields inside task functions, this time I tried passing this pointer as a parameter in xTaskCreate function, and even after casting void pointer into Radar class pointer, but I keep getting error: ‘void*’ is not a pointer-to-object type (ultrasonicThread & servoThread methods)

Radar::Radar(UltrasonicSensor* ultrasonicSensor, Servo* servo)
{
	this->ultrasonicSensor = ultrasonicSensor;
	this->servo = servo;
	
	this->objectAngle = 0;
	this->objectDistance = 0;
	
	xTaskCreate(
	Radar::ultrasonicThread
	,  NULL  // A name just for humans
	,  128  // Stack size
	,  this
	,  2  // priority
	,  NULL );
	
	xTaskCreate(
	Radar::servoThread
	,  NULL  // A name just for humans
	,  128  // Stack size
	,  this
	,  2  // priority
	,  NULL );
}

void Radar::ultrasonicThread(void* parameter)
{
	Radar* instance;
	
	instance = static_cast<class Radar*>(parameter);
	
	while (true)
	{
		parameter->ultrasonicSensor->read( &(instance->objectDistance) );
	}
}

void Radar::servoThread(void* parameter)
{
	int8_t angle;
	Radar* instance;
	
	instance = static_cast<class Radar*>(parameter);
	
	while (true)
	{
		for ( angle = 0; angle < 180; ++angle )
		{
		  instance->servo->write(angle);
		  instance->objectAngle = angle;
		  vTaskDelay(20 / portTICK_PERIOD_MS);
		}
		
		for ( angle = 180; angle > 0; --angle )
		{
		  instance->servo->write(angle);
		  instance->objectAngle = angle;
		  vTaskDelay(20 / portTICK_PERIOD_MS);
		}
	}
}