turning calculate_turn(directions current, directions desired) { if(current==desired) return FORWARD; else if (current==inverse_direction[desired]) return BACKWARD; else if(desired==right_of[current]) return RIGHT; else if(desired==left_of[current]) return LEFT; else cout << __PRETTY_FUNCTION__ << " Failed" << endl; cout << "Current = "; print_direction(current); cout << "\nDesired = "; print_direction(desired); cout << endl; throw(INVALID_DIRECTIONS); }
void print_route(void){ cout << "\n----Printing route plan----\n"; cout << "route starting node = " << route.node[0] << endl; cout << "route.end_node = " << route.end_node << endl; cout << "route.end_direction = "; print_direction(route.end_direction); cout << endl; cout << " (Sanity check -- status.current_node = " << status.current_node << ")\n"; cout << "Proposed route is:\n"; for(unsigned int i = 0; i<=route.length; i++){ cout << route.node[i]; if(i<route.length) cout << " -> "; } cout << "\n---------------------------\n\n"; return; }
/* Saves the matchinfo in parsable form to stdout. */ static void save(const struct ipt_ip *ip, const struct ipt_entry_match *match) { struct ipt_connbytes_info *sinfo = (struct ipt_connbytes_info *)match->data; if (sinfo->count.from > sinfo->count.to) printf("! --connbytes %llu:%llu ", sinfo->count.to, sinfo->count.from); else printf("--connbytes %llu:%llu ", sinfo->count.from, sinfo->count.to); fputs("--connbytes-mode ", stdout); print_mode(sinfo); fputs("--connbytes-dir ", stdout); print_direction(sinfo); }
static void connbytes_save(const void *ip, const struct xt_entry_match *match) { const struct xt_connbytes_info *sinfo = (const void *)match->data; if (sinfo->count.from > sinfo->count.to) printf("! --connbytes %llu:%llu ", (unsigned long long)sinfo->count.to, (unsigned long long)sinfo->count.from); else printf("--connbytes %llu:%llu ", (unsigned long long)sinfo->count.from, (unsigned long long)sinfo->count.to); fputs("--connbytes-mode ", stdout); print_mode(sinfo); fputs("--connbytes-dir ", stdout); print_direction(sinfo); }
/* Prints out the matchinfo. */ static void print(const struct ipt_ip *ip, const struct ipt_entry_match *match, int numeric) { struct ipt_connbytes_info *sinfo = (struct ipt_connbytes_info *)match->data; if (sinfo->count.from > sinfo->count.to) printf("connbytes ! %llu:%llu ", sinfo->count.to, sinfo->count.from); else printf("connbytes %llu:%llu ",sinfo->count.from, sinfo->count.to); fputs("connbytes mode ", stdout); print_mode(sinfo); fputs("connbytes direction ", stdout); print_direction(sinfo); }
static void connbytes_print(const void *ip, const struct xt_entry_match *match, int numeric) { const struct xt_connbytes_info *sinfo = (const void *)match->data; if (sinfo->count.from > sinfo->count.to) printf("connbytes ! %llu:%llu ", (unsigned long long)sinfo->count.to, (unsigned long long)sinfo->count.from); else printf("connbytes %llu:%llu ", (unsigned long long)sinfo->count.from, (unsigned long long)sinfo->count.to); fputs("connbytes mode ", stdout); print_mode(sinfo); fputs("connbytes direction ", stdout); print_direction(sinfo); }
void print_status(void){ cout << "\n------Printing Status------\n"; cout << "Current job: "; print_job(status.job); cout << endl; if(status.travel==AT_NODE) cout << "Currently: AT_NODE - node:" << status.current_node << endl; else cout << "Currently: IN_TRANSIT between nodes " << status.last_node << " and " << status.next_node << endl; cout << "Current direction: "; print_direction(status.direction); cout << endl; cout << "Front parcel is "; print_parcel_type(status.front_parcel); cout << "\nBack parcel is "; print_parcel_type(status.back_parcel); cout << "\nDelivered " << status.parcels_delivered << " parcels\n"; cout << "\n---------------------------\n\n"; return; }
//implemetation of functions int start_finding(int start_x, int start_y) { int inter = 0; int token = 0; int cur_x = start_x, cur_y = start_y; int dir = SOUTH; int ppath = -1; int npop = 0; //how many points should be poped struct POINT *cur_p = NULL; struct POINT *tmp_p = NULL; int ret = 0; #ifdef DEBUG inter = Robot_GetIntersections(); #else inter = get_intersection(); #endif cur_p = mark_point(cur_x, cur_y, inter); dir = get_direction(cur_p); #ifdef DEBUG printf("start point: "); print_point(cur_p); printf("\n"); #endif while(token < TOKEN_COUNT) { #ifdef DEBUG //inter = Robot_GetIntersections(); //print_intersection(inter); #endif if(points[cur_x][cur_y].detected == 0) cur_p = mark_point(cur_x, cur_y, inter); else cur_p = &points[cur_x][cur_y]; push(cur_p); //print_stack(); if(dir = get_direction(cur_p)) { //update current point switch(dir) { case EAST: cur_x += 1; break; case SOUTH: cur_y -= 1; break; case WEST: cur_x -= 1; break; case NORTH: cur_y += 1; break; } #ifdef DEBUG print_direction(cur_p, dir); ret = aud_move(cur_p, dir); #else //move one step display_clear(0); display_goto_xy(0, 0); display_int(cur_p->x, 2); display_goto_xy(3, 0); display_int(cur_p->y, 2); display_goto_xy(7, 0); display_int(cur_x, 2); display_goto_xy(10, 0); display_int(cur_y, 2); display_goto_xy(0, 1); display_int(g_dir, 3); display_goto_xy(5, 1); display_int(dir, 3); display_goto_xy(0, 2); display_int(cur_p->inter&0xF0, 3); display_update(); ret = move(cur_x, cur_y); #endif #ifdef DEBUG inter = Robot_GetIntersections(); #else inter = get_intersection(); #endif cur_p = mark_point(cur_x, cur_y, inter); #ifdef DEBUG print_point(cur_p); #endif if(ret == ROBOT_SUCCESS) { #ifndef DEBUG #endif } else if(ret == ROBOT_TOKENFOUND) { tmp_p = &points[cur_x][cur_y]; if(tmp_p->has_token == 0) { tmp_p->has_token = 1; token++; #ifdef DEBUG printf("[%d. token]\n", token); #endif } else { #ifdef DEBUG printf("[not a new token]\n"); #endif } if(token == TOKEN_COUNT) { //all token were found, go back to start point #ifdef DEBUG printf("going back to start point......\n"); #endif push(cur_p); ppath = find_shortest_path(cur_p->x, cur_p->y, START_X, START_Y); if(ppath) { //going back to last open point ppath--; while(ppath >= 0) { tmp_p = shortest_path[ppath]; dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y); #ifdef DEBUG print_point(tmp_p); printf("\n"); ROBOT_MOVE(tmp_p->x, tmp_p->y); #else display_clear(0); display_goto_xy(0, 0); display_int(cur_p->x, 2); display_goto_xy(3, 0); display_int(cur_p->y, 2); display_goto_xy(7, 0); display_int(tmp_p->x, 2); display_goto_xy(10, 0); display_int(tmp_p->y, 2); display_goto_xy(0, 1); display_int(g_dir, 3); display_goto_xy(5, 1); display_int(dir, 3); display_goto_xy(0, 2); display_int(cur_p->inter&0xF0, 3); display_update(); move(tmp_p->x, tmp_p->y); #endif cur_p = tmp_p; ppath--; } //delete the path in stack pop(npop + 1); cur_p = tmp_p; cur_x = cur_p->x; cur_y = cur_p->y; } #ifdef DEBUG printf("task finished!\n"); #else beep(); #endif break; } } else { #ifdef DEBUG printf("move failed!\n"); #endif } } else { //there is no ways forward, go back to nearest open point tmp_p = get_last_open_point(); npop = stack_pointer - get_stack_index(tmp_p->x, tmp_p->y); #ifdef DEBUG printf("going back to (%d, %d)\n", tmp_p->x, tmp_p->y); #endif if(tmp_p) { if((tmp_p->x == START_X) && (tmp_p->y == START_Y) && !IS_OPEN_POINT(points[tmp_p->x][tmp_p->y])) { #ifdef DEBUG return 0; #else stop_robot(); beep(); return 0; #endif } ppath = find_shortest_path(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y); if(ppath) { //going back to last open point ppath--; while(ppath >= 0) { tmp_p = shortest_path[ppath]; dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y); #ifdef DEBUG ROBOT_MOVE(tmp_p->x, tmp_p->y); #else display_clear(0); display_goto_xy(0, 0); display_int(cur_p->x, 2); display_goto_xy(3, 0); display_int(cur_p->y, 2); display_goto_xy(7, 0); display_int(tmp_p->x, 2); display_goto_xy(10, 0); display_int(tmp_p->y, 2); display_goto_xy(0, 1); display_int(g_dir, 3); display_goto_xy(5, 1); display_int(dir, 3); display_goto_xy(0, 2); display_int(cur_p->inter&0xF0, 3); display_update(); move(tmp_p->x, tmp_p->y); #endif cur_p = tmp_p; ppath--; } //delete the path in stack pop(npop + 1); cur_p = tmp_p; cur_x = cur_p->x; cur_y = cur_p->y; } else { //was already at every point and back to start point //task should be ended //that means, not enough token can be found #ifdef DEBUG printf("task ended without enough token were found.\n"); #endif break; } } } #ifdef DEBUG printf("\n"); #endif } return 0; }
void navigate() { //check the route starts where we if(route.node[0] != status.current_node) { cout << "Route was planned incorrectly\n"; cout << "Route starts at node " << route.node[0] << endl; cout << "But we are currently at node " << status.current_node << endl; throw(INVALID_ROUTE); return; } //Check the route for continuity for(unsigned int i=0; i<route.length; i++) { if(idp_map[route.node[i]][route.node[i+1]] == NC) { cout << "node " << route.node[i] << " is not connected to node " << route.node[i+1] << endl; throw(INVALID_ROUTE); } } turning turn; if(route.length!=0) { for(unsigned int i=1; i<=route.length; i++) { status.last_node = status.current_node; status.next_node = route.node[i]; status.travel = IN_TRANSIT; //navigate from route.node[i] to route.node[i+1] //direction logic directions current = status.direction; directions desired = idp_map[status.current_node][status.next_node]; try { turn = calculate_turn(current, desired); } catch(...) { throw; } cout << "Turning "; print_turn(turn); cout << " to reach node " << status.next_node << endl; DEBUG("Calling lf_turn()"); lf_turn(turn); //turn to face the desired node; cout << "Finished turn, starting transit" << endl; DEBUG("Calling lf_until_junction()"); lf_until_junction(); status.travel = AT_NODE; status.current_node = route.node[i]; status.direction = inverse_direction[(idp_map[status.current_node][status.last_node])]; cout << "Reached node " << status.current_node << endl << endl; } } if(route.end_direction) { try { turn = calculate_turn(status.direction, route.end_direction); cout << "Making final "; print_turn(turn); cout << " turn to face "; print_direction(route.end_direction); cout << endl; lf_turn(turn); status.direction = route.end_direction; } catch(...) { throw; } } cout << "\nnavigate() has finished following the route\n"; }