1 year, 1 month ago.

How to put threads in the program.

Good, I would like to advise me on how to use threads in my code and what process (flags, semaphores or queues). The code has several functions, in which one of them moves me a servo depending on the degree of argument. Another function that joins me three servos and forms a leg to be able to move my robot and another function that performs the different movements of my own robot in which I then call it in the main. My robot consists of 6 legs which have to be synchronized in order to move the robot and I would like to know how to create 6 threads (one thread per leg) and to make the code more direct. What would be better to use queues, flags or traffic lights?

My code is as follows:

#include "hexapod.h"
 
Serial my_serial(TX, RX); //Puerto serie
 
SoftPWM patas[num_patas][num_servos] = {{SoftPWM(coxa_1),SoftPWM(femur_1),SoftPWM(tibia_1)},
                                        {SoftPWM(coxa_2),SoftPWM(femur_2),SoftPWM(tibia_2)},
                                        {SoftPWM(coxa_3),SoftPWM(femur_3),SoftPWM(tibia_3)},
                                        {SoftPWM(coxa_4),SoftPWM(femur_4),SoftPWM(tibia_4)},
                                        {SoftPWM(coxa_5),SoftPWM(femur_5),SoftPWM(tibia_5)},
                                        {SoftPWM(coxa_6),SoftPWM(femur_6),SoftPWM(tibia_6)}};
 
int mov_home[num_patas][num_servos] = { {115,90,90},
                                        {90,90,90},
                                        {60,90,90},
                                        {60,90,90},
                                        {90,90,90},
                                        {115,90,90}};
 
int mov_avanza[num_patas][num_servos] = { {135,90,90},
                                          {90,90,90},
                                          {60,90,90},
                                          {60,90,90},
                                          {90,90,90},
                                          {120,90,90}};
 
int mov_retrocede[num_patas][num_servos] = { {120,90,90},
                                             {90,90,90},
                                             {60,90,90},
                                             {60,90,90},
                                             {90,90,90},
                                             {120,90,90}};
 
int main() {
     mueve_pata(0,'H');
     //mueve_pata(1,'H');
     wait_ms(3000);
     bool mov = false;
     while(1){ //Hilo maestro "iría encolando un juego de comandos a cada pata"
        if (mov == true) {
          //  mueve_pata(0,'A');
         //   mueve_pata(1,'R');
         mueve_hexapodo(0);
        } else {
           // mueve_pata(0,'H');
         //   mueve_pata(1,'A');
        }
         mov = !mov;
         wait_ms(1500);
         }
}
 
 
/*******************************************************************************************
Función mueve_servo -> Realiza el movimiento del servo dependiendo del grado que se le pase
*******************************************************************************************/
void mueve_servo(SoftPWM* servo, int grados){
    float th=0;
    th = 565 + (1920/180)*grados;
    servo->pulsewidth_us(th);
}
 
 
/******************************************************************************************************************
Función mueve_pata -> Realiza el movimiento de la pata que se le pasa de entrada e indicando el tipo de movimiento
******************************************************************************************************************/
void mueve_pata(char pata, char pos){
    switch(pos){
        case 'A':    //Avanza
            mueve_servo(&patas[pata][SERVO_COX], mov_avanza[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_avanza[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_avanza[pata][SERVO_TIB]);
            break;
        case 'R':    //Retrocede
            mueve_servo(&patas[pata][SERVO_COX], mov_retrocede[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_retrocede[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_retrocede[pata][SERVO_TIB]);
            break;
        case 'H':    //Home
            mueve_servo(&patas[pata][SERVO_COX], mov_home[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
        default:    //Home
            mueve_servo(&patas[pata][SERVO_COX], mov_home[pata][SERVO_COX]);
            mueve_servo(&patas[pata][SERVO_FEM], mov_home[pata][SERVO_FEM]);
            mueve_servo(&patas[pata][SERVO_TIB], mov_home[pata][SERVO_TIB]);
            break;
    }
}
 
 
/******************************************************************************************************************
Función mueve_hexapodo -> Realiza el movimiento del hexapodo completo dependiendo del tipo elegido
        type = 0 -> HOME
        type = 1 -> AVANZA
        type = 2 -> RETROCEDE
******************************************************************************************************************/
void mueve_hexapodo(int type){
    if(type == HOME){
        mueve_pata(0,'H');
        mueve_pata(1,'H');
        mueve_pata(2,'H');
        mueve_pata(3,'H');
        mueve_pata(4,'H');
        mueve_pata(5,'H');
    }else if(type == AVANZA){
        if (mov == true) {
            mueve_pata(0,'A');
            mueve_pata(1,'R');
            mueve_pata(2,'A');
            mueve_pata(3,'R');
            mueve_pata(4,'A');
            mueve_pata(5,'R');
        } else {
            mueve_pata(0,'R');
            mueve_pata(1,'A');
            mueve_pata(2,'R');
            mueve_pata(3,'A');
            mueve_pata(4,'R');
            mueve_pata(5,'A');
        }
    }else if(type == RETROCEDE){
        if (mov == true) {
            mueve_pata(0,'R');
            mueve_pata(1,'A');
            mueve_pata(2,'R');
            mueve_pata(3,'A');
            mueve_pata(4,'R');
            mueve_pata(5,'A');
        } else {
            mueve_pata(0,'A');
            mueve_pata(1,'R');
            mueve_pata(2,'A');
            mueve_pata(3,'R');
            mueve_pata(4,'A');
            mueve_pata(5,'R');
        }
}            

Thank you.

_

posted by Jose Romero 24 Oct 2018

1 Answer

1 year, 1 month ago.

Threads are for allowing things to operate independently of each other. Because they are independent communication between threads is problematic and should be minimized.

You don't want each leg running independently of the others, you want them to move at the same time with a lot of communication between them. Putting each leg in a different thread seems like a poor choice in this situation..

What would make a lot more sense would be to create a leg object. And possibly a body object that then controls the individual legs.

something like: (not checked for errors)

class leg;

class body{
  public:
    body();
    void moveForward(float distance);
    etc...
  private:
    leg legs[6];
}

class leg{
  public:
    leg(PinName motor1, PinName motor2, PinName motot3);
    move(char position);
    setHome(int pos1,int po2,int pos3); // repeat for other positions
  private:
    softPWM join[3];
    int mov_home[3];
    int mov_avanza[3];
    int mov_retrocede[3];
}

leg::leg(PinName motor1, PinName motor2, PinName motot3): join[0](motor1),join[1](motor2),join[2](motor3) {
 
}

leg::setHome(int pos1,int po2,int pos3) {
mov_home[0]=pos1;
mov_home[1]=pos2;
mov_home[2]=pos3;
}

leg::move(char position) {
   switch(position){
        case 'A':    //Avanza
            for (int i=0;i<3;i++)
              mueve_servo(&join[i],mov_avanza[i]);
            break;
        case 'R':    //Retrocede
            for (int i=0;i<3;i++)
              mueve_servo(&join[i],mov_retrocede[i]);
            break;
        case 'H':    //Home
        default:    //Home
            for (int i=0;i<3;i++)
              mueve_servo(&join[i],mov_home[i]);
            break;
    }
}

body::body():legs[0](coxa_1,femur_1,tibia_1),legs[1](coxa_2,femur_2,tibia_2), // etc....
{
for (int i=0;i<6;i++)
  legs[i].move('H');
}

__

posted by Jose Romero 24 Oct 2018

You could use threads and keep the waits but you'd still want to create a leg class and split each leg into it's own object so that each thread is working on it's own object. 6 threads all manipulating interdependent global objects and values at the same time and trying to keep things coordinated just seems like asking for things to go wrong.

Personally they way I'd do it is have a state machine inside each leg that keeps track of the current position and then use a Timeout to schedule the next movement based on the current state. It's a slight increase in complexity in your code within the leg class but skips the overhead and complexity of running an OS and multiple threads.

additions to previous code... (with a similar lack of testing)

class leg {
    public:
      bool isMoving(); // indicates that a movement is in progress
    ...
    private:
    // current state of the leg
    enum state {Home1,Home2,Home3,Avanza1,Avanza2,Avanza3,Retrocede1,Retrocede2,Retrocede3,unknown} currentState;
    Timeout nextMove;
    void mov_avanza1();
    void mov_avanza2();
    void mov_avanza3();

    void mov_home1();
    void mov_home2();
    void mov_home3();
...
}

leg::leg(... {
    currentState = unknown;
    mueve_home1();
    }    

bool leg:isMoving() {
  if (currentState == Home3)
    return false;
  if (currentState == Avanza3)
    return false;
  if (currentState == Retrocede3)
    return false;
  if (currentState == unknown)
    return false;
  return true;
  }

leg::move(char position) {
   switch(position){
        case 'A':    //Avanza
            mov_avanza1();
            break;
        case 'R':    //Retrocede
            mov_retrocede1();
            break;
        case 'H':    //Home
        default:    //Home
            mueve_home1();
            break;
    }
}

leg::mov_avanza1() {
    currentState = Avanza1;
    mueve_servo(&join[SERVO_TIB], mov_avanza[SERVO_TIB]);
    mueve_servo(&join[SERVO_FEM], mov_avanza[SERVO_FEM]);
    nextMove.attach(callback(this,mov_avanza2),0.175);
    }
leg::mov_avanza2() {
    currentState = Avanza2;
    mueve_servo(&join[SERVO_COX], mov_avanza[SERVO_COX]);
    nextMove.attach(callback(this,mov_avanza3),0.175);
    }
leg::mov_avanza3() {
    currentState = Avanza3;
    mueve_servo(&join[SERVO_FEM], mov_home[SERVO_FEM]);
    mueve_servo(&join[SERVO_TIB], mov_home[SERVO_TIB]);
  }

The above assumes that there are 3 stages to each movement, you can reduce home to a single stage if it doesn't require any pauses.

posted by Andy A 24 Oct 2018

Good again, in the two ways that you have told me last, the first is how I want to do it. I know it will not be the best way and the second of you will be the best, but I have already created the variables to give values ​​to the legs and that's why I would like to know how to put each leg in a different thread and move both. I prefer this option even if it's worse.

posted by Jose Romero 24 Oct 2018

What I mean is: I create an array of threads and in the main I think, but they give me errors of "No instance of overloaded function" callback "matches the argument list".

This is what I have:

Thread thread[6];

int main() {
     mueve_pata(0,'H');
     mueve_pata(1,'H');
     mueve_pata(2,'H');
     mueve_pata(3,'H');
     mueve_pata(4,'H');
     mueve_pata(5,'H');
     wait_ms(3000);
     thread[0].start(callback(mueve_pata, patas[0][0], 'A'));
posted by Jose Romero 25 Oct 2018

The syntax to start a thread to run a function is thread.start(callback(object,method)); Since you have everything global it would be thread[0].start(callback(this,mueve_pata()));

You'll still get an error with your current structure because when starting a thread you can't pass parameters to the function being called and your current function needs parameters.

You will either need to define a different function for every possible move or set a global variable setting the move type to do that the function then checks when it first starts.

But you couldn't just set the variable, start a thread, set the next value, start the second thread etc... If you did that you'd get 6 threads all doing the last movement you set. Your function would then need to set a flag indicating that it has read the variable indicating the type of movement before the main code could change the value and start the next thread. Which would mean the main code would have to wait until the scheduler has started the new thread and given it some running time. Which is going to make keeping things in sync hard.

Or you stop making things harder for yourself and do this the sensible way.

posted by Andy A 25 Oct 2018

It's perfect, because I'm going to do it in his own way, I'm going to attach all the code that you mentioned and a couple of errors that come out and a couple of doubts. I will do it in several parts because I do not fit into a single comment.

#include "mbed.h"
#include "SoftPWM.h"
#include "Servo.h"
#include <rtos.h>

#define SERVO_COX 0
#define SERVO_FEM 1
#define SERVO_TIB 2

class leg;

class leg{
  public:
    bool isMoving();
    leg(PinName motor1, PinName motor2, PinName motot3);
    void move(char position);
    void setHome(int pos1,int po2,int pos3); // repeat for other positions
  private:
    enum state {Home1,Home2,Home3,Avanza1,Avanza2,Avanza3,Retrocede1,Retrocede2,Retrocede3,unknown} currentState;
    Timeout nextMove;
    void mov_avanza1();
    void mov_avanza2();
    void mov_avanza3();
 
    void mov_home1();
    void mov_home2();
    void mov_home3();
    
    SoftPWM join[3];
    int mov_home[3];
    int mov_avanza[3];
    int mov_retrocede[3];
};

class body{
  public:
    body();
    void moveForward(float distance);
    //etc...
  private:
    leg legs[6];
};

leg::leg(PinName motor1, PinName motor2, PinName motot3): join[0](motor1),join[1](motor2),join[2](motor3) {
    currentState = unknown;
    mueve_home1();
}

void leg::setHome(int pos1,int pos2,int pos3) {
mov_home[0]=pos1;
mov_home[1]=pos2;
mov_home[2]=pos3;
}

void leg::move(char position) {
   switch(position){
        case 'A':    //Avanza
            mov_avanza1();
            break;
        case 'R':    //Retrocede
            mov_retrocede1();
            break;
        case 'H':    //Home
        default:    //Home
            mueve_home1();
            break;
    }
}
 
body::body():legs[0](coxa_1,femur_1,tibia_1),legs[1](coxa_2,femur_2,tibia_2) // etc....
{
for (int i=0;i<6;i++){
  legs[i].move('H');
}

bool leg::isMoving() {
  if (currentState == Home3)
    return false;
  if (currentState == Avanza3)
    return false;
  if (currentState == Retrocede3)
    return false;
  if (currentState == unknown)
    return false;
  return true;
}

void mueve_servo(SoftPWM* servo, int grados){
    float th=0;
    th = 565 + (1920/180)*grados;
    servo->pulsewidth_us(th);
}

void leg::mov_avanza1() {
    currentState = Avanza1;
    mueve_servo(&join[SERVO_TIB], mov_avanza[SERVO_TIB]);
    mueve_servo(&join[SERVO_FEM], mov_avanza[SERVO_FEM]);
    nextMove.attach(callback(this,mov_avanza2),0.175);
}
void leg::mov_avanza2() {
    currentState = Avanza2;
    mueve_servo(&join[SERVO_COX], mov_avanza[SERVO_COX]);
    nextMove.attach(callback(this,mov_avanza3),0.175);
}
void leg::mov_avanza3() {
    currentState = Avanza3;
    mueve_servo(&join[SERVO_FEM], mov_home[SERVO_FEM]);
    mueve_servo(&join[SERVO_TIB], mov_home[SERVO_TIB]);
}

My errors are the following:

-> On lines 44 and 70, Expected a errors appear "(".

-> In lines 98 and 103, errors of Nonstandard form appear for taking the address of a member function.

My doubts are:

-> How do I declare the pins for each servo? Because it does not work the way it used to, because I used to have a matrix of 3x6 elements. The same with mov_home, mov_avanza and mov_retrocede.

-> How could I call it in the main to try this code?

I know I have to add things.

posted by Jose Romero 26 Oct 2018