Multi-threaded Robot

Question

I am working on a small robot project, the code which i made for this has a sequential execution in a single thread. There are a lot of Delay() function needed. Due to which the accuracy is extremely bad. i want to to implement the movements in one thread and the sensor detection in another thread.

#define M1 3
#define M2 5
#define M3 6
#define M4 9
#define L_trig 2
#define L_echo 4
#define R_trig 7
#define R_echo 8
#define servo 10
#define solenoid_pin 11
#define LDR_pin1 12
#define LDR_pin2 13
void setup() 
{
// Motors Initilization
pinMode(M1,OUTPUT);
pinMode(M2,OUTPUT);
pinMode(M3,OUTPUT);
pinMode(M4,OUTPUT);
// Ultrasonic  Initilization
pinMode(L_trig, OUTPUT);
pinMode(L_echo, INPUT);
pinMode(R_trig, OUTPUT);
pinMode(R_echo, INPUT);
S.attach(servo);
}

void loop() 
{

 Runmotor(M1);
 Runmotor(M2);
 Runmotor(M3);
 Runmotor(M4);
 servo_rotation();
 Ultrasonic_value(L_trig,L_echo);
 Ultrasonic_value(R_trig,R_echo);

 if(Ultrasonic_value(L_trig,L_echo)<5 || Ultrasonic_value(R_trig,R_echo)<5)

   {
     turnLeft();
     //Serial.println("Working");
   }

}

// Run Motor
void Runmotor(int which)
{
  for(int i=0;i<1023;i++)
  {
 analogWrite(which, i);  
  }
}
// Stop Motor
void stopmotor(int which)
{
  digitalWrite(which,LOW);
}
// To turn the robot left side
void turnLeft()
{
  stopmotor(M2);
  stopmotor(M3);
  delay(5000);
  Runmotor(M2);
  Runmotor(M3); 
}
// To turn the robot Rightside
void turnRight()
{
  stopmotor(M1);
  stopmotor(M4);
  delay(5000);
  Runmotor(M1);
  Runmotor(M4); 
}
// Servo Rotation
void servo_rotation()
{
 for(angle=0;angle<=180;angle++)
{ 
 S.write(angle); 
 if(angle==90)
 {
   delay(2000);
 }
}
delay(2000);
for(angle=180;angle>=0;angle--)
{
  S.write(angle);
  Serial.println(angle);
  if(angle==90)
 {
   delay(2000);
 }
}
 delay(2000); 
}

// Calculation of distance 
long Ultrasonic_value(int which_trig,int which_echo)
{
  long duration, distance;
  digitalWrite(which_trig, LOW);
  delayMicroseconds(2); 
  digitalWrite(which_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(which_trig, LOW);
  duration = pulseIn(which_echo, HIGH);
  // distance = duration*343*100/2;
  distance = (duration/2) / 29.1;
  delay(100);
  return distance;
}

void Solenoid_Fire()
{
  digitalWrite(solenoid_pin,LOW);
}

void Solenoid_Reload()
{
  digitalWrite(solenoid_pin,HIGH);
}
/*
bool LDR_reading(int LDR_pin1 , int LDR_pin2)
{


}
*/

Please help me. I want to implement this code in a Multi-thread fashion. Thanks in Advance

Answer 1

You could also give my ThreadHandler library a try

https://bitbucket.org/adamb3_14/threadhandler/src/master/

It uses an interrupting scheduler to allow context switching without relaying on yield() or delay().

I created the library because I needed three threads and I needed two of them to run at a precise time no matter what the others were doing. The first thread handled serial communication. The second was running a Kalman filter using float matrix multiplication with the Eigen library. And the third was a fast current control loop thread which had to be able to interrupt the matrix calculations.

How it works

Each cyclic thread has a priority and a period. If a thread, with higher priority than the current executing thread, reaches its next execution time the scheduler will pause the current thread and switch to the higher priority one. Once the high priority thread completes its execution the scheduler switches back to the previous thread.

Scheduling rules

The scheduling scheme of the ThreadHandler library is as follows:

1. Highest priority first.
2. If the priority is the same then the thread with the earliest deadline is executed first.
3. If two threads have the same deadline then the first created thread will execute first.
4. A thread can only be interrupted by threads with higher priority.
5. Once a thread is executing it will block execution for all threads with lower priority until the run function returns.
6. The loop function has priority -128 compared to ThreadHandler threads.

How to use

Threads can be created via c++ inheritance

class MyThread : public Thread
{
public:
    MyThread() : Thread(priority, period, offset){}

    virtual ~MyThread(){}

    virtual void run()
    {
        //code to run
    }
};

MyThread* threadObj = new MyThread();

Or via createThread and a lambda function

Thread* myThread = createThread(priority, period, offset,
    []()
    {
        //code to run
    });

Thread objects automatically connect to the ThreadHandler when they are created.

To start execution of created thread objects call:

ThreadHandler::getInstance()->enableThreadExecution();

Answer 2

I think this needs a major rework. First here:

  for(int i=0;i<1023;i++)
  {
  analogWrite(which, i);  
  }

The maximum you can analogWrite is 255, so change 1023 to 255.

Next, get rid of the delay() calls.

---

Let's look at turnLeft :

// To turn the robot left side
void turnLeft()
{
  stopmotor(M2);
  stopmotor(M3);
  delay(5000);
  Runmotor(M2);
  Runmotor(M3); 
}

Why stop the motor and then start it again? Do you mean:

// To turn the robot left side
void turnLeft()
{
  stopmotor(M1);
  stopmotor(M4);
  delay(5000);
  Runmotor(M2);
  Runmotor(M3); 
}

I don't see what the delay achieves anyway. Get rid of it:

// To turn the robot left side
void turnLeft()
{
  stopmotor(M1);
  stopmotor(M4);
  Runmotor(M2);
  Runmotor(M3); 
}

In fact, I don't see what any of the delay() calls achieve. Try removing them.

---

See my post How to do multiple things at once ... like cook bacon and eggs for tips on doing multiple things without using delay().

---

Also consider using arrays. Instead of M1, M2, M3, M4, make an array:

const int NUMBER_OF_MOTORS = 4;
const int motorPins [NUMBER_OF_MOTORS] = { 3, 4, 6, 9 };

Now it is easier to set them all to output:

// Motors Initilization

for (int i = 0; i < NUMBER_OF_MOTORS; i++)
  pinMode (motorPins [i], OUTPUT);

It is also easier to start or stop all of them:

for (int i = 0; i < NUMBER_OF_MOTORS; i++)
  stopmotor (i);

Where stopmotor is now:

// Stop Motor
void stopmotor(int which)
{
  digitalWrite(motorPins [which],LOW);
}

---

(Edited to add)

I also don't see what this achieves:

void Runmotor(int which)
{
  for(int i=0;i<1023;i++)
  {
 analogWrite(which, i);  
  }
}

Apart from the fact that you can't go up to 1023, if you stop at 255, the time taken to do all those analogWrite is just over 2 ms. Why bother? Why not just do this:

void Runmotor(int which)
{
  digitalWrite (which, HIGH);
}

Answer 3

You may want to take a look at LMX by David Anderson A Light-Weight Multi-Tasking Executive.

He has a very good set of videos on the code that he did for Dallas Personal Robotics Group (DPRG).

The specific videos for LMX are:

• David Anderson on Robotics Software - part 1
• David Anderson on Robotics Software - part 2

Answer 4

You could use a scheduler and simply start tasks with:

#include <Scheduler.h>
...
void setup()
{
  ...
  Scheduler.start(servoSetup, servoLoop);
  Scheduler.start(sensorSetup, sensorLoop);
  ...
}

void servoSetup()
{
  S.attach(servo);
}

void servoLoop()
{
  int angle;
  for(angle = 0;angle <= 180; angle++) { 
    S.write(angle); 
    if (angle == 90) {
      delay(2000);
    }
  }
  delay(2000);
  ...
}
...

The tasks must call yield() or delay() to context switch. The taskSetup function is called first and once by the task. The taskLoop function is repeatedly called by the task. This can be viewed as running an additional sketch.

For more details see the example sketches.

Answer 5

I am looking at this, for a similar project. It seems light and simple enough that you could have a quick trial. Converting those functions to threads shouldn't be too time consuming.

Once every activity is implemented as thread, you could use semaphores to hold them blocked and then create a state machine which controls them through the semaphores.