Navbot/Scheduler.cpp at master · gregk27/Navbot · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
#include "Scheduler.h"
#include <Arduino.h>

// -------------------
// Helper functions
// -------------------

/**
* Command for delay, used by addDelay()
*/
class DelayCommand: public Command {
  private:
    unsigned int duration;
    unsigned long startTime;
  public:
    DelayCommand(unsigned int duration){this->duration = duration;}
    void init(){startTime = millis();}
    bool isFinished(){return millis()-startTime > duration;}
};

// -------------------
// Class functions
// -------------------

Scheduler *Scheduler::master = 0x0;

Scheduler::Scheduler(int maxCommands)
  :schedule(maxCommands){
  currentCommand = 0;
}

void Scheduler::addCommand(Command *command){
  schedule.add(command);
}

bool Scheduler::interrupt(Command *command){
  if(interruptCommand) return false;
  interruptCommand = command;
  interruptCommand->init();
  return true;
}

void Scheduler::addDelay(unsigned int duration){
  addCommand(new DelayCommand(duration));
}

unsigned int Scheduler::getIteration(){
  return iteration;
}

void Scheduler::init() {
  if(currentCommand >= schedule.size()) return;
  Command *c = schedule[currentCommand];
  c->init();
}

void Scheduler::periodic(){
  // Exit if schedule complete
  if(currentCommand >= schedule.size()) return;
  // Increment iteration counter
  iteration ++;

  Command *c = schedule[currentCommand];
  // Run interrupt command if active
  if(interruptCommand) c = interruptCommand;
  // Call periodic function
  c->periodic();

  // If the command is finished
  if(c->isFinished()){
    // End the current command
    c->end();
    if(interruptCommand){
      // If it's an interupt command, remove the dangling pointer
      interruptCommand = 0x0;
    } else {
      // Initialise the next
      currentCommand ++;
      // Exit if schedule complete
      if(currentCommand >= schedule.size()) return;
      Serial.print(F("Starting command: "));
      Serial.print(currentCommand+1);
      Serial.print(F("/"));
      Serial.println(schedule.size());
      c = schedule[currentCommand];
      c->init();
    }
  }
}

void Scheduler::end(){
  if(currentCommand >= schedule.size()) return;
  schedule[currentCommand]->end();
}

bool Scheduler::isFinished(){
  return currentCommand >= schedule.size();
}