Hi guys, im having trouble to configure the servo motors with FreeRTOS. How can i correctly configure the delay for the Servo? In this case, i want the Servo to stay at the same position, but when i start up the servo with FreeRTOS he keeps shaking because the time period isnt correct.
1 Like
Hi @NathanEspindola,
Please provide more context on how you are using FreeRTOS in your project to help better understand your question, including what each tasks does, their priorities, how ISRs are handled, time period requirements, etc.
Hi @tony-josi-aws ,
I am working with 6 Servo Motors, with 1 Task for each of them.
At this moment, i just need to make them stand still at a position without shaking.
Heres the code:
// P I N M A N A G E R
// 12 - Servo1
// 13 - Servo2
// 15 - Servo3
// 2
// 17
// 22 - Ultrasonic Trig
// 21 - Ultrasonic Echo
// 27 - Servo4
// 26 - Servo5
// 25 - Servo6
// 33
// 32
// SVN
// 38
// 37
// SVP
//#include <Arduino.h>
#include <ESP32Servo.h>
#include <TFT_eSPI.h>
#include <SPI.h>
#include <Ultrasonic.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#define configCPU_CLOCK_HZ 240000
#define configTICK_RATE_HZ 50
// DISPLAY ================================================================
TFT_eSPI tft = TFT_eSPI(); // Invoke library, pins defined in User_Setup.h
boolean initial = 1;
byte xcolon = 0;
unsigned int colour = 0;
void displaySetup(void){
tft.init();
tft.setRotation(0);
tft.fillScreen(TFT_BLACK);
tft.setTextColor(TFT_YELLOW, TFT_BLACK); // Note: the new fonts do not draw the background colour
}
// SERVO ================================================================
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
int minUs = 500;
int maxUs = 2500;
int servo1Pin = 12;
int servo2Pin = 13;
int servo3Pin = 15;
int servo4Pin = 27;
int servo5Pin = 26;
int servo6Pin = 25;
ESP32PWM pwm;
//void vInitHW(void);
void vInitHW(void)
{
Serial.begin(115200);
// Allow allocation of all timers
ESP32PWM::allocateTimer(0);
ESP32PWM::allocateTimer(1);
ESP32PWM::allocateTimer(2);
ESP32PWM::allocateTimer(3);
servo1.setPeriodHertz(250);
servo2.setPeriodHertz(250);
servo3.setPeriodHertz(250);
servo4.setPeriodHertz(250);
servo5.setPeriodHertz(250);
servo6.setPeriodHertz(250);
}
// ULTRASONIC ================================================================
const int echoPin = 21;
const int trigPin = 22;
#define SOUND_SPEED 0.034
long duration;
float distance;
void ultrasonicSetup(void)
{
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
}
// RTOS TASKS ================================================================
TaskHandle_t xTask1Handle,xTask2Handle, xTask3Handle, xTask4Handle;
TaskHandle_t xTask5Handle,xTask6Handle, xTask7Handle, xTask8Handle;
//TaskHandle_t xTask9Handle; //,xTask6Handle, xTask7Handle, xTask8Handle;
void vTask1(void *arg);
void vTask2(void *arg);
void vTask3(void *arg);
void vTask4(void *arg);
void vTask5(void *arg);
void vTask6(void *arg);
void vTask7(void *arg);
void vTask8(void *arg);
//void vTask9(void *arg);
void loop() {
vTaskDelay(pdMS_TO_TICKS(3000)); /* Delay de 3 segundos */
}
void vTask1(void *arg) // Controle Servo 1
{
(void) arg;
while(1)
{
servo1.attach(servo1Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
if (distance <= 10)
{
servo1.write(45);
vTaskDelay(portTICK_PERIOD_MS);
}
if (distance > 10)
{
servo1.write(90);
vTaskDelay(portTICK_PERIOD_MS);
}
servo1.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(10));
}
}
void vTask2(void *arg) // Controle Servo 2
{
(void) arg;
while(1)
{
servo2.attach(servo2Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
/* for (int pos2 = 0; pos2 <= 180; pos2 += 1) { // sweep from 0 degrees to 180 degrees
servo2.write(pos2);
delay(20); // waits 20ms for the servo to reach the position
}
for (int pos2 = 180; pos2 >= 0; pos2 -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos2);
delay(20);
}
*/
servo2.write(90);
vTaskDelay(portTICK_PERIOD_MS);
servo2.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
void vTask3(void *arg) // Controle Servo 2
{
(void) arg;
while(1)
{
servo3.attach(servo3Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
/* for (int pos2 = 0; pos2 <= 180; pos2 += 1) { // sweep from 0 degrees to 180 degrees
servo2.write(pos2);
delay(20); // waits 20ms for the servo to reach the position
}
for (int pos2 = 180; pos2 >= 0; pos2 -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos2);
delay(20);
}
*/
servo3.write(90);
vTaskDelay(portTICK_PERIOD_MS);
servo3.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
void vTask4(void *arg) // Controle Servo 2
{
(void) arg;
while(1)
{
servo4.attach(servo4Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
/* for (int pos2 = 0; pos2 <= 180; pos2 += 1) { // sweep from 0 degrees to 180 degrees
servo2.write(pos2);
delay(20); // waits 20ms for the servo to reach the position
}
for (int pos2 = 180; pos2 >= 0; pos2 -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos2);
delay(20);
}
*/
servo4.write(90);
vTaskDelay(portTICK_PERIOD_MS);
servo4.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
void vTask5(void *arg) // Controle Servo 2
{
(void) arg;
while(1)
{
servo5.attach(servo5Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
/* for (int pos2 = 0; pos2 <= 180; pos2 += 1) { // sweep from 0 degrees to 180 degrees
servo2.write(pos2);
delay(20); // waits 20ms for the servo to reach the position
}
for (int pos2 = 180; pos2 >= 0; pos2 -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos2);
delay(20);
}
*/
servo5.write(90);
vTaskDelay(portTICK_PERIOD_MS);
servo5.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
void vTask6(void *arg) // Controle Servo 2
{
(void) arg;
while(1)
{
servo6.attach(servo6Pin, minUs, maxUs);
//pwm.attachPin(27, 10000);//10khz
/* for (int pos2 = 0; pos2 <= 180; pos2 += 1) { // sweep from 0 degrees to 180 degrees
servo2.write(pos2);
delay(20); // waits 20ms for the servo to reach the position
}
for (int pos2 = 180; pos2 >= 0; pos2 -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos2);
delay(20);
}
*/
servo6.write(90);
vTaskDelay(portTICK_PERIOD_MS);
servo6.detach();
//pwm.detachPin(27);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
void vTask7(void *arg) // Display
{
(void) arg;
while(1)
{
// IMPRIME DISTANCIA EM cm NO DISPLAY TFT ==================================
float updatecm;
if(distance != updatecm)
{
tft.setTextColor(0x39C4, TFT_BLACK); // Leave a 7 segment ghost image, comment out next line!
tft.drawString("8888", 0, 75, 7); // Overwrite the text to clear it
}
tft.setTextColor(TFT_GREEN, TFT_BLACK);
tft.drawNumber(distance, 0, 75, 7);
updatecm = distance;
vTaskDelay(pdMS_TO_TICKS(10)); /* Delay de 1 segundos */
}
}
void vTask8(void *arg) // Ultrasonic
{
(void) arg;
while(1)
{
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculate the distance
float distanceCm = duration * SOUND_SPEED/2;
distance = distanceCm;
// Prints the distance in the Serial Monitor
Serial.print("Distance (cm): ");
Serial.println(distanceCm);
vTaskDelay(pdMS_TO_TICKS(100)); /* Delay de 1 segundos */
}
}
// INICIALIZA ================================================================
void setup()
{
vInitHW();
displaySetup();
ultrasonicSetup();
// Endereço da Task1, Identificação da Task, Bytes Disponibilizados para Task1, Permite Enviar um Parametro para Task, Prioridade (pode ser de 1 a 5), Handler
xTaskCreate(vTask1, "servo1", configMINIMAL_STACK_SIZE, NULL, 2, &xTask1Handle);
xTaskCreate(vTask2, "servo2", configMINIMAL_STACK_SIZE, NULL, 2, &xTask2Handle);
xTaskCreate(vTask3, "servo3", configMINIMAL_STACK_SIZE, NULL, 2, &xTask3Handle);
xTaskCreate(vTask4, "servo4", configMINIMAL_STACK_SIZE, NULL, 3, &xTask4Handle);
xTaskCreate(vTask5, "servo5", configMINIMAL_STACK_SIZE, NULL, 3, &xTask5Handle);
xTaskCreate(vTask6, "servo6", configMINIMAL_STACK_SIZE, NULL, 3, &xTask6Handle);
xTaskCreate(vTask7, "Display", configMINIMAL_STACK_SIZE, NULL, 1, &xTask7Handle);
xTaskCreate(vTask8, "Ultrasonic", configMINIMAL_STACK_SIZE, NULL, 1, &xTask8Handle);
//xTaskCreate(vTask9, "MQTT", configMINIMAL_STACK_SIZE + 1048, NULL, 1, &xTask9Handle);
}
#define configTICK_RATE_HZ 50
Why is this macro defined in the source file? Ideally you should be having this macro defined in FreeRTOSConfig.h. Also if configTICK_RATE_HZ is set to 50 Hz then any vTaskDelay()'s less than 20ms (1000/50) [Lines 150, 318: vTaskDelay(pdMS_TO_TICKS(10)); /* Delay de 1 segundos */] will evaluate to vTaskDelay(0) [pdMS_TO_TICKS] and won’t block the task but will perform a task yield, which might not be what you intended to do here.
Why is it required to attach and detach the servo motor object on every iteration of the task loop?
In addition I doubt that the bare minimum stack size is really sufficient at least for task7 and 8.
Did you verify that’s ok (e.g. by enabling stack checking as I’d recommend) ?
@tony-josi-aws
I tried a lot of stuff, initialy i wasnt using the configTICK_RATE_HZ.
Also, i am using ESP32Servo.h library that have FreeRTOS in it.
The reason that i have configured the Tick Rate Hz as 50Hz, is beacuse i need 20ms of delay for the servos.
About the attach and detach, also a thing that i tried to use to resolve the problem.
I am realy not familiarized with the FreeRTOS, im reading a lot, but not getting good results.
@hs2 is there a way that i can check the stack size needed for each task? How do i know that?
Initally i had setup as configMINIMAL_STACK_SIZE + 1024, but i also changed in order to try something else.
That’s usually an educated guess. At least to start with. And is a uxTaskGetStackHighWaterMark helper.
Or use a debugger to inspect the stack memory range of a task. pxCurrentTCB is a global variable you can use to retrieve the current task properties.
Hi @NathanEspindola,
Were you able to resolve your issues based on the provided suggestions? Is there any additional help we can provide?
Thanks!
Hi, I’ve solved a similar problem. I was working with 2 servos in the same task and the first one attached was shaking all the time (regardless of the sent signal). The second one was fine. I switched the order of attachment and I realized that it was always the first attached servo the one showing problems. So I created a new variable of type Servo and attached it to an unused pin before attaching the real ones. It solved the problem. Hope this helps someone! 