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";
}
