/* There shall be one task, called lift_task, for the lift. */ void lift_task(void)//TODO { int next; int dirchange; int send_task_id; int length;//DUMMY message_data_type msg; draw_lift(mainlift); while (1) { si_message_receive((char *) &msg, &length, &send_task_id); if(msg.type == MOVE_MESSAGE) { lift_next_floor(mainlift, &next, &dirchange); lift_move(mainlift, next, dirchange); lift_has_arrived(mainlift); } else if(msg.type == TRAVEL_MESSAGE) { /* a travel message is sent to the lift task when a person would like to make a lift travel */ enter_floor(mainlift, msg.id, msg.from_floor, msg.to_floor); } draw_lift(mainlift); } }
static void *lift_thread(void *unused) { int next_floor=0; int change_direction=0; while(1){ // Move lift one floor lift_next_floor(Lift, &next_floor, &change_direction); lift_move(Lift, next_floor, change_direction); lift_has_arrived(Lift); } return NULL; }
static void *lift_thread(void *unused) { //printf("Started lift_thread\n"); //The functions lift_next_floor, lift_move and lift_has_arrived shall be called by the lift thread while(1){ int next_floor, change_direction; lift_next_floor(Lift, &next_floor, &change_direction); lift_move(Lift, next_floor, change_direction); lift_has_arrived(Lift); // Move lift one floor } return NULL; }
static void *lift_thread(void *unused) { int next_floor=0; int change_direction=0; while(1){ // Move lift one floor lift_next_floor(Lift, &next_floor, &change_direction); lift_move(Lift, next_floor, change_direction); lift_has_arrived(Lift); //usleep(3000000); //fprintf(stderr, "Number of iterations: %d\n", iterations); } return NULL; }
task main() { initializeRobot(); waitForStart(); // Wait for the beginning of autonomous phase. servo[sgrab] = 185; servo[srelease] = 140; wait10Msec(55); Move(100,40); wait10Msec(50); Turn(20); wait10Msec(55); Ultra_Seek(21,48); wait10Msec(55); Turn(6); wait10Msec(55); Move(14,10); wait10Msec(55); servo[sgrab] = 255; wait10Msec(40); Turn(-10); wait10Msec(40); Move(-15,75); wait10Msec(55); lift_move(18); wait10Msec(55); servo[srelease] = 255; wait10Msec(150); Turn(-180); wait10Msec(40); //servo[sgrab] = 100; wait10Msec(40); }
/* There shall be one task, called lift_task, for the lift. */ void lift_task(void) { int next; int dirchange; draw_lift(mainlift); while (1) { lift_next_floor(mainlift, &next, &dirchange); lift_move(mainlift, next, dirchange); lift_has_arrived(mainlift); /* För debug lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_next_floor(mainlift, &next, &dirchange); printf("|%d %d %d |", mainlift->floor, next, dirchange); lift_move(mainlift, next, dirchange); lift_has_arrived(mainlift); printf("| %d %d |", next, dirchange); exit(mainlift->floor); */ } }
task main() { initializeRobot(); waitForStart(); // Wait for the beginning of autonomous phase. servo[srelease] = 140; servo[sgrab] = 185; wait10Msec(55); if(SensorValue[IR] == 5 && SensorValue [IR] < 120) { Ultra_Seek(35,120); wait10Msec(55); Turn(19); wait10Msec(55); lift_move(38); wait10Msec(55); Move(4,5); wait10Msec(55); servo[srelease] = 255; wait10Msec(150); servo[srelease] = 140; wait10Msec(55); lift_move(-30); wait10Msec(55); Turn(117); wait10Msec(55); Move(-10,100); wait10Msec(55); Turn(45); wait10Msec(55); Move(-40, 100); wait10Msec(55); Turn(-180); wait10Msec(55); } if (SensorValue[IR] !=5 || SensorValue[SONAR_1] > 120) { Move(4,75); wait10Msec(55); Turn(-30); wait10Msec(55); Move(70,40); wait10Msec(50); Turn(45); wait10Msec(55); Move(70,75); wait10Msec(55); Turn(15); wait10Msec(55); Ultra_Seek(21,40); wait10Msec(55); //Turn(-4); //wait10Msec(55); Move(4,5); wait10Msec(55); servo[sgrab] = 255; wait10Msec(40); Move(-10,40); wait10Msec(50); //Turn(7); //wait10Msec(55); lift_move(7); wait10Msec(55); servo[srelease] = 255; wait10Msec(150); } /*Move(-200,40); wait10Msec(50); Turn(-180); wait10Msec(55); Move(60, 75); wait10Msec(55);*/ //servo[sgrab] = 100; wait10Msec(40); }
static void lift_process(void) { lift_type Lift; Lift = lift_create(); int change_direction, next_floor; int temp_id; int temp_to_floor; int to_floors_all[MAX_N_PERSONS][NUMBER_MESSAGES]; int from_floors_all[MAX_N_PERSONS][NUMBER_MESSAGES]; int person_message_iterator[MAX_N_PERSONS]; char msgbuf[4096]; while(1){ int i; struct lift_msg reply; struct lift_msg *m; //message_send((char *) Lift, sizeof(*Lift), QUEUE_UI,0); // Draw the lift int len = message_receive(msgbuf, 4096, QUEUE_LIFT); // Wait for a message if(len < sizeof(struct lift_msg)){ fprintf(stderr, "Message too short\n"); continue; } m = (struct lift_msg *) msgbuf; switch(m->type){ case LIFT_MOVE: for (i = 0; i < MAX_N_PASSENGERS; i ++){ if (Lift->passengers_in_lift[i].id != NO_ID){ if(Lift->passengers_in_lift[i].to_floor == Lift->floor){ temp_id = Lift->passengers_in_lift[i].id; Lift->passengers_in_lift[i].id = NO_ID; Lift->passengers_in_lift[i].to_floor = NO_FLOOR; if(person_message_iterator[temp_id] < NUMBER_MESSAGES){ enter_floor(Lift,temp_id,from_floors_all[temp_id][person_message_iterator[temp_id]],to_floors_all[temp_id][person_message_iterator[temp_id]]); //message_send((char *) Lift, sizeof(*Lift), QUEUE_UI,0); // Draw the lift person_message_iterator[temp_id]++; }else{ reply.type = LIFT_TRAVEL_DONE; //message_send((char *) Lift, sizeof(*Lift), QUEUE_UI,0); // Draw the lift message_send(&reply, sizeof(reply), QUEUE_FIRSTPERSON + temp_id, 0); } } } } /* Check if passengers want to enter the lift */ for (i = 0; i < MAX_N_PERSONS; i ++){ temp_id = Lift->persons_to_enter[Lift->floor][i].id; temp_to_floor = Lift->persons_to_enter[Lift->floor][i].to_floor; if ((temp_id != NO_ID) && n_passengers_in_lift(Lift) < MAX_N_PASSENGERS){ leave_floor(Lift, temp_id, Lift->floor); enter_lift(Lift, temp_id, temp_to_floor); //message_send((char *) Lift, sizeof(*Lift), QUEUE_UI,0); // Draw the lift } } lift_next_floor(Lift, &next_floor, &change_direction); lift_move(Lift, next_floor, change_direction); break; case LIFT_TRAVEL: for(i = 0; i < NUMBER_MESSAGES; i++){ to_floors_all[m->person_id][i] = m->to_floor[i]; from_floors_all[m->person_id][i] = m->from_floor[i]; } person_message_iterator[m->person_id] = 1; enter_floor(Lift, m->person_id, m->from_floor[0], m->to_floor[0]); //message_send((char *) Lift, sizeof(*Lift), QUEUE_UI,0); // Draw the lift break; } } return; }
static void lift_process(void) { lift_type Lift; Lift = lift_create(); int change_direction, next_floor; char msgbuf[4096]; struct lift_msg travel_list[MAX_N_PERSONS]; while(1){ int i; struct lift_msg reply; struct lift_msg *m; int len = message_receive(msgbuf, 4096, QUEUE_LIFT); // Wait for a message if(len < sizeof(struct lift_msg)){ fprintf(stderr, "Message too short\n"); continue; } m = (struct lift_msg *) msgbuf; for(i = 0; i < MAX_N_PASSENGERS; i++){ if (Lift->passengers_in_lift[i].to_floor == Lift->floor) { int id = Lift->passengers_in_lift[i].id; //fprintf(stderr,"ID is: %d\n",id); (travel_list[id].trip)++; //fprintf(stderr,"Trip is: %d\n", travel_list[id].trip); //message_send((char *) &reply, sizeof(reply), QUEUE_FIRSTPERSON + Lift->passengers_in_lift[i].id ,0); /*for(j = 0; j < MAX_N_PASSENGERS; j++){ fprintf(stderr, "before removing passengers_in_lift[%d].id = %d\n", j, Lift->passengers_in_lift[j].id); }*/ Lift->passengers_in_lift[i].id = NO_ID; Lift->passengers_in_lift[i].to_floor = NO_FLOOR; if(travel_list[id].trip == PLANNED_TRIPS){ message_send((char *) &reply, sizeof(reply), QUEUE_FIRSTPERSON + id ,0); //travel_list[id].trip = 0; //fprintf(stderr,"inne i skicka meddelande\n"); /*for(j = 0; j < MAX_N_PASSENGERS; j++){ fprintf(stderr, "passengers_in_lift[%d].id = %d\n", j, Lift->passengers_in_lift[j].id); }*/ } else if(travel_list[id].trip > PLANNED_TRIPS){ //fprintf(stderr, "IASFHFHKUEFHKSFH\n"); /*for(j = 0; j < MAX_N_PASSENGERS; j++){ fprintf(stderr, "passengers_in_lift[%d].id = %d\n", j, Lift->passengers_in_lift[j].id); }*/ exit(1); } else{ enter_floor(Lift, id, travel_list[id].from_floor[travel_list[id].trip], travel_list[id].to_floor[travel_list[id].trip]); //fprintf(stderr,"next trip here\n"); } } } // Check if passengers want to enter elevator // Remove the passenger from the floor and into the elevator for(i = 0; i < MAX_N_PERSONS; i++){ person_data_type person = Lift->persons_to_enter[Lift->floor][i]; if (person.id != NO_ID && (n_passengers_in_lift(Lift) < MAX_N_PASSENGERS)) { //fprintf(stderr,"Going to enter lift\n"); leave_floor(Lift, person.id, Lift->floor); enter_lift(Lift, person.id, person.to_floor); } } // Move the lift lift_next_floor(Lift, &next_floor, &change_direction); lift_move(Lift, next_floor, change_direction); break; case LIFT_TRAVEL: // TODO: // Update the Lift structure so that the person with the given ID is now present on the floor travel_list[m->person_id]=*m; enter_floor(Lift, m->person_id, m->from_floor[0], m->to_floor[0]); break; default: fprintf(stderr, "Error: unkown message type sent!!!!! \n"); break; } }