int main(){
// Initialize hardware
if (!elev_init()) {
printf(__FILE__ ": Unable to initialize elevator hardware\n");
return 1;
}
printf("Press STOP button to stop elevator and exit program.\n");
elev_set_speed(300);
while (1) {
if (elev_get_floor_sensor_signal() == 0){
elev_set_speed(300);
} else if (elev_get_floor_sensor_signal() == N_FLOORS-1){
elev_set_speed(-300);
}
if (elev_get_stop_signal()) {
elev_set_speed(0);
break;
}
}
return 0;
}
void bevegerEntry()
{
elev_set_door_open_lamp(false);
int neste = hentNesteOrdre(etasje, opp);
if(neste == etasje)
{
if(opp)
{
opp = false;
elev_set_speed(-300);
} else
{
opp = true;
elev_set_speed(300);
}
} else if(neste > etasje)
{
elev_set_speed(300);
opp = true;
} else
{
elev_set_speed(-300);
opp = false;
}
tilstand1 = BEVEGER;
}
void stop_elev(){
update_timer();
if (current_state == UP) {
elev_set_speed(-300);
usleep(5000);
elev_set_speed(0);
}
if (current_state == DOWN) {
elev_set_speed(300);
usleep(5000);
elev_set_speed(0);
}
}
void run_elevator(){
// Elevator is started and state is unknown.
// Initialize timer.
timer = clock();
current_state = UNDEF;
initialize_elevator();
// Elevator is now at some floor and ready for orders.
while(1){
// Handles io-signals, (sets/clears lights etc.)
io_signals();
// Calls the state machine switch function.
state_machine();
// If ... exit the main loop and terminate the program.
if (elev_get_button_signal(BUTTON_CALL_UP,0) && current_state == EMERGENCY){
elev_set_speed(0);
break;
}
}
initialize_elevator();
}
//else if????
current_state_t sm_idle(int queues[N_QUEUES][N_FLOORS], int previousState) {
elev_set_speed(0);
int currentFloor = ui_get_floor_indicator();
if(elev_get_floor_sensor_signal()>=0)
{
currentFloor = elev_get_floor_sensor_signal();
}
// legge ved commandkøen til denne ifen????????????
if(!queue_up_empty(queues, QUEUE_UP, currentFloor)||!queue_up_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_UP;
}
//samme her!!!!!!!!!!!!!!!!
if(!queue_down_empty(queues, QUEUE_UP, currentFloor)||!queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_DOWN;
}
if(!queue_down_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_DOWN;
}
if(!queue_up_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_UP;
}
//handels if none of the conditions above and still between floors -> undef
if(elev_get_floor_sensor_signal()==-1)
{
return STATE_UNDEF;
}
return STATE_IDLE;
}
void nodstopEntry()
{
elev_set_stop_lamp(true);
slettAlleOrdre();
elev_set_speed(0);
tilstand2 = NODSTOPP;
}
void idleEntry()
{
elev_set_door_open_lamp(false);
elev_set_speed(0);
etasje = elev_get_floor_sensor_signal();
tilstand1 = IDLE;
}
void dorerEntry()
{
elev_set_door_open_lamp(true);
elev_set_speed(0);
slettOrdre(etasje, opp);
startTimer();
tilstand1 = APNE_DORER;
}
//undefined floor state
current_state_t sm_undef(void){
if(elev_get_floor_sensor_signal()==-1)
{
//by default take the elevator to the floor below if between floors
elev_set_speed(-100);
return STATE_UNDEF;
}
return STATE_IDLE;
}
void elev_reset_all_lamps(void) {
elev_set_speed(0);
elev_set_stop_lamp(0);
elev_set_door_open_lamp(0);
int floor;
for (floor = 0; floor < N_FLOORS; ++floor) {
elev_set_button_lamp(ELEV_DIR_DOWN, floor, 0);
elev_set_button_lamp(ELEV_DIR_UP, floor, 0);
elev_set_button_lamp(ELEV_DIR_COMMAND, floor, 0);
}
}
sm_state_t sm_undefined(int queues[N_QUEUES][N_FLOORS]){
ui_set_door_open_lamp(0); //Closes the door after stop state
if(elev_get_floor_sensor_signal()==-1){
elev_set_speed(-100);
queue_clear_all_orders(queues);
return STATE_UNDEFINED;
}
return STATE_IDLE;
}
current_state_t sm_stop(int queues[N_QUEUES][N_FLOORS]) {
elev_set_speed(0);
ui_set_stop_lamp(1);
//this takes the elevator out of the stop state
if(queue_has_orders(queues) && !elev_get_stop_signal())
{
ui_set_stop_lamp(0);
return STATE_UNDEF;
}
queue_clear(queues);
return STATE_STOP;
}
void initialize_elevator(){
elev_set_speed(-300);
current_state = DOWN;
io_clear_all_lights();
while (elev_get_floor_sensor_signal() == -1)
; // Wait until first floor is reached.
stop_elev();
clear_all_orders();
current_state = STOP;
}
sm_state_t sm_idle(int queues[N_QUEUES][N_FLOORS], sm_state_t previousState) {
//Stops the elevator,
elev_set_speed(0);
//Sets currentFloor to the last floor seen.
int currentFloor = ui_get_floor_indicator();
//Sets currentFloor to the current floor it is at.
if(elev_get_floor_sensor_signal()>=0)
{
currentFloor = elev_get_floor_sensor_signal();
}
//Checks if there are any floors over the current floor and goes to move up state.
if(!queue_from_and_up_empty(queues, QUEUE_UP, currentFloor)||!queue_from_and_up_empty(queues, QUEUE_DOWN, currentFloor)){
return STATE_MOVE_UP;
}
//Checks if there are any floors under the current floor and goes to move down state.
else if(!queue_from_and_down_empty(queues, QUEUE_UP, currentFloor)||!queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor)){
return STATE_MOVE_DOWN;
}
//Command is put in its own else if for readability.
//Checks if there are any orders in the command under the current floor.
else if(!queue_from_and_down_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_MOVE_DOWN;
}
//Checks if there are any orders in the command over the current floor.
else if(!queue_from_and_up_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_MOVE_UP;
}
else if(elev_get_floor_sensor_signal() == -1)
{
return STATE_UNDEFINED; //If the elevator is not on a floor in idle the state is undefined.
}
else
{
return STATE_IDLE; //Nothing to do return to idle
}
}
//takes in the order matrix
current_state_t sm_up(int queues[N_QUEUES][N_FLOORS]) {
//trengs dette?????????????
int currentFloor = ui_get_floor_indicator();
if(elev_get_floor_sensor_signal()>=0)
{
currentFloor = elev_get_floor_sensor_signal();
}
elev_set_speed(100);
//checks if there are orders in the up- or (command- and up-) queue at curren floor
if(queues[QUEUE_UP][currentFloor] ||(queues[QUEUE_COMMAND][currentFloor] && queues[QUEUE_UP][currentFloor]))
{
return STATE_DOOR_OPEN;
}
//checks command queue and that the elevator is at an actual floor
else if(queues[QUEUE_COMMAND][currentFloor] && (elev_get_floor_sensor_signal() != -1))
{
return STATE_DOOR_OPEN;
}
//if command queue has order above current floor
else if(!queue_up_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_UP;
}
//if the top floor is ordered in the down queue and the up queue does not have orders above current floor
else if(queues[QUEUE_DOWN][N_FLOORS-1] == 1 && queue_up_empty(queues, QUEUE_UP, currentFloor))
{
//to make sure this conidtion only opens the door when the elevator is on the top floor
if(currentFloor != (N_FLOORS-1))
{
return STATE_UP;
}
return STATE_DOOR_OPEN;
}
//then down queue has order on current floor and there is no orders over the current floor in up queue
else if(queues[QUEUE_DOWN][currentFloor] && queue_up_empty(queues, QUEUE_UP, currentFloor))
{
return STATE_DOOR_OPEN;
}
//if it is order over current floor in up OR down queue
else if(!queue_up_empty(queues, QUEUE_UP, currentFloor)||!queue_up_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_UP;
}
//no more orders
return STATE_IDLE;
}
current_state_t sm_down(int queues[N_QUEUES][N_FLOORS]) {
int currentFloor=ui_get_floor_indicator();
if(elev_get_floor_sensor_signal() >= 0)
{
currentFloor = elev_get_floor_sensor_signal();
}
elev_set_speed(-100);
if(queues[QUEUE_DOWN][currentFloor]||((queues[QUEUE_COMMAND][currentFloor]) && queues[QUEUE_DOWN][currentFloor]))
{
return STATE_DOOR_OPEN;
}
else if(queues[QUEUE_COMMAND][currentFloor] && (elev_get_floor_sensor_signal() != -1))
{
return STATE_DOOR_OPEN;
}
else if(!queue_down_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_DOWN;
}
else if(queues[QUEUE_UP][0] == 1 && queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
if(currentFloor != 0)
{
return STATE_DOWN;
}
return STATE_DOOR_OPEN;
}
else if(queues[QUEUE_UP][currentFloor] && queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_DOOR_OPEN;
}
else if(!queue_down_empty(queues, QUEUE_UP, currentFloor) || !queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_DOWN;
}
//finished orders
return STATE_IDLE;
}
sm_state_t sm_stop(int queues[N_QUEUES][N_FLOORS]) {
elev_set_speed(0); //Stops the elevator
ui_set_stop_lamp(1); //Light the stop light
//Checks if command queues has order if not return stop state.
if(queue_command_has_orders(queues) && !ui_get_stop_signal())
{
ui_set_stop_lamp(0);//Turns of the stop light
ui_set_door_open_lamp(0);
return STATE_UNDEFINED;
}
else
{
if(elev_get_floor_sensor_signal() >= 0)
{
ui_set_door_open_lamp(1);
}
queue_clear_all_orders(queues); //Clear all the queue orders.
return STATE_STOP;
}
}
//1 is up and 0 is down
current_state_t sm_door_open(int queues[N_QUEUES][N_FLOORS], int previousState) {
elev_set_speed(0);
int timer = 0;
int currentFloor = elev_get_floor_sensor_signal();
ui_set_door_open_lamp(1);
startTimer();
//bool
while(timer != 1)
{
if(elev_get_stop_signal())
{
return STATE_STOP;
}
//listens for button signals, to take stop and order while the door is open
ui_button_signals(queues);
timer = checkTimer(3);
}
while(elev_get_obstruction_signal() != 0)
{
if(elev_get_stop_signal())
{
ui_set_door_open_lamp(0);
return STATE_STOP;
}
ui_set_door_open_lamp(1);
}
ui_set_door_open_lamp(0);
if (previousState == STATE_UP)
{
queues[QUEUE_COMMAND][currentFloor] = 0;
//if on the top floor
if(currentFloor == (N_FLOORS-1))
{
queues[QUEUE_DOWN][currentFloor] = 0;
}
//if orders over current in up queue
else if(!queue_up_empty(queues, QUEUE_UP, currentFloor))
{
queues[QUEUE_UP][currentFloor] = 0;
}
//if no orders over current in up queue AND orders below in down queue
else if(queue_up_empty(queues, QUEUE_UP, currentFloor) && !queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
queues[QUEUE_DOWN][currentFloor] = 0;
return STATE_DOWN;
}
return STATE_UP;
}
else if(previousState == STATE_DOWN)
{
queues[QUEUE_COMMAND][currentFloor] = 0;
//if on bottom floor
if(currentFloor == 0)
{
queues[QUEUE_UP][currentFloor] = 0;
//return IDLE??????????????????????????????
}
//if orders below in down queue
else if(!queue_down_empty(queues, QUEUE_DOWN, currentFloor))
{
queues[QUEUE_DOWN][currentFloor] = 0;
}
//if no orders below in down queue and orders over in up queue
else if(queue_down_empty(queues, QUEUE_DOWN, currentFloor) && !queue_up_empty(queues, QUEUE_UP, currentFloor))
{
queues[QUEUE_UP][currentFloor] = 0;
return STATE_UP;
}
return STATE_DOWN;
}
return STATE_IDLE;
}
void obstruksjonEntry()
{
elev_set_stop_lamp(false);
elev_set_speed(0);
tilstand2 = OBSTRUKSJON;
}
void initHeisstyring()
{
elev_set_speed(100);
}
int main()
{
if (!elev_init()) {
printf(__FILE__ ": Unable to initialize elevator hardware\n");
return 1;
}
slettAlleOrdre();
elev_set_speed(0);
initHeisstyring();
int forgjeEtasje = 0;
int etasje;
bool forgjeStopp;
bool stopp;
bool forgjeObstruksjon;
bool obstruksjon;
bool forgjeOpp[4];
bool opp[4];
bool forgjeInne[4];
bool inne[4];
bool forgjeNed[4];
bool ned[4];
while(1)
{
stopp = elev_get_stop_signal();
obstruksjon = elev_get_obstruction_signal();
etasje = elev_get_floor_sensor_signal();
for(int i = 0; i < N_FLOORS; i++) // poller masse, fix
{
if(i != N_FLOORS-1)
opp[i] = elev_get_button_signal(BUTTON_CALL_UP, i);
if(i != 0)
ned[i] = elev_get_button_signal(BUTTON_CALL_DOWN, i);
inne[i] = elev_get_button_signal(BUTTON_COMMAND, i);
}
for(int i = 0; i < N_FLOORS; i++)
{
if(opp[i] && !forgjeOpp[i])
leggTilOrdre(i, false, true);
if(ned[i] && !forgjeNed[i])
leggTilOrdre(i, false, false);
if(inne[i] && !forgjeInne[i])
leggTilOrdre(i, true, true);
forgjeOpp[i] = opp[i];
forgjeNed[i] = ned[i];
forgjeInne[i] = inne[i];
}
if(harTimetUt())
timeOut();
if (etasje != -1 && etasje != forgjeEtasje)
{
elev_set_floor_indicator(etasje);
etasjeAnkommet(etasje);
forgjeEtasje = etasje;
}
if(obstruksjon && !forgjeObstruksjon)
{
forgjeObstruksjon = obstruksjon;
forgjeEtasje = -1;
if(obstruksjon)
obstruksjonPa();
else
obstruksjonAv();
}
if(stopp && !forgjeStopp)
{
forgjeStopp = stopp;
forgjeEtasje = -1;
if(stopp)
nodStopp();
}
}
return 0;
}
sm_state_t sm_move_down(int queues[N_QUEUES][N_FLOORS]){
//Checks if the door is forced open if it is stop the elevator and light open door and stops the elevator.
if(elev_get_obstruction_signal())
{
return STATE_OPEN_DOOR;
}
//Sets currentFloor to the last floor seen.
int currentFloor = ui_get_floor_indicator();
//Sets currentFloor to the current floor it is at.
if(elev_get_floor_sensor_signal()>=0)
{
currentFloor = elev_get_floor_sensor_signal();
}
//Sets the motor speed to 100 downwards.
elev_set_speed(-100);
//Checks if there are orders in the down- or (command- and down-) queue at current floor.
if((queues[QUEUE_DOWN][currentFloor]||((queues[QUEUE_COMMAND][currentFloor]) && queues[QUEUE_DOWN][currentFloor])))
{
if(elev_get_floor_sensor_signal() != -1) //Checks if it is at a floor.
{
return STATE_OPEN_DOOR;
}
else
{
return STATE_MOVE_DOWN;
}
}
//Checks if the command queue has an order at current floor and that the elevator is at an actual floor.
else if(queues[QUEUE_COMMAND][currentFloor] && (elev_get_floor_sensor_signal() != -1))
{
return STATE_OPEN_DOOR;
}
//Checks if the down queue has any orders bellow current floor.
else if(!queue_from_and_down_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_MOVE_DOWN;
}
//Checks if the bottom floor is ordered in the up queue and the down queue does not have orders bellow the current floor.
else if(queues[QUEUE_UP][0] == 1 && queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor))
{
if(currentFloor != 0)
{
return STATE_MOVE_DOWN;
}
else
{
return STATE_OPEN_DOOR;
}
}
//Checks if the up queue has order on current floor and there is no orders over the current floor in the down queue.
else if(queues[QUEUE_UP][currentFloor] && queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor))
{
if(elev_get_floor_sensor_signal() != -1) //Checks if it is at a floor.
{
return STATE_OPEN_DOOR;
}
else
{
return STATE_MOVE_DOWN;
}
}
//Checks if it is order bellow current floor in up or down queue.
else if(!queue_from_and_down_empty(queues, QUEUE_UP, currentFloor) || !queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_MOVE_DOWN;
}
//Finished all downwards orders.
else
{
return STATE_IDLE;
}
}
sm_state_t sm_move_up(int queues[N_QUEUES][N_FLOORS]){
//Checks if the door is forced open if it is stop the elevator and light open door.
if(elev_get_obstruction_signal())
{
return STATE_OPEN_DOOR;
}
//Sets currentFloor to the last floor seen.
int currentFloor = ui_get_floor_indicator();
//Sets currentFloor to the current floor it is at.
if(elev_get_floor_sensor_signal()>=0)
{
currentFloor = elev_get_floor_sensor_signal();
}
//Sets the motor speed to 100 upwards.
elev_set_speed(100);
//Checks if there are orders in the up- or (command- and up-) queue at current floor.
if(queues[QUEUE_UP][currentFloor] ||(queues[QUEUE_COMMAND][currentFloor] && queues[QUEUE_UP][currentFloor]))
{
if(elev_get_floor_sensor_signal() != -1) //Checks if it is at a floor.
{
return STATE_OPEN_DOOR;
}
else
{
return STATE_MOVE_UP;
}
}
//Checks if the command queue has an order at current floor and that the elevator is at an actual floor.
else if(queues[QUEUE_COMMAND][currentFloor] && (elev_get_floor_sensor_signal() != -1))
{
return STATE_OPEN_DOOR;
}
//Checks if the command queue has order above the current floor
else if(!queue_from_and_up_empty(queues, QUEUE_COMMAND, currentFloor))
{
return STATE_MOVE_UP;
}
//Checks if the top floor is ordered in the down queue and the up queue does not have orders above the current floor
else if(queues[QUEUE_DOWN][N_FLOORS-1] == 1 && queue_from_and_up_empty(queues, QUEUE_UP, currentFloor))
{
//To make sure this conidtion only opens the door when the elevator is on the top floor.
if(currentFloor != N_FLOORS-1)
{
return STATE_MOVE_UP;
}
else
{
return STATE_OPEN_DOOR;
}
}
//Checks if the down queue has order on current floor and there is no orders over the current floor in up queue.
else if(queues[QUEUE_DOWN][currentFloor] && queue_from_and_up_empty(queues, QUEUE_UP, currentFloor))
{
if(elev_get_floor_sensor_signal() != -1) //Checks if it is at a floor.
{
return STATE_OPEN_DOOR;
}
else
{
return STATE_MOVE_UP;
}
}
//Checks if it is order over current floor in up or down queue.
else if(!queue_from_and_up_empty(queues, QUEUE_UP, currentFloor)||!queue_from_and_up_empty(queues, QUEUE_DOWN, currentFloor))
{
return STATE_MOVE_UP;
}
//Finished all upwards orders.
else
return STATE_IDLE;
}
sm_state_t sm_open_door(int queues[N_QUEUES][N_FLOORS],sm_state_t previousState){
elev_set_speed(0); //Stops the elevator
int currentFloor = elev_get_floor_sensor_signal(); //Gets the current floor the elevator.
// If the elevator has an obstruction between floors stop the elevator
while(elev_get_obstruction_signal() && currentFloor == -1)
{
return previousState;
}
if(currentFloor == -1)
{
return previousState;
}
ui_set_door_open_lamp(1); //Open doors
startTimer();
while(checkTimer(3) != 1){
if(elev_get_floor_sensor_signal() == -1)
{
break; //return previousState? both possible depends on wanted behaviour
}
else if(ui_get_stop_signal()) //Checks stop button for input while the door is open.
{
return STATE_STOP;
}
ui_button_signals(queues); //Checks for button input while the door is open.
}
while(elev_get_obstruction_signal() != 0){
ui_button_signals(queues); //Checks for button input while the obstruction is active.
if(ui_get_stop_signal()) //Checks stop button for input when obstructed.
{
return STATE_STOP;
}
ui_set_door_open_lamp(1); //Sets door lamp to Open.
}
ui_set_door_open_lamp(0); //Sets door lamp to closed.
//If it came from the state of moving up the it checks the following cases.
if (previousState == STATE_MOVE_UP)
{
queues[QUEUE_COMMAND][currentFloor] = 0; //Reached ordered command floor and clears queue element.
//If the top floor is reached it switches state to check if there is any orders bellow.
if(currentFloor == N_FLOORS-1)
{
queues[QUEUE_DOWN][currentFloor] = 0; //Clear ordered floor in down queue.
return STATE_MOVE_DOWN;
}
//Reached queue element in up that is ordered and clears it.
else if(!queue_from_and_up_empty(queues, QUEUE_UP, currentFloor))
{
queues[QUEUE_UP][currentFloor] = 0;
}
//If no orders over current in up queue AND orders below in down queue.
else if(queue_from_and_up_empty(queues, QUEUE_UP, currentFloor) && !queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor))
{
queues[QUEUE_DOWN][currentFloor] = 0;
return STATE_MOVE_DOWN;
}
//More orders in up queue so returns to its previous state and continoues.
return STATE_MOVE_UP;
}
//If it came from the state of moving down the it checks the following cases.
else if(previousState == STATE_MOVE_DOWN)
{
queues[QUEUE_COMMAND][currentFloor] = 0; //Reached ordered command floor and clears queue element
//If the bottom floor is reached it switches state to check if there is any orders abow.
if(currentFloor == 0)
{
queues[QUEUE_UP][currentFloor] = 0;
return STATE_MOVE_UP;
}
//Reached queue element in down that is ordered and clears it.
else if(!queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor))
{
queues[QUEUE_DOWN][currentFloor] = 0;
}
//If no orders below in down queue AND orders over in up queue.
else if(queue_from_and_down_empty(queues, QUEUE_DOWN, currentFloor) && !queue_from_and_up_empty(queues, QUEUE_UP, currentFloor))
{
queues[QUEUE_UP][currentFloor] = 0;
return STATE_MOVE_UP;
}
//More orders in down queue so returns to its previous state and continoues.
return STATE_MOVE_DOWN;
}
//Returns to previous state
else if(previousState == STATE_MOVE_UP)
{
return STATE_MOVE_UP;
}
else
{
return STATE_MOVE_DOWN;
}
}
