void open_door(){ // printf("En kjøring av open_door\n"); elev_set_door_open_lamp(1); time_t start, end; double elapsed = 0; time(&start); while(elapsed < 2){ time(&end); elapsed = difftime(end, start); } elev_set_door_open_lamp(0); }
States fsm_door_open(system_state_t* system_state, States fsm_previous_state){ elev_set_direction(system_state, 0); elev_set_door_open_lamp(1); if (!fsm_pause_3_sec(system_state)) return stop; elev_set_door_open_lamp(0); check_floor(system_state); if (fsm_previous_state == idle){ elev_set_button_lamp (BUTTON_COMMAND, system_state->current_floor,0); orders_clearAt(system_state, system_state->current_floor); if (system_state->current_floor < 3) elev_set_button_lamp (BUTTON_CALL_UP, system_state->current_floor,0); if (system_state->current_floor > 0) elev_set_button_lamp (BUTTON_CALL_DOWN, system_state->current_floor,0); return idle; } else if (fsm_previous_state == going_down) { for (int i = system_state->current_floor; i >= 0; --i){ if (system_state->elev_active_floors[i] || system_state->elev_called_floors_up[i] || system_state->elev_called_floors_down[i]){ system_state->motor_direction = -1; return going_down; } } return idle; } else if (fsm_previous_state == going_up){ for (int i = system_state->current_floor; i < N_FLOORS; ++i){ if (system_state->elev_active_floors[i] || system_state->elev_called_floors_up[i] || system_state->elev_called_floors_down[i]){ system_state->motor_direction = 1; return going_up; } } } return idle; }
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; }
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; }
int elev_init(void) { int i; // Init hardware if (!io_init()) return 0; // Zero all floor button lamps for (i = 0; i < N_FLOORS; ++i) { if (i != 0) elev_set_button_lamp(BUTTON_CALL_DOWN, i, 0); if (i != N_FLOORS - 1) elev_set_button_lamp(BUTTON_CALL_UP, i, 0); elev_set_button_lamp(BUTTON_COMMAND, i, 0); } // Clear stop lamp, door open lamp, and set floor indicator to ground floor. elev_set_stop_lamp(0); elev_set_door_open_lamp(0); elev_set_floor_indicator(0); // Return success. return 1; }
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 idleEntry() { elev_set_door_open_lamp(false); elev_set_speed(0); etasje = elev_get_floor_sensor_signal(); tilstand1 = IDLE; }
/* From DOOR_CLOSED, go to DOOR_OPENED */ static void action_closed_open (void) { elev_set_door_open_lamp(1); door_last_state = door_current_state; door_current_state = DOOR_OPENED; }
void dorerEntry() { elev_set_door_open_lamp(true); elev_set_speed(0); slettOrdre(etasje, opp); startTimer(); tilstand1 = APNE_DORER; }
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); } }
/* From DOOR_OPENED, go to DOOR_CLOSED */ static void action_opened_close (void) { if (elev_get_obstruction_signal()) { door_set_timer(); return; } elev_set_door_open_lamp(0); door_last_state = door_current_state; door_current_state = DOOR_CLOSED; }
void elev_init(void) { int init_success = io_init(); assert(init_success && "Unable to initialize elevator hardware!"); 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); } } elev_set_stop_lamp(0); elev_set_door_open_lamp(0); elev_set_floor_indicator(0); }
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; }
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); } } elev_set_stop_lamp(0); elev_set_door_open_lamp(0); elev_set_floor_indicator(0); return 1; }
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); }
void elevator_thread(){ int sum=0; int stop=0; Message message_send; elevator.queue[0] = 0; elevator.queue[1] = 0; elevator.queue[2] = 0; elevator.queue[3] = 0; FILE *fp; char buff[255]; fp = fopen("local_order_queue.txt", "a+"); for(int i=0;i<N_FLOORS;i++){ fscanf(fp, "%s", buff); printf("%d : %s\n", i, buff ); if(buff[0] == '1'){ elevator.queue[i]=1; sum++; } else elevator.queue[i]=0; } fclose(fp); if(!sum){ elev_init(); elevator.floor_current = 0; elevator.direction=DIRN_STOP; } message_send.elevator.direction = elevator.direction; message_send.elevator.floor_current = elevator.floor_current; char transmit[BUFSIZE]; serialization(ELEV_UPDATE, message_send, transmit); pthread_t message_send_thread; pthread_create(&message_send_thread, NULL , tcp_send, (void*)transmit); int floor_update_not_sent=1; while(1){ if(sum != 0){ elevator.floor_current = elev_get_floor_sensor_signal(); int orders_up=0; int orders_down=0; if(elevator.direction == DIRN_STOP){ for(int i=elevator.floor_current+1;i<N_FLOORS;i++){ if(elevator.queue[i] != 0){ orders_up++; puts("Order up"); } } for( int i=elevator.floor_current-1;i>=0;i--){ if(elevator.queue[i] != 0){ orders_down++; puts("Order down"); } } if (orders_up == 0 && orders_down == 0){ elevator.direction=DIRN_STOP; } else if(orders_up>orders_down){ elevator.direction=DIRN_UP; puts("DIRN UP"); } else{ elevator.direction=DIRN_DOWN; puts("DIRN DOWN"); } } for(int i=0;i<N_FLOORS;i++){ message_send.elevator.queue[i]=elevator.queue[i]; } message_send.elevator.direction = elevator.direction; message_send.elevator.floor_current = elevator.floor_current; char transmit[BUFSIZE]; serialization(ELEV_UPDATE, message_send, transmit); pthread_t message_send_thread; pthread_create(&message_send_thread, NULL , tcp_send, (void*)transmit); elev_set_motor_direction(elevator.direction); } while(sum){ elevator.floor_current=elev_get_floor_sensor_signal(); if(elevator.floor_current!=-1){ elev_set_floor_indicator(elevator.floor_current); if((elevator.floor_current==0 && elevator.direction==DIRN_DOWN) || (elevator.floor_current==N_FLOORS&&elevator.direction==DIRN_UP)){ elevator.direction=DIRN_STOP; elev_set_motor_direction(elevator.direction); stop=1; } if(elevator.queue[elevator.floor_current]==1){ stop=1; } else if(elevator.queue[elevator.floor_current]==2){ sum=0; if(elevator.direction==DIRN_UP){ for(int i=elevator.floor_current;i<N_FLOORS;i++){ if(elevator.queue[i]==1){ sum++; } } switch (elevator.floor_current){ case 1: if(elev_orders[elevator.floor_current][1]==my_ID){ stop=1; break; } case 2: if(elev_orders[elevator.floor_current][1]==my_ID){ stop=1; break; } default: if(sum==0){ stop=1; } } } else if(elevator.direction==DIRN_DOWN){ for(int i=elevator.floor_current;i>=0;i--){ if(elevator.queue[i]==1){ sum++; } } switch (elevator.floor_current){ case 1: if(elev_orders[elevator.floor_current+2][1]==my_ID){ stop=1; break; } case 2: if(elev_orders[elevator.floor_current+2][1]==my_ID){ stop=1; break; } default: if(sum==0){ stop=1; } } } } if(stop){ stop=0; elev_set_motor_direction(DIRN_STOP); elev_set_button_lamp(BUTTON_INSIDE,elevator.floor_current,0); elevator.queue[elevator.floor_current]=0; printf("Elevator queue updated: \t"); sum = 0; for(int i=0;i<N_FLOORS;i++){ message_send.elevator.queue[i]=elevator.queue[i]; sum+=elevator.queue[i]; printf("%d ", elevator.queue[i]); } FILE *fp; fp = fopen("local_order_queue.txt", "w+"); fprintf(fp,"%d %d %d %d", elevator.queue[0], elevator.queue[1], elevator.queue[2], elevator.queue[3] ); fclose(fp); message_send.elevator.prev_direction = elevator.direction; elevator.direction = DIRN_STOP; message_send.elevator.direction = elevator.direction; message_send.elevator.floor_current = elevator.floor_current; printf("Floor current: %d\n", message_send.elevator.floor_current); for(int i = 0;i<6;i++){ message_send.orders[i][0] = elev_orders[i][0]; message_send.orders[i][1] = elev_orders[i][1]; } switch (elevator.floor_current){ case 0: message_send.orders[0][0] = 0; message_send.orders[0][1] = my_ID; elevator.next_direction = DIRN_UP; break; case 1: if(elev_orders[1][1] == my_ID){ message_send.orders[1][0] = 0; message_send.orders[1][1] = my_ID; elevator.next_direction = DIRN_UP; } else { message_send.orders[3][0] = 0; message_send.orders[3][1] = my_ID; elevator.next_direction = DIRN_DOWN; } break; case 2: if(elev_orders[2][1] == my_ID){ message_send.orders[2][0] = 0; message_send.orders[2][1] = my_ID; elevator.next_direction = DIRN_UP; } else { message_send.orders[4][0] = 0; message_send.orders[4][1] = my_ID; elevator.next_direction = DIRN_DOWN; } break; case 3: message_send.orders[5][0] = 0; message_send.orders[5][1] = my_ID; elevator.next_direction = DIRN_DOWN; break; } puts("ORDER UPDATE"); for(int i = 0;i<6;i++){ printf("Order: %d \t ID: %d\n",message_send.orders[i][0], message_send.orders[i][1]); } char transmit[BUFSIZE]; serialization(ORDER_UPDATE, message_send, transmit); pthread_t message_send_thread; pthread_create(&message_send_thread, NULL , tcp_send, (void*)transmit); floor_update_not_sent=0; elev_set_door_open_lamp(1); elev_set_button_lamp(BUTTON_INSIDE,elevator.floor_current,0); sleep(2); elev_set_door_open_lamp(0); sum = 0; switch (elevator.next_direction){ case DIRN_DOWN: for(int i = elevator.floor_current;i>=0;i--){ sum += elevator.queue[i]; } if(sum!=0){ elevator.direction = DIRN_DOWN; } else { for(int i = elevator.floor_current;i<N_FLOORS;i++){ sum+=elevator.queue[i]; } if(sum!=0) elevator.direction = DIRN_UP; } break; case DIRN_UP: for(int i = elevator.floor_current;i<N_FLOORS;i++){ sum += elevator.queue[i]; } if(sum!=0){ elevator.direction = DIRN_UP; } else { for(int i = elevator.floor_current;i>=0;i--){ sum+=elevator.queue[i]; } if(sum!=0) elevator.direction = DIRN_DOWN; } break; } elev_set_motor_direction(elevator.direction); } else if(floor_update_not_sent){ message_send.elevator.direction = elevator.direction; message_send.elevator.prev_direction = elevator.direction; message_send.elevator.floor_current = elevator.floor_current; char transmit[BUFSIZE]; serialization(ELEV_UPDATE, message_send, transmit); pthread_t message_send_thread; pthread_create(&message_send_thread, NULL , tcp_send, (void*)transmit); floor_update_not_sent=0; } } else floor_update_not_sent=1; } for(int i=0;i<N_FLOORS;i++){ sum+=elevator.queue[i]; } if(!connection){ break; } } }
/* * Class: no_ntnu_stud_torbjovn_comedielevator_NativeInterface * Method: elev_set_door_open_lamp * Signature: (I)V */ JNIEXPORT void JNICALL Java_no_ntnu_stud_torbjovn_comedielevator_NativeInterface_elev_1set_1door_1open_1lamp (JNIEnv * env, jobject obj, jint value) { elev_set_door_open_lamp(value); }
States fsm_stop_button_pressed(system_state_t* system_state, States fsm_previous_state){ system_state->next_floor = system_state->current_floor + system_state->motor_direction; elev_set_motor_direction(DIRN_STOP); reset_active_floors(system_state); reset_called_floors(system_state); initialize_button_indicators(system_state); if (elev_get_floor_sensor_signal() >= 0){ elev_set_stop_lamp(1); elev_set_door_open_lamp(1); if (!elev_get_stop_signal()){ elev_set_stop_lamp(0); return idle; } return stop; } else{ while (elev_get_stop_signal()) elev_set_stop_lamp(1); if (!elev_get_stop_signal()){ elev_set_stop_lamp(0); check_cmd_buttons(system_state); for (int i = 0; i < N_FLOORS; ++i){ if (system_state->elev_active_floors[i] == 1){ if (i < system_state->current_floor){ return going_down; } else if (i > system_state->current_floor){ return going_up; } else{ // if i is the floor currently indicated if (system_state->next_floor > system_state->current_floor){ return going_down; } else 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 < system_state->current_floor){ return going_down; } else if (i > system_state->current_floor){ return going_up; } else{ // if i is the floor currently indicated0 if (system_state->next_floor > system_state->current_floor) return going_down; else return going_up; } } } } } return stop; }