Multi-threaded Robot
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();