int he_lvl[];
int exp_lvl[];
int s_lvl[];
int door_state[];

s_lvl = {1, 5};
he_lvl = { 2, 3, 4};

void setup() {
  // put your setup code here, to run once:
  // when floor button is selected, push that
}

void loop() {
  // put your main code here, to run repeatedly:
  state_stop_closed () {
    //you want to have a door closed and door open
    //door closed will be negated in this state
  }
  state_stope_open () {
    //door closed and open state
    //door open will be negated

  }
}

#define SWITCH1 11
#define SWITCH2 12
#define SWITCH3 3
#define SWITCH4 4
#define SWITCH5 5
#define LED1 6
#define LED2 7
#define LED3 8
#define LED4 9
#define LED5 10
#define TOPFLOOR 5
#define IDLELIMIT 10
#define INTERVAL 2000L

enum direction { UP, DOWN, STOPPED }; // this is our main STATE enumeration

unsigned long previousMillis = 0;
short currentFloor=0; // computers start counting at zero, unlike humans
short floorSelected[TOPFLOOR]={0,0,0,0,0}; // array of buttons, initalized to not pressed
enum direction elevatorDirection=STOPPED; // this is our STATE variable
short moved;
unsigned int idleCount=0;

void setup(){
Serial.begin(115200);
pinMode(SWITCH1, INPUT); // set pin to input
digitalWrite(SWITCH1, HIGH); // turn on pullup resistor
pinMode(SWITCH2, INPUT); // set pin to input
digitalWrite(SWITCH2, HIGH); // turn on pullup resistor
pinMode(SWITCH3, INPUT); // set pin to input
digitalWrite(SWITCH3, HIGH); // turn on pullup resistor
pinMode(SWITCH4, INPUT); // set pin to input
digitalWrite(SWITCH4, HIGH); // turn on pullup resistor
pinMode(SWITCH5, INPUT); // set pin to input
digitalWrite(SWITCH5, HIGH); // turn on pullup resistor

pinMode(LED1, OUTPUT); // set pin to output
digitalWrite(LED1, LOW); // turn LED off
pinMode(LED2, OUTPUT); // set pin to output
digitalWrite(LED2, LOW); // turn LED off
pinMode(LED3, OUTPUT); // set pin to output
digitalWrite(LED3, LOW); // turn LED off
pinMode(LED4, OUTPUT); // set pin to output
digitalWrite(LED4, LOW); // turn LED off
pinMode(LED5, OUTPUT); // set pin to output
digitalWrite(LED5, LOW); // turn LED off
setLED(0);
}

void printEnum(enum direction elevatorDirection){
switch(elevatorDirection){
case UP :
Serial.print(“UP”);
break;
case DOWN :
Serial.print(“DOWN”);
break;
case STOPPED :
Serial.print(“STOPPED”);
break;
default :
Serial.print(“Out of range!”);
}
}

void setLED(short currentFloor){
Serial.print(“3 setting LED:”);
Serial.println(itoa(currentFloor+1,(char*)” “,10));
digitalWrite(LED1,(currentFloor==0)?HIGH:LOW); // set LED on or off if currentFloor
digitalWrite(LED2,(currentFloor==1)?HIGH:LOW); // set LED on or off if currentFloor
digitalWrite(LED3,(currentFloor==2)?HIGH:LOW); // set LED on or off if currentFloor
digitalWrite(LED4,(currentFloor==3)?HIGH:LOW); // set LED on or off if currentFloor
digitalWrite(LED5,(currentFloor==4)?HIGH:LOW); // set LED on or off if currentFloor
}

void openDoors(short currentFloor){
unsigned long currentMillis;
Serial.print(“5 Open doors at floor:”);
Serial.println(itoa(currentFloor+1,(char*)” “,10));
do {
currentMillis = millis();
delay(INTERVAL/10);
setLED(-1);
delay(INTERVAL/10);
setLED(currentFloor);
} while(currentMillis – previousMillis = INTERVAL) { // is it time to move?
previousMillis = currentMillis; // save the time to have it for the next cycle
moved=0; // we haven’t moved during this cycle yet
switch(elevatorDirection){
case UP : // we’re moving upwards
if(currentFloor=0; i–){ // for each floor below us
if(floorSelected[i]){ // check if selected
setLED(–currentFloor); // move down one and set led for that floor
moved=1; // we’ve not stopped yet
break; // only move one floor per cycle
}
}
if(floorSelected[currentFloor]){
openDoors(currentFloor);
floorSelected[currentFloor]=0; // we’ve arrived, clear the button
}
}
break;
case STOPPED :
for(short i=0; icurrentFloor?UP:DOWN); //head towards that floor
moved=1; // avoid stopping before we even moved
break;
}
}
}
break;
}
if(!moved) elevatorDirection=STOPPED;
else idleCount=0;
if((elevatorDirection==STOPPED) && (currentFloor != 0)) idleCount++; // if stopped, count seconds
if(idleCount>IDLELIMIT) { // once over the preset limit
floorSelected[0]=1; // return to the ground floor
idleCount=0;
}
Serial.print(“2 elevatorDirection:”);
printEnum(elevatorDirection);
Serial.print(” currentFloor:”);
Serial.print(itoa(currentFloor+1,(char*)” “,10));
Serial.print(” buttons:”);
Serial.print(itoa(floorSelected[0],(char*)” “,10));
Serial.print(itoa(floorSelected[1],(char*)” “,10));
Serial.print(itoa(floorSelected[2],(char*)” “,10));
Serial.print(itoa(floorSelected[3],(char*)” “,10));
Serial.print(itoa(floorSelected[4],(char*)” “,10));
Serial.print(” idleCount:”);
Serial.println(idleCount);
}
if(!digitalRead(SWITCH1)){ if(floorSelected[0]==0) Serial.println(“SWITCH1 pressed”); floorSelected[0]=1; } // read the buttons and adjust the floor if needed
if(!digitalRead(SWITCH2)){ if(floorSelected[1]==0) Serial.println(“SWITCH2 pressed”); floorSelected[1]=1; }
if(!digitalRead(SWITCH3)){ if(floorSelected[2]==0) Serial.println(“SWITCH3 pressed”); floorSelected[2]=1; }
if(!digitalRead(SWITCH4)){ if(floorSelected[3]==0) Serial.println(“SWITCH4 pressed”); floorSelected[3]=1; }
if(!digitalRead(SWITCH5)){ if(floorSelected[4]==0) Serial.println(“SWITCH5 pressed”); floorSelected[4]=1; }
}

enum State_enum {STOP, FORWARD, ROTATE_RIGHT, ROTATE_LEFT};
enum Sensors_enum {NONE, SENSOR_RIGHT, SENSOR_LEFT, BOTH};

void state_machine_run(uint8_t sensors);
void motors_stop();
void motors_forward();
void motors_right();
void motors_left();
uint8_t read_IR();

uint8_t state = STOP;

void setup(){
}

void loop(){
  state_machine_run(read_IR());

  delay(10);
}

void state_machine_run(uint8_t sensors)
{
  switch(state)
  {
    case STOP:
      if(sensors == NONE){
        motors_forward();
        state = FORWARD;
      }
      else if(sensors == SENSOR_RIGHT){
        motors_left();
        state = ROTATE_LEFT;
      }
      else{
        motors_right();
        state = ROTATE_RIGHT;
      }
      break;

    case FORWARD:
      if(sensors != NONE){
        motors_stop();
        state = STOP;
      }
      break;

    case ROTATE_RIGHT:
      if(sensors == NONE || sensors == SENSOR_RIGHT){
        motors_stop();
        state = STOP;
      }
      break;

    case ROTATE_LEFT:
      if(sensors != SENSOR_RIGHT)
      {
        motors_stop();
        state = STOP;
      }
      break;
  }
}

void motors_stop()
{
  //code for stopping motors
}

void motors_forward()
{
  //code for driving forward
}

void motors_right()
{
  //code for turning right
}

void motors_left()
{
  //code for turning left
}

uint8_t read_IR()
{
  //code for reading both sensors
}