//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;
}
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;
}
int elev_init(void) {
int init_success = io_init();
if (!init_success){
return 0;
}
for (int f = 0; f < N_FLOORS; f++) {
for (elev_button_type_t b = 0; b < N_BUTTONS; b++){
elev_set_button_lamp(b, f, 0);
}
}
//drive down to valid floor
if (elev_get_floor_sensor_signal() == -1){
elev_set_motor_direction(DIRN_DOWN);
}
while (elev_get_floor_sensor_signal() == -1)
{
///Keep driving down until reached a floor
}
elev_set_motor_direction(DIRN_STOP);
elev_set_stop_lamp(0);
elev_set_door_open_lamp(0);
elev_set_floor_indicator(elev_get_floor_sensor_signal());
return 1;
}
void elev_goto_nearest_floor(){
int currFloor = elev_get_floor_sensor_signal();
while(currFloor == -1){
currFloor = elev_get_floor_sensor_signal();
elev_set_motor_direction(DIRN_UP);
}
printf("Floor found\n");
elev_set_motor_direction(0);
}
void elevator_init_pos(){
if(elev_get_floor_sensor_signal()==BETWEEN_FLOORS){
control_down();
while(elev_get_floor_sensor_signal()==BETWEEN_FLOORS)
; // wait
}
int floor = elev_get_floor_sensor_signal();
set_last_floor(floor);
control_setcurpos(floor);
control_stop();
}
int handle_event_boot() {
int next_state = IDLE;
int floor = elev_get_floor_sensor_signal();
printf("\nBOOT IN PROGRESS; PLEASE WAIT...\n");
while(floor == -1){
change_speed(STOP, S_DOWN);
floor = elev_get_floor_sensor_signal();
}
elev_set_floor_indicator(floor);
change_speed(S_DOWN, STOP);
printf("\nELEVATOR READY\n");
return next_state;
}
States fsm_going_up(system_state_t* system_state, States fsm_previous_state){
if (elev_get_stop_signal()){
return stop;
}
system_state->next_floor = system_state->current_floor + system_state->motor_direction;
elev_set_door_open_lamp(0);
elev_set_motor_direction(DIRN_UP);
check_cmd_buttons(system_state);
check_call_buttons(system_state);
check_floor(system_state);
if (elev_get_floor_sensor_signal() >= 0) {
elev_set_floor_indicator(system_state->current_floor);
if (should_stop(system_state, system_state->current_floor)){
elev_set_direction(system_state, 0);
orders_clearAt(system_state, system_state->current_floor);
reset_button_indicators(system_state);
return door_open;
}
}
return going_up;
}
void idleEntry()
{
elev_set_door_open_lamp(false);
elev_set_speed(0);
etasje = elev_get_floor_sensor_signal();
tilstand1 = IDLE;
}
void Driver::event_generator()
{
/* Poll for floor signal */
int floor_signal = elev_get_floor_sensor_signal();
poll(last_floor_signal, floor_signal, -1, FloorSignalEvent(floor_signal));
for (int floor = 0; floor < FLOORS; floor++) {
for (auto button_type : {ButtonType::UP, ButtonType::DOWN}) {
/* first floor doesn't have a down button and top floor doesn't have an up
button */
if ((floor == 0 && button_type == ButtonType::DOWN)
|| (floor == 3 && button_type == ButtonType::UP)) {
continue;
}
int btn_index = static_cast<int>(button_type);
int button_signal = elev_get_button_signal(static_cast<elev_button_type_t>(btn_index), floor);
poll(last_button_signals[floor][btn_index], button_signal, 0,
ExternalButtonEvent(floor, button_type));
}
/* Internal buttons: */
int button_signal = elev_get_button_signal(BUTTON_COMMAND, floor);
poll(last_button_signals[floor][2], button_signal, 0,
InternalButtonEvent(floor));
}
}
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;
}
int main()
{
// Initialize hardware
if (!elev_init()) {
printf(__FILE__ ": Unable to initialize elevator hardware\n");
return 1;
}
printf("Best Elevator Software, version: 2.27.14\n" );
initialize();
lastFloorReached = N_FLOORS+1;
while (1) {
queueIsEmpty = emptyQueue();
obstructionSignal = elev_get_obstruction_signal();
stoppedSignal = elev_get_stop_signal();
thisFloor = elev_get_floor_sensor_signal();
duration = clock()-start;
if ( timerActive && duration > seconds ) {
timerActive = 0;
duration = 0;
stateMachine( TIMEROUT );
}
if ( obstructionSignal != lastObstructionSignal ) {
if ( obstructionSignal == 1 )
stateMachine( OBSTRON );
if ( obstructionSignal == 0 )
stateMachine( OBSTROFF );
lastObstructionSignal = obstructionSignal;
}
if ( stoppedSignal != lastStoppedSignal ) {
if ( stoppedSignal == 1 )
stateMachine( STOP );
lastStoppedSignal = stoppedSignal;
}
if ( thisFloor != -1 && ( lastFloorReached != thisFloor ) ) {
if ( thisFloor-lastFloorReached < 0 )
direction = DOWN;
if ( thisFloor-lastFloorReached > 0 )
direction = UP;
lastFloorReached = thisFloor;
elev_set_floor_indicator( thisFloor );
}
if ( order() && directionToGo( thisFloor, getNextOrder( thisFloor ) ) == 0 )
stateMachine( NEWORDERCURRENTFLOOR );
if ( thisFloor != -1 && directionToGo( thisFloor , getNextOrder( thisFloor ) ) == 0 && !timerActive )
stateMachine( FLOORREACHED );
if( queueIsEmpty == 1 && order() )
stateMachine( NEWORDEREMPTYQUEUE );
}
return 0;
}
//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;
}
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;
}
int handle_event_emergency(int event, int order_table[N_FLOORS][N_BUTTONS]) {
int next_state = EMERGENCY;
switch (event) {
case EMERGENCY_BUTTON:
activate_emergency(order_table);
next_state = EMERGENCY;
break;
case GO_UP:
if(elev_get_floor_sensor_signal() != -1 && timer_elapsed() < DOOR_DELAY){ //Elevator will wait for # seconds defined by DOOR_DELAY before continuing when at a floor
return EMERGENCY;
}
deactivate_emergency();
change_speed(S_STOP, S_UP);
next_state = GOING_UP;
break;
case GO_DOWN:
if(elev_get_floor_sensor_signal() != -1 && timer_elapsed() < DOOR_DELAY){ //Elevator will wait for # seconds defined by DOOR_DELAY before continuing when at a floor
return EMERGENCY;
}
deactivate_emergency();
change_speed(S_STOP, S_DOWN);
next_state = GOING_DOWN;
break;
case STOP:
next_state = EMERGENCY;
break;
case OBSTRUCTION:
timer_set();
next_state = EMERGENCY;
break;
}
return next_state;
}
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;
}
int Driver::initialize_position()
{
const std::chrono::duration<double> wait_time = std::chrono::seconds(4);
for (elev_motor_direction_t dir : {DIRN_UP, DIRN_DOWN}) {
LOG_DEBUG("Trying to go " << (dir == DIRN_UP ? "up" : "down"));
elev_set_motor_direction(dir);
TimePoint start = std::chrono::system_clock::now();
while (std::chrono::system_clock::now() - start < wait_time) {
int floor_signal = elev_get_floor_sensor_signal();
if (floor_signal != -1) {
elev_set_motor_direction(DIRN_STOP);
return floor_signal;
}
}
}
return -1;
}
States fsm_init_state(system_state_t* system_state, States fsm_previous_state){
if (system_state->motor_direction == 0){
elev_set_direction(system_state, -1);
}
int floor = elev_get_floor_sensor_signal();
if (floor >= 0){
elev_set_floor_indicator(floor);
elev_set_direction(system_state, 0);
system_state->current_floor = floor;
elev_set_door_open_lamp(0);
return idle;
}
reset_active_floors(system_state);
reset_called_floors(system_state);
return init;
}
void elev_init(void) {
puts("ELEVATOR INIT CALLED");
int init_success = io_init();
assert(init_success && "Unable to initialize elevator hardware!");
elev_set_stop_lamp(0);
elev_set_door_open_lamp(0);
for (int f = 0; f < N_FLOORS; f++) {
for (elev_button_type_t b = 0; b < N_BUTTONS; b++){
elev_set_button_lamp(b, f, 0);
}
}
while(elev_get_floor_sensor_signal() != 0){
elev_set_motor_direction(DIRN_DOWN);
}
elev_set_motor_direction(DIRN_STOP);
elev_set_floor_indicator(0);
}
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;
}
States fsm_idle_state(system_state_t* system_state, States fsm_previous_state){
int floor = elev_get_floor_sensor_signal();
system_state->next_floor = system_state->prev_floor + system_state->motor_direction;
if (elev_get_stop_signal()){
return stop;
}
if (floor == 0){
if (elev_get_button_signal(BUTTON_COMMAND, floor) ||
elev_get_button_signal(BUTTON_CALL_UP, floor)){
return door_open;
}
}
else if (floor == 3){
if (elev_get_button_signal(BUTTON_COMMAND, floor) ||
elev_get_button_signal(BUTTON_CALL_DOWN, floor)){
return door_open;
}
}
else{
if (elev_get_button_signal(BUTTON_COMMAND, floor) ||
elev_get_button_signal(BUTTON_CALL_UP, floor) ||
elev_get_button_signal(BUTTON_CALL_DOWN, floor)){
return door_open;
}
}
check_cmd_buttons(system_state);
for (int i = 0; i < N_FLOORS; ++i){
if (system_state->elev_active_floors[i] == 1){
if (i < floor){
system_state->motor_direction = -1;
return going_down;
}
else if (i > floor){
system_state->motor_direction = 1;
return going_up;
}
}
}
check_call_buttons(system_state);
for (int i=0; i<N_FLOORS;++i){
if (system_state->elev_called_floors_down[i] || system_state->elev_called_floors_up[i]){
if (i < floor){
system_state->motor_direction = -1;
return going_down;
}
else if (i > floor){
system_state->motor_direction = 1;
return going_up;
}
}
}
return idle;
}
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;
}
}
//Makes elev check the floor sensors signals
int sm_check_floor_sensor_signal(void){
return elev_get_floor_sensor_signal();
}
void* goto_floor_with_network(int targetFloor){
// Baserer meg her på at heis befinner seg i etasje
//Try to reconnect
//If orders in orderlist
int currentFloor;
int directionIndex;
int flag = 0;
// Funksjoner som sjekker bestillinger kan implementeres som tråder og fjernes fra tilstandsmaskiner
printf("Searching for orders\n");
//While løkke kjører så lenge bestillingslister har elementer i seg
printf("Element in ButtonVector, internalButtonVector: %i externalButtonVector: %i\n", !check_if_internalButtonVector_is_empty(), !check_if_externalButtonVector_is_empty() );
print_button_vectors();
currentFloor = elev_get_floor_sensor_signal();
lastFloor = currentFloor;
printf("TargetFloor etter end_order_in_direction %i \n", targetFloor);
direction = get_direction(currentFloor, targetFloor);
directionIndex = direction_to_index(direction); // Til bruk når vi aksesser externalButtonVector
printf("Direction er %i directionIndex er %i\n", direction, directionIndex );
set_motor_direction(lastFloor, targetFloor);
time_t start, end;
double elapsed = 0;
time(&start);
while(currentFloor != targetFloor){ //Heis beveger seg mot targetFloor i gitt retning
if (elapsed > 5 ){
connectStatus = 0;
printf("Motor Disconnected\n");
while(!((currentFloor != -1) && (currentFloor != lastFloor) )){
elapsed = 0;
time(&start);
currentFloor = elev_get_floor_sensor_signal();
}
connectStatus = 1;
printf("Motor connected\n");
}
else{
connectStatus = 1;
time(&end);
}
elapsed = difftime(end, start);
if((currentFloor != -1) && (currentFloor != lastFloor) ){ //Elevator have reached new floor
time(&start);
lastFloor = currentFloor;
elev_set_floor_indicator(lastFloor);
if (internalButtonVector[currentFloor]){
pthread_mutex_lock(&elevLock);
internalButtonVector[currentFloor] = 0;
pthread_mutex_unlock(&elevLock);
elev_set_motor_direction(DIRN_STOP);
printf("Open door 1\n");
elev_set_button_lamp(BUTTON_COMMAND, currentFloor, 0);
open_door();
flag = 1;
}
if(externalButtonVector[currentFloor][directionIndex] ){
pthread_mutex_lock(&elevLock);
externalButtonVector[currentFloor][directionIndex] = 0;
send_order_handeled_confirmation(currentFloor, directionIndex);
pthread_mutex_unlock(&elevLock);
if (flag != 1){
elev_set_motor_direction(DIRN_STOP);
printf("Open door 1\n");
elev_set_button_lamp(BUTTON_COMMAND, currentFloor, 0);
open_door();
flag = 0;
}
}
}
set_motor_direction(lastFloor, targetFloor);
pthread_mutex_lock(&elevLock);
targetFloor = end_order_in_direction(direction, targetFloor); // Oppdaterer targetfloor under reise
pthread_mutex_unlock(&elevLock);
// printf("LastFloor er %i\n", lastFloor);
currentFloor = elev_get_floor_sensor_signal();
// sleep(0.2); //Forsinke prints
}
//Exit while -> framme ved targetFloor
if ( (currentFloor == targetFloor) ){
lastFloor = currentFloor;
elev_set_floor_indicator(lastFloor);
elev_set_motor_direction(DIRN_STOP);
printf("Open door 2, framme ved targetFloor %i\n", targetFloor);
open_door();
elev_set_button_lamp(BUTTON_COMMAND, targetFloor, 0);
pthread_mutex_lock(&elevLock);
internalButtonVector[targetFloor] = 0;
externalButtonVector[targetFloor][0] = 0; // I denne bestillingsslettingen kan det fort ligge bug
externalButtonVector[targetFloor][1] = 0;
pthread_mutex_unlock(&elevLock);
send_order_handeled_confirmation(targetFloor, 0);
send_order_handeled_confirmation(targetFloor, 1);
direction = 0;
}
// sleep(0.2);
return NULL;
}
void start_elevator(){
elev_init();
elev_goto_nearest_floor();
init_buttonVectors();
for (int i = 0; i < N_FLOORS; i++){
printf("%i\n", internalButtonVector[i]);
}
lastFloor = elev_get_floor_sensor_signal();
pthread_mutex_init(&fileLock, NULL);
pthread_mutex_lock(&fileLock);
printf("Trying to read internal orders and set internalButtonVector\n");
internalOrdersText = fopen("internal.txt", "r");
int character;
if (internalOrdersText != NULL){
for (int i = 0; i < N_FLOORS; i++){
character = fgetc(internalOrdersText);
if((character-48) == 1){
internalButtonVector[i] = 1;
}
else{
internalButtonVector[i] = 0;
}
printf("%i\n", internalButtonVector[i]);
elev_set_button_lamp(BUTTON_COMMAND, i, internalButtonVector[i]);
}
fclose(internalOrdersText);
}
else{
printf("Inititating internalOrdersText\n");
init_internalOrdersText();
}
pthread_mutex_unlock(&fileLock);
printf("Hva faen?\n");
init_socket(&broadCastSocket);
bind_socket(&broadCastSocket, BROADCAST_PORT, "255.255.255.255");
int reuseEnable = 1;
if (setsockopt(broadCastSocket, SOL_SOCKET, SO_REUSEADDR, &reuseEnable, sizeof(reuseEnable)) < 0){
error("setsockopt");
}
init_socket(&sendSock);
int broadcastEnable = 1;
if (setsockopt(sendSock, SOL_SOCKET, SO_BROADCAST, &broadcastEnable, sizeof(broadcastEnable))){
error("setsockopt");
}
pthread_create(&connectMasterThread, NULL, &connect_to_master, NULL);
pthread_join(connectMasterThread, NULL);
printf("First init finished, lytter på port: %i ip: %s\n", slp, masterIPAddr);
pthread_mutex_init(&internalOrdersLock, NULL);
pthread_create(&internalOrdersThread, NULL, &thread_check_internal_orders, NULL);
pthread_mutex_init(&externalOrdersLock, NULL);
pthread_create(&checkExternalOrderThread, NULL, &check_external_orders, NULL);
pthread_mutex_init(&receiveLock, NULL);
pthread_create(&receiveThread, NULL, &thread_udp_receive_from_master, NULL);
pthread_mutex_init(&sendLock, NULL);
pthread_create(&threadSendLocDir, NULL, &thread_send_loc_dir_master, NULL);
printf("Initialization complete\n");
sleep(1);
elev_slave_waiting_with_network();
pthread_join(internalOrdersThread, NULL);
pthread_join(receiveThread, NULL);
}
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;
}
void check_floor(system_state_t* system_state){
if (elev_get_floor_sensor_signal() >= 0){
system_state->current_floor = elev_get_floor_sensor_signal();
}
}
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;
}
}
