//
// Return all the path of nodes that can be linearally reached from this node
// The path expands in both directions so the first node in the path is not necessarily the source
//
Path Bigraph::constructLinearPath(VertexID id)
{
Path sensePath;
Path antisensePath;
followLinear(id, ED_SENSE, sensePath);
followLinear(id, ED_ANTISENSE, antisensePath);
// Construct the final path
Path final = reversePath(antisensePath);
final.insert(final.end(), sensePath.begin(), sensePath.end());
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;
}
