Aleksei Tertychnyi's pages

Some example how to blink multiple LEDs differenly  with only one function queue with chained functions approach. Do not need to warry about any LED blinking any more :)

#include <antirtos.h>

typedef
struct pinOut { // structure (index - pin number, logic - 1/0 = ON/OFF)
  int index;
  int logic;
  int period;
} pinout ;

del_fQP<pinout> Q1(9); // maximum 9 function pointers with parameters in queue

void writePin(pinout cmd) { // write a pin logic

  digitalWrite(cmd.index, cmd.logic);
  if (cmd.logic) // inverting logic
    cmd.logic = LOW;
  else
    cmd.logic = HIGH;

  Q1.push_delayed(writePin, cmd, cmd.period); //add this function into a queue again
}

void setup() {
  // put your setup code here, to run once:
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);

  //timet setup
  TCCR1A = 0x00;  //Normal Mode
  TCCR1B = 0x00;  //TC1 is OFF
  TCNT1 = 0;
  OCR1A = 6250;  //0.1s delay; prescaler 256
  bitSet(TIMSK1, OCIE1A);   //local intterupt is active
  TCCR1B |= bit(CS12);   //Start TC1 with prescale 256

  Q1.push_delayed(writePin, {5, HIGH, 5}, 5);   // period 1 sec.
  Q1.push_delayed(writePin, {6, HIGH, 10}, 10); // period 2 sec.
  Q1.push_delayed(writePin, {7, HIGH, 20}, 20); // period 4 sec.
  Q1.push_delayed(writePin, {8, HIGH, 40}, 40); // period 8 sec.
  Q1.push_delayed(writePin, {9, HIGH, 80}, 80); // period 16 sec.
  Q1.push_delayed(writePin, {10, HIGH, 160}, 160); // period 32 sec.
  Q1.push_delayed(writePin, {11, HIGH, 320}, 320); // period 64 sec.
  Q1.push_delayed(writePin, {12, HIGH, 640}, 640); //period 128 sec.

}

void loop() {
  // put your main code here, to run repeatedly:
  Q1.pull(); // pull from the queue
}

ISR(TIMER1_COMPA_vect) // timer interrupt ticks one per 0.1 sec
{
  TCNT1 = 0;
  OCR1A = 6250;
  Q1.tick(); // execute tick method for make delayed functionality works
}

 Try it youself here: wokwi

Here below is easy to repeat ease LEDs test, utilizing one of the classes on the ANTIRTOS library, classes are in depended, so you may use one you need. See simulation here: Simulation in wokwi.com 

The ANTIRTOS is a broad use library, not only for the Arduino,  use it where ever you want! It is native C++.

Example run on Arduino Nano (including everything, no any external library include needed):

// HERE IS ONE OF ANTIRTOS CLASSes below : del_fQP (see the rest desc. on https://github.com/WeSpeakEnglish/ANTIRTOS )
/// @brief delayed functional pointers queue with parameters
template <typename T>
class del_fQP {
private:
    int first;
    volatile int last;
    int length;
    unsigned int time;
    typedef void (*fP)(T);
    fP * FP_Queue;
    fP * del_FP_Queue;                  // delayed functions
    bool * execArr;                     //is need to be executed?
    unsigned int * execTime;            //execution time arr 
    T* PARAMS_array;
    T* delayed_PARAMS_array;
    int push(void (*pointerQ)(T), T parameterQ);
    
public:
    del_fQP(int sizeQ);
    ~del_fQP();
    int push_delayed(void (*pointerQ)(T), T parameterQ, unsigned int delayTime);
    void tick(void);
    int pull();
};

template <typename T>
del_fQP<T>::del_fQP(int sizeQ) {
    FP_Queue = new fP[sizeQ];
    del_FP_Queue = new fP[sizeQ];
    execArr = new bool[sizeQ];
    PARAMS_array = new T[sizeQ];
    delayed_PARAMS_array = new T[sizeQ];
    execTime = new unsigned int[sizeQ];
    last = 0;
    first = 0;
    time = 0;
    for(unsigned int i = 0; i < sizeQ; i++){
      execArr[i] = false;
    }
    length = sizeQ;
}

template <typename T>
del_fQP<T>::~del_fQP() {
    delete[] FP_Queue;
    delete[] del_FP_Queue;
    delete[] PARAMS_array;
    delete[] delayed_PARAMS_array;
    delete [] execArr;
    delete [] execTime;
}

template <typename T>
int del_fQP<T>::push(void (*pointerQ)(T), T parameterQ) {
    if ((last + 1) % length == first) return 1;
    FP_Queue[last] = pointerQ;
    PARAMS_array[last] = parameterQ;
    last = (last + 1) % length;
    return 0;
}

template <typename T>
int del_fQP<T>::push_delayed(void (*pointerQ)(T), T parameterQ, unsigned int delayTime) {
bool fullQ = true;                                              // is Queue full?
     for(unsigned int i = 0; i < length; i++){
      if (!execArr[i] ){
       del_FP_Queue[i] = pointerQ;                              // put function pointer into exec queue 
       delayed_PARAMS_array[i] = parameterQ;                    // put parameter into exec queue    
       execArr[i] = true;                                       // true flag for execution
       execTime[i] = time + delayTime;                          //calc execution time, no worry if overload
       fullQ = false;
       break;
       }
  }
  if (fullQ) return 1;
  return 0;
}

template <typename T>
void del_fQP<T>::tick(void){
  static unsigned int i = 0 ;  //uses in search cycle every tick
   for(i=0; i < length; i++){
     if(execTime[i] == time)
      if(execArr[i]){
       push(del_FP_Queue[i],delayed_PARAMS_array[i]);  // bump into normal queue part of delayed Queue
       execArr[i] = false;
     }
   }
  time++;
}

template <typename T>
int del_fQP<T>::pull() {
    fP pullVar;
    if (last != first) {
        T Params = PARAMS_array[first];
        pullVar = FP_Queue[first];
        first = (first + 1) % length;
        pullVar(Params);
        return 0;
    }
    else {
        return 1;
    }
}
//////////////////////////////////////////// HERE THE SIMLE TEST BELOW//////////////////////////////////
typedef 
struct pinOut{ // structure (index - pin number, logic - 1/0 = ON/OFF)
  int index;
  bool logic;
} pinout ;

del_fQP<pinout> Q1(8); // maximum 8 function pointers with parameters in queue

void writePin(pinout cmd){ // write a pin true =- ON
  
  digitalWrite(cmd.index, cmd.logic);

}

void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);
  pinMode(12, OUTPUT);

  TCCR1A = 0x00;  //Normal Mode
  TCCR1B = 0x00;  //TC1 is OFF
  TCNT1 = 0;
  OCR1A = 6250;  //0.1s delay; prescaler 256
  bitSet(TIMSK1, OCIE1A);   //local intterupt is active
  TCCR1B |= bit(CS12);   //Start TC1 with prescale 256
  
  // here putting 8 pointers to functions, parameters and delays needed

  Q1.push_delayed(writePin,{12,true},20); //yellow led ON after 2 sec. (0.1*20 = 2 seconds)
  Q1.push_delayed(writePin,{12,false},30); //yellow led OFF after 3 sec.
 Q1.push_delayed(writePin,{...

fQ is a class for creation simple queue of procedures (functions  getting and returning void).
An example:

#include <antirtos.h>
void task1(void){   // some procedure
  //do something
}
void task2(void){   // some procedure
  //do something
}
fQ queue1(8); // create a FIFO queue of 8 pointers length
queue1.push(task1); // anywhere, for example example in some interrupt
queue1.push(task2); // for example in the same interrupt
//.....
queue1.pull();  // in the main loop

fQP is a class for creation queue of functions receiving parameter, it may be your own class with fixed size, like structure or standard one.
An example:

uint32_t somevalue1 = 123456;  //value to call task1 with
uint32_t somevalue2 = 789012; // /value to call task1 with

void task1(uint32_t){   // some function with parameter
  //do something
}
void task2(uint32_t){   // some function with parameter
  //do something
}
fQP<uint32_t> queue2(4); // create a FIFO queue of 4 pointers length for functions returning void, and receiving uint32_t parameter

queue2.push(task1, somevalue1); // anywhere, for example example in some interrupt
queue2.push(task2, somevalue2); // for example in the same interrupt
//.....
queue2.pull();  // in the main loop

del_fQ is a class for creating simple queue of procedures (functions  getting and returning void) but delayed for some 'ticks'.
An example:

void task1(void){   // some procedure
  //do something
}
void task2(void){   // some procedure
  //do something
}
del_fQ queue3(8); // create a 'delayed'  FIFO queue of 8 pointers length for functions returning void, and receiving void

queue3.push(task1, 123); // anywhere, for example example in some interrupt, this task will be delayed for delay1 ticks from here 
                         // this delay const or variable of type unsigned int 
queue3.push(task2, 344); // for example in the same interrupt, this task will be delayed for delay2 ticks from here
                         //  this delay const or variable of type unsigned int 
//.....
queue3.pull();  // in the main loop

//...
queue3.tick();  //in same periodic function, it maybe for example some timer elapsed interrupt, or even main loop