Control actions with state machine

In this tutorial I cover the following topics

Problem description
Solution with multiple ifs
State machine

Problem description

natalia_car.mov

Solution with multiple ifs

// Output pins (all for LEDs)
#define LED_SYSTEM 7
#define LED_SUBSYSTEM_01 8
#define LED_SUBSYSTEM_02 9
#define LED_SUBSYSTEM_03 10

// Input pins (all for switches)
#define IN_SYSTEM 2
#define IN_SYSTEM_START 3
#define IN_SUBSYSTEM_01 4
#define IN_SUBSYSTEM_02 5
#define IN_SUBSYSTEM_03 6

// Indexes in arrays
#define SYSTEM 0
#define SUBSYSTEM_01 1
#define SUBSYSTEM_02 2
#define SUBSYSTEM_03 3

#define TIME_COUNTDOWN 2000
#define ON HIGH
#define OFF LOW
#define TRUE ON
#define FALSE OFF

int startCountdown = TRUE; //Possible states: TRUE, FALSE
int systemState = OFF; //Possible states: ON, OFF
long countdownTimeBegin;

int ledState[] = {OFF, OFF, OFF, OFF};
int ledTimeOn[] = {0, 1000, 2000, 500};
int ledTimeOff[] = {0, 1000, 500, 500};
long ledTimeBegin[] = {0, 0, 0, 0};

int ledPin[] = {LED_SYSTEM, LED_SUBSYSTEM_01, LED_SUBSYSTEM_02, LED_SUBSYSTEM_03};

void setup() {
  pinMode(LED_SYSTEM, OUTPUT);
  pinMode(LED_SUBSYSTEM_01, OUTPUT);
  pinMode(LED_SUBSYSTEM_02, OUTPUT);
  pinMode(LED_SUBSYSTEM_03, OUTPUT);

  pinMode(IN_SYSTEM, INPUT);
  pinMode(IN_SYSTEM_START, INPUT);
  pinMode(IN_SUBSYSTEM_01, INPUT);
  pinMode(IN_SUBSYSTEM_02, INPUT);
  pinMode(IN_SUBSYSTEM_03, INPUT);

  digitalWrite(LED_SYSTEM, OFF);
  digitalWrite(LED_SUBSYSTEM_01, OFF);
  digitalWrite(LED_SUBSYSTEM_02, OFF);
  digitalWrite(LED_SUBSYSTEM_03, OFF);

  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
}

void loop() {
  loopStart();
}

void loopStart() {
  if (digitalRead(IN_SYSTEM) == HIGH) {
    // Debouncing is needed only on state change, so this solution is not
    // correct but I'll leave it in order not to complicate a code.
    debounce();
    led(SYSTEM, ON);
    if (systemState == OFF) {
      if (digitalRead(IN_SYSTEM_START) == HIGH) {
        debounce();
        if (startCountdown == TRUE) {
          countdownTimeBegin = millis();
          startCountdown = FALSE;
        } else {
          if ((millis() - countdownTimeBegin) > TIME_COUNTDOWN) {
            systemState = ON;
          }
        }
      } else {
        if (startCountdown != TRUE) {
          startCountdown = TRUE;
        }
      }
    } else {
      blinkAllLEDs();
      loopAllSubsystems();
      turnOffAllSystems();
    }
  } else {
    debounce();
    if (systemState == ON) {
      turnOffAllSystems();
    }
  }
}

void loopAllSubsystems() {
  int subsystems[] = {SUBSYSTEM_01, SUBSYSTEM_02, SUBSYSTEM_03};
  int subsystemsPins[] = {IN_SUBSYSTEM_01, IN_SUBSYSTEM_02, IN_SUBSYSTEM_03};
  int SUBSYSTEMS_ALL = 3;
  int i;
  long now;
  
  while (digitalRead(IN_SYSTEM) == HIGH) {
    for (i=0; i<SUBSYSTEMS_ALL; i++) {
      if (digitalRead(subsystemsPins[i]) == HIGH) {
        debounce();
        if (ledState[subsystems[i]] == ON) {
          now = millis();
          if (now - ledTimeBegin[subsystems[i]] > ledTimeOn[subsystems[i]]) {
            ledTimeBegin[subsystems[i]] = now;
            led(subsystems[i], OFF);
          } else {
            led(subsystems[i], ON);
          }
        } else {
          now = millis();
          if (now - ledTimeBegin[subsystems[i]] > ledTimeOff[subsystems[i]]) {
            ledTimeBegin[subsystems[i]] = now;
            led(subsystems[i], ON);
          } else {
            led(subsystems[i], OFF);
          }
        }
      } else {
        debounce();
        ledTimeBegin[subsystems[i]] = millis();
        led(subsystems[i], OFF);
      }
    }
  }
}

// Prevent switch bouncing.
void debounce() {
  delay(15);
}

// Set led of "system" in state "state" if necessary.
void led(int system, int state) {
  if (ledState[system] != state) {
    ledState[system] = state;
    digitalWrite(ledPin[system], state);
  } 
}

// Turn off all running systems if there are any.
void turnOffAllSystems() {
  led(SYSTEM, OFF);
  led(SUBSYSTEM_01, OFF);
  led(SUBSYSTEM_02, OFF);
  led(SUBSYSTEM_03, OFF);
  systemState = OFF;
  startCountdown = TRUE;
}

void blinkAllLEDs() {
  digitalWrite(LED_SUBSYSTEM_01, ON);
  digitalWrite(LED_SUBSYSTEM_02, ON);
  digitalWrite(LED_SUBSYSTEM_03, ON);
  delay(50);
  digitalWrite(LED_SUBSYSTEM_01, OFF);
  digitalWrite(LED_SUBSYSTEM_02, OFF);
  digitalWrite(LED_SUBSYSTEM_03, OFF);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

// Output pins (all for LEDs)

#define LED_SYSTEM 7

#define LED_SUBSYSTEM_01 8

#define LED_SUBSYSTEM_02 9

#define LED_SUBSYSTEM_03 10

// Input pins (all for switches)

#define IN_SYSTEM 2

#define IN_SYSTEM_START 3

#define IN_SUBSYSTEM_01 4

#define IN_SUBSYSTEM_02 5

#define IN_SUBSYSTEM_03 6

// Indexes in arrays

#define SYSTEM 0

#define SUBSYSTEM_01 1

#define SUBSYSTEM_02 2

#define SUBSYSTEM_03 3

#define TIME_COUNTDOWN 2000

#define ON HIGH

#define OFF LOW

#define TRUE ON

#define FALSE OFF

int startCountdown = TRUE; //Possible states: TRUE, FALSE

int systemState = OFF; //Possible states: ON, OFF

long countdownTimeBegin;

int ledState[] = {OFF, OFF, OFF, OFF};

int ledTimeOn[] = {0, 1000, 2000, 500};

int ledTimeOff[] = {0, 1000, 500, 500};

long ledTimeBegin[] = {0, 0, 0, 0};

int ledPin[] = {LED_SYSTEM, LED_SUBSYSTEM_01, LED_SUBSYSTEM_02, LED_SUBSYSTEM_03};

void setup() {

pinMode(LED_SYSTEM, OUTPUT);

pinMode(LED_SUBSYSTEM_01, OUTPUT);

pinMode(LED_SUBSYSTEM_02, OUTPUT);

pinMode(LED_SUBSYSTEM_03, OUTPUT);

pinMode(IN_SYSTEM, INPUT);

pinMode(IN_SYSTEM_START, INPUT);

pinMode(IN_SUBSYSTEM_01, INPUT);

pinMode(IN_SUBSYSTEM_02, INPUT);

pinMode(IN_SUBSYSTEM_03, INPUT);

digitalWrite(LED_SYSTEM, OFF);

digitalWrite(LED_SUBSYSTEM_01, OFF);

digitalWrite(LED_SUBSYSTEM_02, OFF);

digitalWrite(LED_SUBSYSTEM_03, OFF);

Serial.begin(9600);

while (!Serial) {

; // wait for serial port to connect. Needed for native USB port only

}

void loop() {

loopStart();

}

void loopStart() {

if (digitalRead(IN_SYSTEM) == HIGH) {

// Debouncing is needed only on state change, so this solution is not

// correct but I'll leave it in order not to complicate a code.

debounce();

led(SYSTEM, ON);

if (systemState == OFF) {

if (digitalRead(IN_SYSTEM_START) == HIGH) {

debounce();

if (startCountdown == TRUE) {

countdownTimeBegin = millis();

startCountdown = FALSE;

} else {

if ((millis() - countdownTimeBegin) > TIME_COUNTDOWN) {

systemState = ON;

}

} else {

if (startCountdown != TRUE) {

startCountdown = TRUE;

}

} else {

blinkAllLEDs();

loopAllSubsystems();

turnOffAllSystems();

}

} else {

debounce();

if (systemState == ON) {

turnOffAllSystems();

}

void loopAllSubsystems() {

int subsystems[] = {SUBSYSTEM_01, SUBSYSTEM_02, SUBSYSTEM_03};

int subsystemsPins[] = {IN_SUBSYSTEM_01, IN_SUBSYSTEM_02, IN_SUBSYSTEM_03};

int SUBSYSTEMS_ALL = 3;

int i;

long now;

while (digitalRead(IN_SYSTEM) == HIGH) {

for (i=0; i<SUBSYSTEMS_ALL; i++) {

if (digitalRead(subsystemsPins[i]) == HIGH) {

debounce();

if (ledState[subsystems[i]] == ON) {

now = millis();

if (now - ledTimeBegin[subsystems[i]] > ledTimeOn[subsystems[i]]) {

ledTimeBegin[subsystems[i]] = now;

led(subsystems[i], OFF);

} else {

led(subsystems[i], ON);

}

} else {

now = millis();

if (now - ledTimeBegin[subsystems[i]] > ledTimeOff[subsystems[i]]) {

ledTimeBegin[subsystems[i]] = now;

led(subsystems[i], ON);

} else {

led(subsystems[i], OFF);

}

} else {

debounce();

ledTimeBegin[subsystems[i]] = millis();

led(subsystems[i], OFF);

}

// Prevent switch bouncing.

void debounce() {

delay(15);

}

// Set led of "system" in state "state" if necessary.

void led(int system, int state) {

if (ledState[system] != state) {

ledState[system] = state;

digitalWrite(ledPin[system], state);

}

// Turn off all running systems if there are any.

void turnOffAllSystems() {

led(SYSTEM, OFF);

led(SUBSYSTEM_01, OFF);

led(SUBSYSTEM_02, OFF);

led(SUBSYSTEM_03, OFF);

systemState = OFF;

startCountdown = TRUE;

}

void blinkAllLEDs() {

digitalWrite(LED_SUBSYSTEM_01, ON);

digitalWrite(LED_SUBSYSTEM_02, ON);

digitalWrite(LED_SUBSYSTEM_03, ON);

delay(50);

digitalWrite(LED_SUBSYSTEM_01, OFF);

digitalWrite(LED_SUBSYSTEM_02, OFF);

digitalWrite(LED_SUBSYSTEM_03, OFF);

}

State machine

// Output pins (all for LEDs)
#define LED_SYSTEM 7
#define LED_SUBSYSTEM_01 8
#define LED_SUBSYSTEM_02 9
#define LED_SUBSYSTEM_03 10

// Input pins (all for switches)
#define IN_SYSTEM 2
#define IN_SYSTEM_START 3
#define IN_SUBSYSTEM_01 4
#define IN_SUBSYSTEM_02 5
#define IN_SUBSYSTEM_03 6

// Indexes in arrays
#define SYSTEM 0
#define SUBSYSTEM_01 1
#define SUBSYSTEM_02 2
#define SUBSYSTEM_03 3

#define TIME_COUNTDOWN 2000
#define ON HIGH
#define OFF LOW
#define TRUE ON
#define FALSE OFF

#define UNDEFINED -1

byte startCountdown = TRUE; //Possible states: TRUE, FALSE
long countdownTimeBegin;
long now;

byte ledState[] = {OFF, OFF, OFF, OFF};
int ledTimeOn[] = {0, 1000, 2000, 500};
int ledTimeOff[] = {0, 1000, 500, 500};
long ledTimeBegin[] = {0, 0, 0, 0};

int ledPin[] = {LED_SYSTEM, LED_SUBSYSTEM_01, LED_SUBSYSTEM_02, LED_SUBSYSTEM_03};

void setup() {
  pinMode(LED_SYSTEM, OUTPUT);
  pinMode(LED_SUBSYSTEM_01, OUTPUT);
  pinMode(LED_SUBSYSTEM_02, OUTPUT);
  pinMode(LED_SUBSYSTEM_03, OUTPUT);

  pinMode(IN_SYSTEM, INPUT);
  pinMode(IN_SYSTEM_START, INPUT);
  pinMode(IN_SUBSYSTEM_01, INPUT);
  pinMode(IN_SUBSYSTEM_02, INPUT);
  pinMode(IN_SUBSYSTEM_03, INPUT);

  digitalWrite(LED_SYSTEM, OFF);
  digitalWrite(LED_SUBSYSTEM_01, OFF);
  digitalWrite(LED_SUBSYSTEM_02, OFF);
  digitalWrite(LED_SUBSYSTEM_03, OFF);

  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
}

void loop() {
  stateOff();
  loopStateMachine();
}

// Prevent switch bouncing.
void debounce() {
  delay(15);
}

// Set led of "system" in state "state" if necessary.
void led(int system, int state) {
  if (ledState[system] != state) {
    ledState[system] = state;
    digitalWrite(ledPin[system], state);
  } 
}

// Turn off all running systems if there are any.
void turnOffAllSystems() {
  led(SYSTEM, OFF);
  led(SUBSYSTEM_01, OFF);
  led(SUBSYSTEM_02, OFF);
  led(SUBSYSTEM_03, OFF);
  startCountdown = TRUE;
}

void blinkAllLEDs() {
  digitalWrite(LED_SUBSYSTEM_01, ON);
  digitalWrite(LED_SUBSYSTEM_02, ON);
  digitalWrite(LED_SUBSYSTEM_03, ON);
  delay(50);
  digitalWrite(LED_SUBSYSTEM_01, OFF);
  digitalWrite(LED_SUBSYSTEM_02, OFF);
  digitalWrite(LED_SUBSYSTEM_03, OFF);
}

// Output pins (all for LEDs)

#define LED_SYSTEM 7

#define LED_SUBSYSTEM_01 8

#define LED_SUBSYSTEM_02 9

#define LED_SUBSYSTEM_03 10

// Input pins (all for switches)

#define IN_SYSTEM 2

#define IN_SYSTEM_START 3

#define IN_SUBSYSTEM_01 4

#define IN_SUBSYSTEM_02 5

#define IN_SUBSYSTEM_03 6

// Indexes in arrays

#define SYSTEM 0

#define SUBSYSTEM_01 1

#define SUBSYSTEM_02 2

#define SUBSYSTEM_03 3

#define TIME_COUNTDOWN 2000

#define ON HIGH

#define OFF LOW

#define TRUE ON

#define FALSE OFF

#define UNDEFINED -1

byte startCountdown = TRUE; //Possible states: TRUE, FALSE

long countdownTimeBegin;

long now;

byte ledState[] = {OFF, OFF, OFF, OFF};

int ledTimeOn[] = {0, 1000, 2000, 500};

int ledTimeOff[] = {0, 1000, 500, 500};

long ledTimeBegin[] = {0, 0, 0, 0};

int ledPin[] = {LED_SYSTEM, LED_SUBSYSTEM_01, LED_SUBSYSTEM_02, LED_SUBSYSTEM_03};

void setup() {

pinMode(LED_SYSTEM, OUTPUT);

pinMode(LED_SUBSYSTEM_01, OUTPUT);

pinMode(LED_SUBSYSTEM_02, OUTPUT);

pinMode(LED_SUBSYSTEM_03, OUTPUT);

pinMode(IN_SYSTEM, INPUT);

pinMode(IN_SYSTEM_START, INPUT);

pinMode(IN_SUBSYSTEM_01, INPUT);

pinMode(IN_SUBSYSTEM_02, INPUT);

pinMode(IN_SUBSYSTEM_03, INPUT);

digitalWrite(LED_SYSTEM, OFF);

digitalWrite(LED_SUBSYSTEM_01, OFF);

digitalWrite(LED_SUBSYSTEM_02, OFF);

digitalWrite(LED_SUBSYSTEM_03, OFF);

Serial.begin(9600);

while (!Serial) {

; // wait for serial port to connect. Needed for native USB port only

}

void loop() {

stateOff();

loopStateMachine();

}

// Prevent switch bouncing.

void debounce() {

delay(15);

}

// Set led of "system" in state "state" if necessary.

void led(int system, int state) {

if (ledState[system] != state) {

ledState[system] = state;

digitalWrite(ledPin[system], state);

}

// Turn off all running systems if there are any.

void turnOffAllSystems() {

led(SYSTEM, OFF);

led(SUBSYSTEM_01, OFF);

led(SUBSYSTEM_02, OFF);

led(SUBSYSTEM_03, OFF);

startCountdown = TRUE;

}

void blinkAllLEDs() {

digitalWrite(LED_SUBSYSTEM_01, ON);

digitalWrite(LED_SUBSYSTEM_02, ON);

digitalWrite(LED_SUBSYSTEM_03, ON);

delay(50);

digitalWrite(LED_SUBSYSTEM_01, OFF);

digitalWrite(LED_SUBSYSTEM_02, OFF);

digitalWrite(LED_SUBSYSTEM_03, OFF);

}

LED_ON, LED_OFF 
akcje związane z "miganiem" diody;
aktywne gdy odpowiedni przycisk jest w stanie ON

SUB_01--CONTINUE--SUB_01_OFF
gdy odpowiedni przycisk jest w stanie OFF, to wyłączam
procedurę mrugania i przechodzę do sprawdzenia kolejnej diody.

LED_ON, LED_OFF

akcje związane z "miganiem" diody;

aktywne gdy odpowiedni przycisk jest w stanie ON

SUB_01--CONTINUE--SUB_01_OFF

gdy odpowiedni przycisk jest w stanie OFF, to wyłączam

procedurę mrugania i przechodzę do sprawdzenia kolejnej diody.

#define INDEX_STATE_CURRENT 0
#define INDEX_ACTION 1
#define INDEX_STATE_NEW 2

#define STATE_OFF 0
#define STATE_ON 1
#define STATE_COUNTDOWN 2
#define STATE_COUNTDOWN_RESET 3
#define STATE_ACTIVE 4
#define STATE_BLINK 5
#define STATE_SUB_01 6
#define STATE_SUB_02 7
#define STATE_SUB_03 8
#define STATE_LED_01_OFF 9
#define STATE_LED_01_ON 10
#define STATE_LED_02_OFF 11
#define STATE_LED_02_ON 12
#define STATE_LED_03_OFF 13
#define STATE_LED_03_ON 14
#define STATE_SUB_01_OFF 15
#define STATE_SUB_02_OFF 16
#define STATE_SUB_03_OFF 17

#define ACTION_NONE -1
#define ACTION_POWER_ON 0
#define ACTION_POWER_OFF 1
#define ACTION_CONTINUE 3
#define ACTION_STARTER_ON 4
#define ACTION_STARTER_OFF 5
#define ACTION_SYSTEM_ON 6
#define ACTION_TICK 7
#define ACTION_LED_ON 8
#define ACTION_LED_OFF 9

#define NUMBER_OF_ROUTES 33

// Warning!!!
// Remember to update NUMBER_OF_ROUTES when changing routing table
byte   routes[][3] = 
                   {{STATE_OFF, ACTION_POWER_ON, STATE_ACTIVE},
                   
                   {STATE_ACTIVE, ACTION_STARTER_ON, STATE_COUNTDOWN},
                   {STATE_ACTIVE, ACTION_STARTER_OFF, STATE_COUNTDOWN_RESET},
                   {STATE_ACTIVE, ACTION_POWER_OFF, STATE_OFF},
                   
                   {STATE_COUNTDOWN, ACTION_SYSTEM_ON, STATE_BLINK},
                   {STATE_COUNTDOWN, ACTION_CONTINUE, STATE_ACTIVE},
                   {STATE_COUNTDOWN, ACTION_POWER_OFF, STATE_OFF},
                   {STATE_COUNTDOWN_RESET, ACTION_CONTINUE, STATE_ACTIVE},
                   {STATE_COUNTDOWN_RESET, ACTION_POWER_OFF, STATE_OFF},

                   {STATE_BLINK, ACTION_SYSTEM_ON, STATE_ON},
                   
                   {STATE_ON, ACTION_TICK, STATE_SUB_01},
                   {STATE_ON, ACTION_POWER_OFF, STATE_OFF},
                   
                   {STATE_SUB_01, ACTION_LED_ON, STATE_LED_01_ON},
                   {STATE_SUB_01, ACTION_LED_OFF, STATE_LED_01_OFF},
                   {STATE_SUB_01, ACTION_CONTINUE, STATE_SUB_01_OFF},
                   {STATE_SUB_01, ACTION_POWER_OFF, STATE_OFF},

                   {STATE_SUB_02, ACTION_LED_ON, STATE_LED_02_ON},
                   {STATE_SUB_02, ACTION_LED_OFF, STATE_LED_02_OFF},
                   {STATE_SUB_02, ACTION_CONTINUE, STATE_SUB_02_OFF},
                   {STATE_SUB_02, ACTION_POWER_OFF, STATE_OFF},

                   {STATE_SUB_03, ACTION_LED_ON, STATE_LED_03_ON},
                   {STATE_SUB_03, ACTION_LED_OFF, STATE_LED_03_OFF},
                   {STATE_SUB_03, ACTION_CONTINUE, STATE_SUB_03_OFF},
                   {STATE_SUB_03, ACTION_POWER_OFF, STATE_OFF},
                   
                   {STATE_LED_01_ON, ACTION_CONTINUE, STATE_SUB_02},
                   {STATE_LED_01_OFF, ACTION_CONTINUE, STATE_SUB_02},

                   {STATE_LED_02_ON, ACTION_CONTINUE, STATE_SUB_03},
                   {STATE_LED_02_OFF, ACTION_CONTINUE, STATE_SUB_03},

                   {STATE_LED_03_ON, ACTION_CONTINUE, STATE_ON},
                   {STATE_LED_03_OFF, ACTION_CONTINUE, STATE_ON},
                   
                   {STATE_SUB_01_OFF, ACTION_CONTINUE, STATE_SUB_02},
                   {STATE_SUB_02_OFF, ACTION_CONTINUE, STATE_SUB_03},
                   {STATE_SUB_03_OFF, ACTION_CONTINUE, STATE_ON}
                   }; 

byte state;
byte action;
byte action_last;

void stateOn() {
  if (digitalRead(IN_SYSTEM) == LOW) {
    debounce();
    action = ACTION_POWER_OFF;
  } else {
    debounce();
    action = ACTION_TICK;
  }
}

void stateOff() {
  turnOffAllSystems();
  while(true) {
    if (digitalRead(IN_SYSTEM) == HIGH) {
      debounce();
      break;
    }
  }
  action = ACTION_POWER_ON;
}

void stateCountdown() {
  if (digitalRead(IN_SYSTEM) == LOW) {
    debounce();
    action = ACTION_POWER_OFF;
  } else {
    debounce();
    if (startCountdown == TRUE) {
      countdownTimeBegin = millis();
      startCountdown = FALSE;
      action = ACTION_CONTINUE;
    } else {
      if ((millis() - countdownTimeBegin) > TIME_COUNTDOWN) {
        action = ACTION_SYSTEM_ON;
      } else {
        action = ACTION_CONTINUE;
      }
    }
  }
}

void stateCountdownReset() {
  if (digitalRead(IN_SYSTEM) == LOW) {
    debounce();
    action = ACTION_POWER_OFF;
  } else {
    debounce();
    if (startCountdown != TRUE) {
       startCountdown = TRUE;
    }
    action = ACTION_CONTINUE;
  }
}

void stateActive() {
  led(SYSTEM, ON);
  if (digitalRead(IN_SYSTEM) == LOW) {
    debounce();
    action = ACTION_POWER_OFF;
  } else if (digitalRead(IN_SYSTEM_START) == HIGH) {
    debounce();
    action = ACTION_STARTER_ON;
  } else {
    debounce();
    action = ACTION_STARTER_OFF;
  }
}

void stateBlink() {
  blinkAllLEDs();
  action = ACTION_SYSTEM_ON;
}

void stateSub(int systemID) {
  byte subsystemsPins[] = {UNDEFINED, IN_SUBSYSTEM_01, IN_SUBSYSTEM_02, IN_SUBSYSTEM_03};

  if (digitalRead(IN_SYSTEM) == LOW) {
    debounce();
    action = ACTION_POWER_OFF;
  } else {
    if (digitalRead(subsystemsPins[systemID]) == LOW) {
      debounce();
      ledTimeBegin[systemID] = millis();
      action = ACTION_CONTINUE;
    } else {
      debounce();
      if (ledState[systemID] == ON) {
        now = millis();
        if (now - ledTimeBegin[systemID] > ledTimeOn[systemID]) {
          ledTimeBegin[systemID] = now;
          action = ACTION_LED_OFF;
        } else {
          action = ACTION_LED_ON;
        }
      } else {
        now = millis();
        if (now - ledTimeBegin[systemID] > ledTimeOff[systemID]) {
          ledTimeBegin[systemID] = now;
          action = ACTION_LED_ON;
        } else {
          action = ACTION_LED_OFF;
        }
      }
    }
  }
}

void stateLED_on(int systemID) {
  led(systemID, ON);
  action = ACTION_CONTINUE;
}

void stateLED_off(int systemID) {
  led(systemID, OFF);
  action = ACTION_CONTINUE;
}

void stateSub_off(int systemID) {
  led(systemID, OFF);
  action = ACTION_CONTINUE;
}

void executeStateAction(){
  switch(state)
  {
    case STATE_ON:
      stateOn();
      break;
    case STATE_OFF:
      stateOff();
      break;
    case STATE_COUNTDOWN:
      stateCountdown();
      break;
    case STATE_COUNTDOWN_RESET:
      stateCountdownReset();
      break;
    case STATE_ACTIVE:
      stateActive();
      break;
    case STATE_BLINK:
      stateBlink();
      break;
    case STATE_SUB_01:
      stateSub(SUBSYSTEM_01);
      break;
    case STATE_SUB_02:
      stateSub(SUBSYSTEM_02);
      break;
    case STATE_SUB_03:
      stateSub(SUBSYSTEM_03);
      break;
    case STATE_LED_01_OFF:
      stateLED_off(SUBSYSTEM_01);
      break;
    case STATE_LED_01_ON:
      stateLED_on(SUBSYSTEM_01);
      break;
    case STATE_LED_02_OFF:
      stateLED_off(SUBSYSTEM_02);
      break;
    case STATE_LED_02_ON:
      stateLED_on(SUBSYSTEM_02);
      break;
    case STATE_LED_03_OFF:
      stateLED_off(SUBSYSTEM_03);
      break;
    case STATE_LED_03_ON:
      stateLED_on(SUBSYSTEM_03);
      break;
    case STATE_SUB_01_OFF:
      stateSub_off(SUBSYSTEM_01);
      break;
    case STATE_SUB_02_OFF:
      stateSub_off(SUBSYSTEM_02);
      break;
    case STATE_SUB_03_OFF:
      stateSub_off(SUBSYSTEM_03);
      break;
  }
}

void loopStateMachine(){
  byte i;

  while(true) {
    if(action != ACTION_NONE){
      action_last = action;
      action = ACTION_NONE;
      for(i=0; i<NUMBER_OF_ROUTES; ++i){
        if(routes[i][INDEX_STATE_CURRENT] == state &&
          routes[i][INDEX_ACTION] == action_last){
          state = routes[i][INDEX_STATE_NEW];
          break;
        }
      }
      executeStateAction();
    }
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

#define INDEX_STATE_CURRENT 0

#define INDEX_ACTION 1

#define INDEX_STATE_NEW 2

#define STATE_OFF 0

#define STATE_ON 1

#define STATE_COUNTDOWN 2

#define STATE_COUNTDOWN_RESET 3

#define STATE_ACTIVE 4

#define STATE_BLINK 5

#define STATE_SUB_01 6

#define STATE_SUB_02 7

#define STATE_SUB_03 8

#define STATE_LED_01_OFF 9

#define STATE_LED_01_ON 10

#define STATE_LED_02_OFF 11

#define STATE_LED_02_ON 12

#define STATE_LED_03_OFF 13

#define STATE_LED_03_ON 14

#define STATE_SUB_01_OFF 15

#define STATE_SUB_02_OFF 16

#define STATE_SUB_03_OFF 17

#define ACTION_NONE -1

#define ACTION_POWER_ON 0

#define ACTION_POWER_OFF 1

#define ACTION_CONTINUE 3

#define ACTION_STARTER_ON 4

#define ACTION_STARTER_OFF 5

#define ACTION_SYSTEM_ON 6

#define ACTION_TICK 7

#define ACTION_LED_ON 8

#define ACTION_LED_OFF 9

#define NUMBER_OF_ROUTES 33

// Warning!!!

// Remember to update NUMBER_OF_ROUTES when changing routing table

byte routes[][3] =

{{STATE_OFF, ACTION_POWER_ON, STATE_ACTIVE},

{STATE_ACTIVE, ACTION_STARTER_ON, STATE_COUNTDOWN},

{STATE_ACTIVE, ACTION_STARTER_OFF, STATE_COUNTDOWN_RESET},

{STATE_ACTIVE, ACTION_POWER_OFF, STATE_OFF},

{STATE_COUNTDOWN, ACTION_SYSTEM_ON, STATE_BLINK},

{STATE_COUNTDOWN, ACTION_CONTINUE, STATE_ACTIVE},

{STATE_COUNTDOWN, ACTION_POWER_OFF, STATE_OFF},

{STATE_COUNTDOWN_RESET, ACTION_CONTINUE, STATE_ACTIVE},

{STATE_COUNTDOWN_RESET, ACTION_POWER_OFF, STATE_OFF},

{STATE_BLINK, ACTION_SYSTEM_ON, STATE_ON},

{STATE_ON, ACTION_TICK, STATE_SUB_01},

{STATE_ON, ACTION_POWER_OFF, STATE_OFF},

{STATE_SUB_01, ACTION_LED_ON, STATE_LED_01_ON},

{STATE_SUB_01, ACTION_LED_OFF, STATE_LED_01_OFF},

{STATE_SUB_01, ACTION_CONTINUE, STATE_SUB_01_OFF},

{STATE_SUB_01, ACTION_POWER_OFF, STATE_OFF},

{STATE_SUB_02, ACTION_LED_ON, STATE_LED_02_ON},

{STATE_SUB_02, ACTION_LED_OFF, STATE_LED_02_OFF},

{STATE_SUB_02, ACTION_CONTINUE, STATE_SUB_02_OFF},

{STATE_SUB_02, ACTION_POWER_OFF, STATE_OFF},

{STATE_SUB_03, ACTION_LED_ON, STATE_LED_03_ON},

{STATE_SUB_03, ACTION_LED_OFF, STATE_LED_03_OFF},

{STATE_SUB_03, ACTION_CONTINUE, STATE_SUB_03_OFF},

{STATE_SUB_03, ACTION_POWER_OFF, STATE_OFF},

{STATE_LED_01_ON, ACTION_CONTINUE, STATE_SUB_02},

{STATE_LED_01_OFF, ACTION_CONTINUE, STATE_SUB_02},

{STATE_LED_02_ON, ACTION_CONTINUE, STATE_SUB_03},

{STATE_LED_02_OFF, ACTION_CONTINUE, STATE_SUB_03},

{STATE_LED_03_ON, ACTION_CONTINUE, STATE_ON},

{STATE_LED_03_OFF, ACTION_CONTINUE, STATE_ON},

{STATE_SUB_01_OFF, ACTION_CONTINUE, STATE_SUB_02},

{STATE_SUB_02_OFF, ACTION_CONTINUE, STATE_SUB_03},

{STATE_SUB_03_OFF, ACTION_CONTINUE, STATE_ON}

};

byte state;

byte action;

byte action_last;

void stateOn() {

if (digitalRead(IN_SYSTEM) == LOW) {

debounce();

action = ACTION_POWER_OFF;

} else {

debounce();

action = ACTION_TICK;

}

void stateOff() {

turnOffAllSystems();

while(true) {

if (digitalRead(IN_SYSTEM) == HIGH) {

debounce();

break;

}

action = ACTION_POWER_ON;

}

void stateCountdown() {

if (digitalRead(IN_SYSTEM) == LOW) {

debounce();

action = ACTION_POWER_OFF;

} else {

debounce();

if (startCountdown == TRUE) {

countdownTimeBegin = millis();

startCountdown = FALSE;

action = ACTION_CONTINUE;

} else {

if ((millis() - countdownTimeBegin) > TIME_COUNTDOWN) {

action = ACTION_SYSTEM_ON;

} else {

action = ACTION_CONTINUE;

}

void stateCountdownReset() {

if (digitalRead(IN_SYSTEM) == LOW) {

debounce();

action = ACTION_POWER_OFF;

} else {

debounce();

if (startCountdown != TRUE) {

startCountdown = TRUE;

}

action = ACTION_CONTINUE;

}

void stateActive() {

led(SYSTEM, ON);

if (digitalRead(IN_SYSTEM) == LOW) {

debounce();

action = ACTION_POWER_OFF;

} else if (digitalRead(IN_SYSTEM_START) == HIGH) {

debounce();

action = ACTION_STARTER_ON;

} else {

debounce();

action = ACTION_STARTER_OFF;

}

void stateBlink() {

blinkAllLEDs();

action = ACTION_SYSTEM_ON;

}

void stateSub(int systemID) {

byte subsystemsPins[] = {UNDEFINED, IN_SUBSYSTEM_01, IN_SUBSYSTEM_02, IN_SUBSYSTEM_03};

if (digitalRead(IN_SYSTEM) == LOW) {

debounce();

action = ACTION_POWER_OFF;

} else {

if (digitalRead(subsystemsPins[systemID]) == LOW) {

debounce();

ledTimeBegin[systemID] = millis();

action = ACTION_CONTINUE;

} else {

debounce();

if (ledState[systemID] == ON) {

now = millis();

if (now - ledTimeBegin[systemID] > ledTimeOn[systemID]) {

ledTimeBegin[systemID] = now;

action = ACTION_LED_OFF;

} else {

action = ACTION_LED_ON;

}

} else {

now = millis();

if (now - ledTimeBegin[systemID] > ledTimeOff[systemID]) {

ledTimeBegin[systemID] = now;

action = ACTION_LED_ON;

} else {

action = ACTION_LED_OFF;

}

void stateLED_on(int systemID) {

led(systemID, ON);

action = ACTION_CONTINUE;

}

void stateLED_off(int systemID) {

led(systemID, OFF);

action = ACTION_CONTINUE;

}

void stateSub_off(int systemID) {

led(systemID, OFF);

action = ACTION_CONTINUE;

}

void executeStateAction(){

switch(state)

{

case STATE_ON:

stateOn();

break;

case STATE_OFF:

stateOff();

break;

case STATE_COUNTDOWN:

stateCountdown();

break;

case STATE_COUNTDOWN_RESET:

stateCountdownReset();

break;

case STATE_ACTIVE:

stateActive();

break;

case STATE_BLINK:

stateBlink();

break;

case STATE_SUB_01:

stateSub(SUBSYSTEM_01);

break;

case STATE_SUB_02:

stateSub(SUBSYSTEM_02);

break;

case STATE_SUB_03:

stateSub(SUBSYSTEM_03);

break;

case STATE_LED_01_OFF:

stateLED_off(SUBSYSTEM_01);

break;

case STATE_LED_01_ON:

stateLED_on(SUBSYSTEM_01);

break;

case STATE_LED_02_OFF:

stateLED_off(SUBSYSTEM_02);

break;

case STATE_LED_02_ON:

stateLED_on(SUBSYSTEM_02);

break;

case STATE_LED_03_OFF:

stateLED_off(SUBSYSTEM_03);

break;

case STATE_LED_03_ON:

stateLED_on(SUBSYSTEM_03);

break;

case STATE_SUB_01_OFF:

stateSub_off(SUBSYSTEM_01);

break;

case STATE_SUB_02_OFF:

stateSub_off(SUBSYSTEM_02);

break;

case STATE_SUB_03_OFF:

stateSub_off(SUBSYSTEM_03);

break;

}

void loopStateMachine(){

byte i;

while(true) {

if(action != ACTION_NONE){

action_last = action;

action = ACTION_NONE;

for(i=0; i<NUMBER_OF_ROUTES; ++i){

if(routes[i][INDEX_STATE_CURRENT] == state &&

routes[i][INDEX_ACTION] == action_last){

state = routes[i][INDEX_STATE_NEW];

break;

}

executeStateAction();

}