Door Procedure

door_procedure.cpp

#include "config.h"
#include "classes.h"
#include <iostream>
using namespace std;

int door_procedure(emc::LaserData laser){
count_doorpr += 1;

if (count_doorpr < 2){              //ring  bell once
    io.sendOpendoorRequest();
    cout<<"TRIIIINGNNGNGNGG"<<endl;
    return 0;
}

if (count_doorpr > 5SEC){            //after 5 seconds
    for(int i=FRONT_START; i<FRONT_END; i=i+1){   //For laser points in front
        if (laser.ranges[i]<VIEW_DIST && laser.ranges[i]>MIN_DIST && laser.ranges[i+1]<VIEW_DIST && laser.ranges[i+1]>MIN_DIST &&laser.ranges[i+2]<VIEW_DIST && laser.ranges[i+2]>MIN_DIST){
            return 1;               // dead end
        }
        else {
            return 2;               // door has opened: continue
        }
    }

}
return 0;
}