int parallel(double *curr_coord){
int i = 0;
double leftir;
double rightir;
int parallel = 0;
double error = 0.04 ;
double time = 30.0;
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
while (!parallel){
for ( i = 0; i < time; i++)
{
leftir += get_front_ir_dist(LEFT);
rightir+= get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
if(leftir/time > rightir/time + error){
set_motors(1, -1);
position_tracker(curr_coord);
for ( i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else if(rightir/time > leftir/time + error){
set_motors(-1, 1);
position_tracker(curr_coord);
for (i = 0; i < time; i++){
leftir += get_front_ir_dist(LEFT);
rightir += get_front_ir_dist(RIGHT);
}
leftir = leftir/time;
rightir = rightir/time;
}
else{
parallel = 1;
set_motors(0, 0);
}
printf(" ### Paralleling LEFTIR %f, RIGHTIR %f \n", leftir/time, rightir/time);
usleep(300000);
set_motors(0, 0);
position_tracker(curr_coord);
}
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
usleep(20000);
}
void setup()
{
connect_to_robot();
initialize_robot();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
set_origin();
set_point(0, 0);
print_square_centres();
get_motor_encoders(&leftprev, &rightprev);
moveBackwards(PHASE1_SPEED, PHASE1_SLOW, START_OFFSET * CM_TO_ENCODER); //move robot to centre of first square.
}
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;
}
void re_initialize_robot() {
printf("re-initializing\n");
set_ir_angle(LEFT, ir_angle[LEFT]);
set_ir_angle(RIGHT, ir_angle[RIGHT]);
}
void initialize_robot() {
printf("initializing\n");
set_ir_angle(LEFT, 0);
set_ir_angle(RIGHT, 0);
reset_motor_encoders();
}
int main() {
connect_to_robot();
initialize_robot();
set_origin();
printf("Ghandy-Bot activated, destroy!!!\n");
//Set front IR sensors to to face left and right
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
init_map();
print_map();
robot.map[1][1].walls[0] = 1;
robot.map[1][2].walls[2] = 1;
robot.map[1][1].walls[1] = 1;
robot.map[2][1].walls[3] = 1;
robot.map[1][3].walls[2] = 1;
robot.map[1][2].walls[0] = 1;
robot.map[0][4].walls[2] = 1;
robot.map[0][3].walls[0] = 1;
robot.map[3][4].walls[3] = 1;
robot.map[2][4].walls[1] = 1;
robot.map[3][2].walls[2] = 1;
robot.map[3][1].walls[0] = 1;
robot.map[3][2].walls[3] = 1;
robot.map[2][2].walls[1] = 1;
int i;
for(i=1;i<=4;i++)
robot.map[0][i].walls[3] = 1;
for(i=1;i<=4;i++)
robot.map[i][1].walls[2] = 1;
for(i=1;i<=4;i++)
robot.map[3][i].walls[1] = 1;
print_map();
/* Phase 1 */
//Perform depth first search on maze
//dfs(0, 0, M_PI / 2);
/* Phase 2 */
lee(0,1); //Update matrix with Lee costs from node [0,1]
/* Generate shortest path to node [3,4] (finish line) */
generate_path(3,4);
follow_path(); //Follow the generated path to finish
//Phew, did we win???
printf("Ghandy-Bot deactivated!\n");
return 0;
}
void setAngles(){
set_ir_angle(RIGHT, 20);
set_ir_angle(LEFT, -20);
}
void setAnglesStart()
{
set_ir_angle(RIGHT, 35);
set_ir_angle(LEFT, -35);
}
void setAngles(){
set_ir_angle(RIGHT, 45);
set_ir_angle(LEFT, -45);
}
