Problem to make a code with FreeRTOS

LeafLabs Garden » Community » The Lounge

Problem to make a code with FreeRTOS

(7 posts) (3 voices)

Started 3 years ago by superprp
Latest reply from feurig

superprp
Member

I wanna send data to a UART (motor controller) and when I test the software the UART stop to send in few seconds, I dont know why, but when I remove this part of the code:

enviado = sendDataUART(0x21,0,2);  //Leemos la velocidad de motor 1 (izquierdo)
        while(enviado == false)
        {
          enviado = sendDataUART(0x21,0,2);
        }
        enviado = sendDataUART(0x22,0,2);  //Leemos la velocidad de motor 2 (derecho)
        while(enviado == false)
        {
          enviado = sendDataUART(0x22,0,2);
        }
        enviado = sendDataUART(0x26,0,2);  //Leemos la tensión
        while(enviado == false)
        {
          enviado = sendDataUART(0x26,0,2);
        }

it works fine. Could you tell me why? Thank you
Here is the complete code:

#include <MapleFreeRTOS.h>
#include <exti.h>

//***********************************
//PARAMETROS CONFIGURACIÓN PLATAFORMA
//***********************************
#define PULSOS_POR_VUELTA 360  //ticks de una vuelta completa para el eje de salida
#define DIAMETRO_RUEDA 0.09  //radio en m (100mm)
#define DISTANCIA_EJES 0.25  //Distancia entre ejes en m (200mm)

//***************************************
//CONFIGURAMOS LOS NOMBRES DE LOS PUERTOS
//***************************************

int ledPin =  13;    // LED connected to digital pin 13

//*********************
//CREAMOS LAS VARIABLES
//*********************
//Calculo de posición y encoders

double x_actual = 0;
double y_actual = 0;
float teta_actual = 0;
int delta_ticksLeft = 0;
int delta_ticksRight = 0;
boolean reading_enc = false;
unsigned long encoder_izq_new = 0;//encoder izq = encoder 1 de la PCB
unsigned long encoder_der_new = 0;//encoder_der = encoder 2 de la PCB
unsigned long encoder_izq_old = 0;//encoder izq = encoder 1 de la PCB
unsigned long encoder_der_old = 0;//encoder_der = encoder 2 de la PCB
double x_enc = 0;
double y_enc = 0;
float teta_enc = 0;
double s_izq = 0;
double s_der = 0;

//lecturas de la MD25
int vel_izq = 0;
int vel_der = 0;
int volt_12v = 0;
int curr_izq = 0;
int curr_der = 0;

//otros
xQueueHandle xQueue_izq;
xQueueHandle xQueue_der;
xSemaphoreHandle xSemaphore_UART;

//********************
//DEFINIMOS LAS TAREAS
//********************
static void vTaskSerialUSB( void *pvParameters )
{
    boolean enviado;
    portTickType xLastWakeTimeSerialUSB;
    const portTickType xFrequencySerialUSB = 1000;  // Ejecutamos la tarea cada 1000ms
    xLastWakeTimeSerialUSB = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
    for(;;)
    {
        vTaskDelayUntil(&xLastWakeTimeSerialUSB, xFrequencySerialUSB);
        SerialUSB.print("encoder izq: ");
        SerialUSB.print(encoder_izq_new);
        SerialUSB.print("\t");
        SerialUSB.print("encoder der: ");
        SerialUSB.print(encoder_der_new);
        SerialUSB.print("\t");
        SerialUSB.print("x_enc: ");
        SerialUSB.print(x_enc);
        SerialUSB.print("\t");
        SerialUSB.print("y_enc: ");
        SerialUSB.print(y_enc);
        SerialUSB.print("\t");
        SerialUSB.print("teta_enc: ");
        SerialUSB.print(teta_enc);
        SerialUSB.print("\n");
        enviado = sendDataUART(0x21,0,2);  //Leemos la velocidad de motor 1 (izquierdo)
        while(enviado == false)
        {
          enviado = sendDataUART(0x21,0,2);
        }
        enviado = sendDataUART(0x22,0,2);  //Leemos la velocidad de motor 2 (derecho)
        while(enviado == false)
        {
          enviado = sendDataUART(0x22,0,2);
        }
        enviado = sendDataUART(0x26,0,2);  //Leemos la tensión
        while(enviado == false)
        {
          enviado = sendDataUART(0x26,0,2);
        }
        SerialUSB.print("vel izq: ");
        SerialUSB.print(vel_izq);
        SerialUSB.print("\t");
        SerialUSB.print("ve der: ");
        SerialUSB.print(vel_der);
        SerialUSB.print("\t");
        SerialUSB.print("volt 12v: ");
        SerialUSB.print(volt_12v);
        SerialUSB.print("\n");
        //togglePin(ledPin);
    }
}

static void vTaskPosCalc( void *pvParameters )
{
    for(;;)
    {
          sendDataUART(0x32,0x08,3);
          sendDataUART(0x31,0x08,3);
          while(x_enc < 1.0)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0xf8,3);
          while(teta_enc<1.57)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0x08,3);
          while(y_enc < 1)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0xf8,3);
          while(abs(teta_enc)<3.10)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0x08,3);
          while(abs(x_enc) > 0.03)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0xf8,3);
          while(abs(teta_enc)>1.57)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0x08,3);
          while(abs(y_enc) > 0.03)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x31,0xf8,3);
          while(abs(teta_enc)<0.03)
          {
            digitalWrite(ledPin, HIGH);
          }
          sendDataUART(0x32,0x00,3);
          sendDataUART(0x31,0x00,3);
    }
}

static void vTaskEncCount( void *pvParameters )
{
    boolean enviado;
    portTickType xLastWakeTimeEncCount;
    const portTickType xFrequencyEncCount = 10;  // Ejecutamos la tarea vTaskEncCount cada 10ms
    xLastWakeTimeEncCount = xTaskGetTickCount();  // Initialise the xLastWakeTime variable with the current time.
    for(;;)
    {
        vTaskDelayUntil(&xLastWakeTimeEncCount, xFrequencyEncCount);
        enviado = sendDataUART(0x25,0,2);  //Leemos la posición global de los encoders
        while(enviado == false)
        {
          enviado = sendDataUART(0x25,0,2);
        }
        reading_enc = true;
        xQueueReceive(xQueue_der, &encoder_der_new, 0);
        xQueueReceive(xQueue_izq, &encoder_izq_new, 0);
        odometer_thread();

    }
}

//**********
//FUNCIONES
//**********
boolean sendDataUART(char command, char data, char numbytes)
{
  xSemaphoreTake( xSemaphore_UART, ( portTickType ) 100 );
  int numUnread=0;
  int watchdog=0;
  unsigned long time1;
  unsigned long time2;
  char buf[3];
  buf[0] = 0x00;
  buf[1] = command;
  buf[2] = data;
  Serial1.write(buf, numbytes);
  if (command == 0x25)  //LECTURA ENCODERS
  {
      time1 = millis();
      do
      {
          numUnread = Serial1.available();
          time2 = millis();
          if(time2 > time1)
            watchdog = time2-time1;
          else
            watchdog = 0xffffffff - time2 + time1;
      }
      while(numUnread < 8 || watchdog > 500);
      if(watchdog > 500)
          return false;
      else
      {
          encoder_izq_old = encoder_izq_new;  //guardo los viejos encoders en variables
          encoder_der_old = encoder_der_new;  //guardo los viejos encoders en variables
          encoder_izq_new = 0;
          encoder_der_new = 0;
          for(int i = 0; i < 4; i++)
          {
            encoder_izq_new = encoder_izq_new + (Serial1.read()  << (24-8*i));
          }
          for(int i = 0; i < 4; i++)
          {
            encoder_der_new = encoder_der_new + (Serial1.read()  << (24-8*i));
          }
          xSemaphoreGive( xSemaphore_UART );
          return true;
      }
  }
  else if (command == 0x21 || command == 0x22 || command == 0x26 || command == 0x27 || command == 0x28)
  {
      time1 = millis();
      do
      {
          numUnread = Serial1.available();
          time2 = millis();
          if(time2 > time1)
            watchdog = time2-time1;
          else
            watchdog = 0xffffffff - time2 + time1;
      }
      while(numUnread < 1 || watchdog > 500);
      if(watchdog > 500)
        return false;
      else
      {
          if(command == 0x21)
          {
              vel_izq = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x22)
          {
              vel_der = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x26)
          {
              volt_12v = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x27)
          {
              curr_izq = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x28)
          {
              curr_der = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
      }
  }
  xSemaphoreGive( xSemaphore_UART );
}

void initialize_odometry(double new_x, double new_y, float new_teta)
{
  x_enc = new_x;
  y_enc = new_y;
  teta_enc = new_teta;
}

void odometer_thread()
{
  int delta_enc_izq;
  int delta_enc_der;
  double expr1;
  double cos_current;
  double sin_current;
  double right_minus_left;
  double MUL_COUNT;
  MUL_COUNT  = PI * DIAMETRO_RUEDA / PULSOS_POR_VUELTA;
  delta_enc_izq = encoder_izq_new - encoder_izq_old;
  delta_enc_der = encoder_der_new - encoder_der_old;
  s_izq = (double)delta_enc_izq * MUL_COUNT;
  s_der = (double)delta_enc_der * MUL_COUNT;
  cos_current = cos(teta_enc);
  sin_current = sin(teta_enc);

  if (s_izq == s_der)
  {
    /* Moving in a straight line */
    x_enc = x_enc + s_izq * cos_current;
    y_enc = y_enc + s_der * sin_current;
  }
  else
  {
    /* Moving in an arc */
    expr1 = DISTANCIA_EJES * (s_der + s_izq) / 2.0 / (s_der - s_izq);
    right_minus_left = s_der - s_izq;
    x_enc = x_enc + expr1 * (sin(right_minus_left / DISTANCIA_EJES + teta_enc) - sin_current);
    y_enc = y_enc - expr1 * (cos(right_minus_left / DISTANCIA_EJES + teta_enc) - cos_current);

     /* Calculate new orientation */
    teta_enc = teta_enc + right_minus_left / DISTANCIA_EJES;

     /* Keep in the range -PI to +PI */
     while(teta_enc > PI)
       teta_enc = teta_enc - (2.0*PI);
     while(teta_enc < -PI)
       teta_enc = teta_enc + (2.0*PI);
  }
} 

// The setup() method runs once, when the sketch starts
void setup()
{
  //configurar UART
    Serial1.begin(38400);
  // initialize the digital pin as an output:
    pinMode(ledPin, OUTPUT);
    delay(5000);
    digitalWrite(ledPin, HIGH);
    xQueue_izq = xQueueCreate( 2, sizeof(unsigned long) );
    xQueue_der = xQueueCreate( 2, sizeof(unsigned long) );
    vSemaphoreCreateBinary( xSemaphore_UART );
    xTaskCreate(vTaskSerialUSB, (const signed char *)"SerialUSB", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, NULL);
    xTaskCreate(vTaskPosCalc, (const signed char *)"PosCalc", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 2, NULL);
    xTaskCreate(vTaskEncCount, (const signed char *)"EncCount", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 3, NULL);
    sendDataUART(0x35,0,2);  //Reseteo encoders
    sendDataUART(0x34,0x01,3);  //Configuro motores en modo
    vTaskStartScheduler();

}

void loop()
{
    // Insert background code here
}

Posted 3 years ago #

gbulmer
Moderator

superprp - Welcome!
Would you please read the posting guidelines and especialy avoid posting long pieces of code?

Have you asked questions at FreeRTOS e.g. http://sourceforge.net/projects/freertos/forums/forum/382005

It might help if you simplified this down to a minimal program which still has the bug. The example you've posted is probably unnecessarily long. I am not going to spend time reading it.

I corrected the code quotes, hoping the code would be easier to read, but I'm not convinced that helped. Please feel free to change it back.

Posted 3 years ago #

superprp
Member

ok, thank you for your advice.

I think that the problem is in this part of the code:

boolean sendDataUART(char command, char data, char numbytes)
{
  xSemaphoreTake( xSemaphore_UART, ( portTickType ) 100 );
  int numUnread=0;
  int watchdog=0;
  unsigned long time1;
  unsigned long time2;
  char buf[3];
  buf[0] = 0x00;
  buf[1] = command;
  buf[2] = data;
  Serial1.write(buf, numbytes);
  if (command == 0x25)  //LECTURA ENCODERS
  {
      time1 = millis();
      do
      {
          numUnread = Serial1.available();
          time2 = millis();
          if(time2 > time1)
            watchdog = time2-time1;
          else
            watchdog = 0xffffffff - time2 + time1;
      }
      while(numUnread < 8 || watchdog > 500);
      if(watchdog > 500)
          return false;
      else
      {
          encoder_izq_old = encoder_izq_new;  //guardo los viejos encoders en variables
          encoder_der_old = encoder_der_new;  //guardo los viejos encoders en variables
          encoder_izq_new = 0;
          encoder_der_new = 0;
          for(int i = 0; i < 4; i++)
          {
            encoder_izq_new = encoder_izq_new + (Serial1.read()  << (24-8*i));
          }
          for(int i = 0; i < 4; i++)
          {
            encoder_der_new = encoder_der_new + (Serial1.read()  << (24-8*i));
          }
          xSemaphoreGive( xSemaphore_UART );
          return true;
      }
  }
  else if (command == 0x21 || command == 0x22 || command == 0x26 || command == 0x27 || command == 0x28)
  {
      time1 = millis();
      do
      {
          numUnread = Serial1.available();
          time2 = millis();
          if(time2 > time1)
            watchdog = time2-time1;
          else
            watchdog = 0xffffffff - time2 + time1;
      }
      while(numUnread < 1 || watchdog > 500);
      if(watchdog > 500)
        return false;
      else
      {
          if(command == 0x21)
          {
              vel_izq = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x22)
          {
              vel_der = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x26)
          {
              volt_12v = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x27)
          {
              curr_izq = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
          else if(command == 0x28)
          {
              curr_der = Serial1.read();
              xSemaphoreGive( xSemaphore_UART );
              return true;
          }
      }
  }
  xSemaphoreGive( xSemaphore_UART );
}

because i call to this function in serveral task and i have tried that with this code, Dont matter which task call to this function or how may times it is called.

Posted 3 years ago #

gbulmer
Moderator
superprp - Are you learning to program and this is coursework?

I think this is still too complex to test. I also think it is more complex than is required in one function.
For example the function can exit in may places, leaving the semaphore xSemaphoreTake

How are you testing this code?

Have you go a way to read and write data via the UART so that you can see what is happening when it starts to fail? A USB-to-UART might be enough to do that job.

I can see a way that this code can exit without giving up the semaphore, which might kill it:
```
if(watchdog > 500)
          return false;
```
I can also see a possible infinite loop if some code gets blocked from running:
while(numUnread < 8 || watchdog > 500);

Also it seems the function can exit without a defined value by falling off the end, which should matter (or the code is too complex).

It doesn't check that xSemaphoreTake or xSemaphoreGive have worked without error.

There are other things which look suspicious like Serial1.write(buf, numbytes); would break if numbytes is not 2 or 3. In which case why take it as a parameter, or not check that it? What does the caller have to know that the packet has an initial 0x00 byte? AFAICT there is a command byte with an optional data byte, and that is all the caller needs to tell the function.

Any of those problems might be enough to cause it to fail. IMHO this is too fragile.

AFAICT, if it is one of the valid commands it always does:
```
time1 = millis();
      do
      {
          numUnread = Serial1.available();
          time2 = millis();
          if(time2 > time1)
            watchdog = time2-time1;
          else
            watchdog = 0xffffffff - time2 + time1;
      }
      while(numUnread < X || watchdog > 500);  // or 1
      if(watchdog > 500)
          return false;
      else
      {
```
where X is 1 or 8.
I wouldn't expect that code repeated twice.

Also I'd use a constant to hold the value of the watchdog timeout (500) so that I could, for example, make it huge by editing one value so that this code would be easier to test.

In the case where the command is 0x25 (I would use constants to give commands names) it relies on numUnread = Serial1.available(); to reach 8. Do you know that it is possible for Serial1.available() to reach 8? What happens if the buffer is less than 8 bytes, so it never reaches 8? Why not just try to read whatever bytes are available until it gets 8 or it times out?

In the case where the command is valid and not 0x25 it reads one character, and releases the semaphore. But this code mixes managing the semaphore with decoding the command. separate the two and it should be easier to test the critical part, which is the part that can block or fail.

AFAICT all that needs to happen at the level of the UART is: a command is sent, and at some later stage a one byte or eight byte response is read. That's all.

The UART communication function doesn't need to decode the communication protocol as well; that adds complexity. Decoding or detecting an error can be done at a higher layer, in another function, once the correct number of bytes are retrieved or there has been a timeout.

IMHO this function it is too complex for you to debug.

I also think the code needs to take a more robust approach to errors.

When you try to add the error checking, you might see that this structure has too many ways to fail, and you might see how to reorganise it so that it is simpler to test and use.

I also think you need to get more information to understand what it is doing. Use extra output pins to indicate where it is when it gets stuck, for example create signals for an oscilloscope (or put LEDs on those pins). Or get a cheap ST-LINK and run gdb to see what is happening when it breaks.

I'm done.
Posted 3 years ago #
superprp
Member

Thank you so much for your advices, I will take into account everything what you said to improve the debug code. I am not a programmer (electronic) and for me it is too dificult to program with a RTOS (i am learning now).

For more information, I am comunicating with a MD25 (driver controller) and I have review the specs and I think that the other posibility is that MD25 is a 5V board and maple is a 3.3v but i am using the serial port 1 that is 5v tolerant, but this dont mean that the MD25 will be 3.3v tolerant, then, Have you notice if some people have had problems with that?

I will continue debuging the code to improve with your comments. Thank you

Posted 3 years ago #
gbulmer
Moderator

superprp - it might be a problem to talk to a 5V UART from a 3.3V Maple.

A Maple 5V tolerant pin simply means that it can tolerate being pulled up to 5V. It does not mean the pin will pull the signal voltage to 5V, it will only reach 3.3V. Having said that, some devices are designed to work with 3.3V, for example a 'high' is less than 2.5V.

Your first post in this thread said that it worked except for a few lines of code. So what makes you think that it could be the voltage difference?

Looking at the command set for the MD25, e.g. http://www.robot-electronics.co.uk/htm/md25ser.htm
you could write a very simple program, using the ordinary Maple setup() and loop() functions, removing the complexity of the RTOS, to test talking to that board. Just write commands and read the results. Nothing else. Leave it running while you try to test that it is robust.

To remove any concerns about interfacing the board and Maple, you could make yourself simple level shifters with MOSFETs and resistors. Then use the simple test program to ensure everything is robust.

Try to keep the functions small and easy to test, try to set up some test infrastructure so you can be confident that things are working.

Posted 3 years ago #
feurig
Member

On my app I used one of the single gate t4hct125s to drive from the 3v to 5v serial. http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00001436.pdf

Posted 3 years ago #

RSS feed for this topic

Reply

You must log in to post.