int main(){
connect_to_robot();
initialize_robot();
set_origin();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
initialize_maze();
reset_motor_encoders();
int i;
for (i = 0; i < 17; i++){
set_point(nodes[i]->x, nodes[i]->y);
}
double curr_coord[2] = {0, 0};
map(curr_coord, nodes[0]);
breadthFirstSearch(nodes[0]);
reversePath(nodes[16]);
printPath(nodes[0]);
struct point* tail = malloc(sizeof(struct point));
tail->x = nodes[0]->x;
tail->y = nodes[0]->y;
struct point* startpoint = tail;
build_path(tail, nodes[0]);
// Traverse to end node.
while(tail->next){
set_point(tail->x, tail->y); // Visual display for Simulator only.
tail = tail->next;
}
tail->next = NULL; // Final node point to null.
printf("tail: X = %f Y = %f \n", tail->x, tail->y);
parallel(curr_coord);
spin(curr_coord, to_rad(180));
sleep(2);
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
mazeRace(curr_coord, nodes[0]);
return 0;
}
int main()
{
/*freopen("816.in", "r", stdin);
freopen("my.out", "w", stdout);*/
char name[30];
char tempC;
scanf("%s", name);
while(strcmp(name, "END")!=0)
{
initialize_maze();
Qhead = Qtail = 0;
int start_row, start_col, start_direction;
int end_row, end_col;
scanf("%d %d %c", &start_row, &start_col, &tempC);
int start_row2 = start_row, start_col2 = start_col, start_direction2;
/* translate input (starting destination) into my meaning of node in que(starting source) */
if(tempC=='E') {
start_col2++;
start_direction2 = WEST;
} else if(tempC=='S') {
start_row2++;
start_direction2 = NORTH;
} else if(tempC=='W') {
start_col2--;
start_direction2 = EAST;
} else if(tempC=='N') {
start_row2--;
start_direction2 = SOUTH;
} else fprintf(stderr, "no such start direction : %c\n", tempC);
pushQ(NULL, &maze[start_row2][start_col2][start_direction2]);
scanf("%d %d", &end_row, &end_col);
read_maze();
/* print_maze(); */
BFS(end_row, end_col);
/* if maze[end_row][end_col][0~3].visited are all 0 */
int i;
Node *current = NULL;
for(i=0 ; i<4 ; i++){
if( maze[end_row][end_col][i].visited ){
current = &maze[end_row][end_col][i];
}
}
printf("%s\n", name);
if(current==NULL){
printf(" No Solution Possible");
}
else
{
Node * Nstack[500];
int NstackSize = 0;
Nstack[NstackSize++] = current;
while(current->parent != NULL){
current = current->parent;
Nstack[NstackSize++] = current;
}
int count = 1;
i = NstackSize-1;
printf(" (%d,%d)", start_row, start_col);
if(i >= 0) putchar(' ');
while(i >= 0)
{
count++;
printf("(%d,%d)", Nstack[i]->row, Nstack[i]->col);
if(i!=0){
if(count%10 == 0) printf("\n ");
else putchar(' ');
}
i--;
}
}
putchar('\n');
scanf("%s", name);
}
return 0;
}
