Multi-threaded Robot 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)&lt;5 || Ultrasonic_value(R_trig,R_echo)&lt;5)

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

}

// Run Motor
void Runmotor(int which)
{
  for(int i=0;i&lt;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&lt;=180;angle++)
{ 
 S.write(angle); 
 if(angle==90)
 {
   delay(2000);
 }
}
delay(2000);
for(angle=180;angle&gt;=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
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:


Highest priority first.
If the priority is the same then the thread with the earliest deadline is executed first.
If two threads have the same deadline then the first created thread will execute first.
A thread can only be interrupted by threads with higher priority.
Once a thread is executing it will block execution for all threads with lower priority until the run function returns.
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()-&gt;enableThreadExecution();