Reddit servo + LED v2.0

/*
  Reddit servo example --- v 2.0
  Just moves a servo and turns a LED on and off, without using delay()

  This version uses a state flag to control the whole program.
  The main loop as a switch-case statement that separates what happens into different steps.
  This model is somewhat easier to write complex processes with, since you can directly state what happens at every stage.

  Board: any
  Other hardware: an RC servo and an LED
*/

#include <Servo.h> // Standard Arduino IDE servo library


int program_state = 0; // This variable controls everything that happens.
//                        In this particular example, there are 16 stages of the program that are numbered from zero to 15.
//                        Placing serial.print statements into each stage allows you to easily troubleshoot where a program gets stuck or skips.

Servo servo;
const int LED_pin = 3;
const int servo_pin = 9;
int position_to_use = 0;

int servo_positions[] = {0, 50, 180, 90};
int delay_times[] = {1000, 2000, 3000, 4000}; // These are millisecond time delay values
int led_light_up_value = 180;

bool servo_waiting = false; // This is used to change between moving servos and waiting for a time delay.
int current_delay = 0; // Whatever current value time delay is stored in here.
unsigned long delay_begin_time = 0;
unsigned long delay_current_time = 0;


void setup() {
  Serial.begin(9600);
  pinMode(LED_pin, OUTPUT);
  servo.attach(servo_pin);

  pinMode(13, OUTPUT);
  digitalWrite(13, LOW); // The particular clone Uno board I used needs this or it sets it to HIGH...

  Serial.println("Exiting setup()");
}


void loop() {
  switch (program_state) {

    case 0:
      show_program_state();
      servo.write(servo_positions[position_to_use]);
      program_state = 1;
      break;

    case 1:
      show_program_state();
      check_LED_state(servo_positions[position_to_use]);
      program_state = 2;
      break;

    case 2:
      show_program_state();
      start_waiting_period(delay_times[position_to_use]);
      program_state = 3;
      break;

    case 3:
      // This stage gets checked a bunch of times while it is waiting to re-enable the servo,
      // so it only prints the program_stage when it is completed.
      check_to_enable_servo();
      if (servo_waiting == false) {
        show_program_state();
        program_state = 4;
        position_to_use = 1;
      }
      break;

    case 4:
      show_program_state();
      servo.write(servo_positions[position_to_use]);
      program_state = 5;
      break;

    case 5:
      show_program_state();
      check_LED_state(servo_positions[position_to_use]);
      program_state = 6;
      break;

    case 6:
      show_program_state();
      start_waiting_period(delay_times[position_to_use]);
      program_state = 7;
      break;

    case 7:
      // This stage gets checked a bunch of times while it is waiting to re-enable the servo,
      // so it only prints the program_stage when it is completed.
      check_to_enable_servo();
      if (servo_waiting == false) {
        show_program_state();
        program_state = 8;
        position_to_use = 2;
      }
      break;

    case 8:
      show_program_state();
      servo.write(servo_positions[position_to_use]);
      program_state = 9;
      break;

    case 9:
      show_program_state();
      check_LED_state(servo_positions[position_to_use]);
      program_state = 10;
      break;

    case 10:
      show_program_state();
      start_waiting_period(delay_times[position_to_use]);
      program_state = 11;
      break;

    case 11:
      // This stage gets checked a bunch of times while it is waiting to re-enable the servo,
      // so it only prints the program_stage when it is completed.
      check_to_enable_servo();
      if (servo_waiting == false) {
        show_program_state();
        program_state = 12;
        position_to_use = 3;
      }
      break;

    case 12:
      show_program_state();
      servo.write(servo_positions[position_to_use]);
      program_state = 13;
      break;

    case 13:
      show_program_state();
      check_LED_state(servo_positions[position_to_use]);
      program_state = 14;
      break;

    case 14:
      show_program_state();
      start_waiting_period(delay_times[position_to_use]);
      program_state = 15;
      break;

    case 15:
      // This stage gets checked a bunch of times while it is waiting to re-enable the servo,
      // so it only prints the program_stage when it is completed.
      check_to_enable_servo();
      if (servo_waiting == false) {
        show_program_state();
        program_state = 0; // This gets reset back to zero at the last step, for the program to repeat.
        //                    If you left this value at 17, then all the steps would only be done one time.
        position_to_use = 0; // This needs to be reset back to zero here also since the arrays only had 4 positions that start at zero.
      }
      break;

  }
}



void change_servo_position(int servo_position) {
  servo.write(servo_position);
}



void start_waiting_period(int time_to_wait) {
  // This function just begins a waiting time properly.
  // time_to_wait is the value of milliseconds that you want the waiting period to be.
  current_delay = time_to_wait;
  delay_begin_time = millis();
  servo_waiting = true;
}



void check_to_enable_servo() {
  // This changes servo_waiting back to false after the time stored in current_delay has passed.
  delay_current_time = millis();
  if (delay_current_time >= delay_begin_time) {
    if (delay_current_time >= (delay_begin_time + current_delay)) {
      servo_waiting = false;
    }
  }
  else {
    delay_begin_time = millis(); // This is a condition to reset the begin time, in case millis() had rolled over. After running for 53 days or so.
    // This is more of an issue with using micros() that rolls over in only about 14 hours, but I'm just showing it here anyway.
  }
}



void check_LED_state(int current_servo_position) {
  // This just turns the LED on and off when the servo position matches led_light_up_value.
  if (current_servo_position == led_light_up_value) {
    digitalWrite(LED_pin, HIGH);
  }
  else {
    digitalWrite(LED_pin, LOW);
  }
}


void show_program_state() {
  // This prints the current program state every time it is called.
  Serial.print("Executing program_state: ");
  Serial.println(program_state);
}


/*
  // This sketch does not use this function, as it just assigns values to position_to_use directly.
  void advance_position() {
  position_to_use += 1;
  if (position_to_use = 4) {
    position_to_use = 0; // Reset this when it gets too high
  }
  }
*/