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 set_motor_direction(int lastFloor, int targetFloor){
if (targetFloor == lastFloor){
printf("Invalid input to find_direction\n");
}
else if (targetFloor > lastFloor){
elev_set_motor_direction(DIRN_UP);
}
else if (targetFloor < lastFloor){
elev_set_motor_direction(DIRN_DOWN);
}
}
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 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;
}
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);
}
/*
* Class: no_ntnu_stud_torbjovn_comedielevator_NativeInterface
* Method: elev_set_motor_direction
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_no_ntnu_stud_torbjovn_comedielevator_NativeInterface_elev_1set_1motor_1direction
(JNIEnv * env, jobject obj, jint dirn) {
elev_motor_direction_t direction = DIRN_STOP;
// Looks a bit redundant, but it's the least ugly way I could find to convert an int to an enum var
switch(dirn) {
case DIRN_DOWN:
direction = DIRN_DOWN;
break;
case DIRN_UP:
direction = DIRN_UP;
break;
}
elev_set_motor_direction(direction);
}
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;
}
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;
}
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;
}
}
}
