// boom-gate simulation using an incredibly simple state machine
// Simon Shea
// 2011-08-28
// www.10dogs.net

#include <Servo.h>

// servo limits
const int LOWER_LIMIT = 875;  // horizontal
const int UPPER_LIMIT = 1855;  // vertical
const int INCREMENTS = 25000;  // approx. 2 second open / close time

// state enumeration
const int CLOSED = 0;
const int OPENING = 1;
const int OPEN = 2;
const int CLOSING = 3;

// input pins
const int OPEN_REQUEST_PIN = 2;
const int SAFETY_PIN = 3;

// output pins
const int OPEN_STATE_PIN = 4;
const int SERVO_CONTROL_PIN = 5;

// servo
Servo _servo;
int _position;

// variables
int _state;
volatile int _openRequest = false;
volatile int _safety = false;
int _openState = false;

void setup()
{
  // initial state
  _state = CLOSED;

  // setup pins
  pinMode(OPEN_REQUEST_PIN, INPUT);
  pinMode(SAFETY_PIN, INPUT);
  pinMode(OPEN_STATE_PIN, OUTPUT);
  pinMode(SERVO_CONTROL_PIN, OUTPUT);

  // setup servo
  _servo.attach(SERVO_CONTROL_PIN);
  _position = LOWER_LIMIT;
  _servo.write(_position);

  // interupts
  attachInterrupt(0, openRequest, CHANGE);
  attachInterrupt(1, safety, CHANGE);
}

void loop()
{
  digitalWrite(OPEN_STATE_PIN, _openState);
  
  // incredibly simple state machine
  // check for valid transitions from the current state
  switch(_state)
  {
    case OPENING:
      // current state: Opening
      // transition to: Open
      // guard: Position >= Upper Limit
      // action: none
      if (_position >= INCREMENTS)
      {
        _state = OPEN;
      }
      // current state: Opening
      // transition to: Closing
      // guard: Open Request = false  AND Safety = false
      // action: set Closed
      else if (_openRequest == false && _safety == false)
      {
        _state = CLOSING;
        _openState = false;
      }
      // open boom-gate
      else
      {
        // increment position and set servo
        setServoPosition(++_position);
      }   
      break;
  
    case OPEN:
      // current state: Open
      // transition to: Closing
      // guard: Open Request = false AND Safety = false
      // action: set Closed
      if (_openRequest == false && _safety == false)
      {
        _state = CLOSING;
        _openState = false;
      }
      break;
  
    case CLOSING:
      // current state: Closing
      // transition to: Closed
      // guard: Position <= Lower Limit
      // action: none
      if(_position <= 0)
      {
        _state = CLOSED;
      }
      // current state: Closing
      // transition to: Opening
      // guard: Open Request = true OR Safety = true
      // action: set Open
      else if(_openRequest == true || _safety == true)
      {
        _state = OPENING;
        _openState = true;
      }
      // internal state activity: close boom-gate
      else
      {
        // decrement position and set servo
        setServoPosition(--_position);
      }
      break;
  
    default:
      // current state: Closed
      // transition to: Opening
      // guard: Open Request = true
      // action: set Open
      if (_openRequest)
      {
        _state = OPENING;
        _openState = true;
      }
  }
}

// helper method to set the servo position
void setServoPosition(int pos)
{
  // map the timing scale to the limits of the servo travel and set the position
  _servo.writeMicroseconds(map(pos, 0, INCREMENTS, LOWER_LIMIT, UPPER_LIMIT));
}

// handle the open request interrupt
void openRequest()
{
  _openRequest = digitalRead(OPEN_REQUEST_PIN);
}

// handle the safety interrupt
void safety()
{
  _safety = digitalRead(SAFETY_PIN);
}