void correctToStraight(int motorSpeed) {
if (get_side_ir_dist(LEFT) < 15) {
set_motors(motorSpeed + 3, motorSpeed);
}
if (get_side_ir_dist(RIGHT) < 15) {
set_motors(motorSpeed, motorSpeed + 3);
}
}
// Move to coordinate of a node, and checking walls, assigning the values in adjacent arrays
void move_to_node(double curr_coord[2], struct node* node){
printf("\t \t ### Moving to node: %d ###\n",node->name );
printf("Moving to coord: x %f y %f \n",node->x, node->y );
move_to(curr_coord, node->x, node->y);
printf(" Arrived at node[%d] ! \n", node->name);
// Start mapping walls
struct node* currentnode = node;
currentnode->visited = 1;
int currentfront = node_in_front(face_angle, currentnode);
int currentleft = node_on_left(face_angle, currentnode);
int currentright = node_on_right(face_angle, currentnode);
int i = available_adjacent(currentnode);
int j;
if (no_wall_front() == 1){
if (nodes[currentfront]->visited == 0){
currentnode->adjacent[i] = nodes[currentfront];
i = available_adjacent(currentnode);
j = available_adjacent(nodes[currentfront]);
nodes[currentfront]->adjacent[j] = currentnode;
}
printf("There is no wall front , US dist: %d \n", get_us_dist());
}
else if(no_wall_front() == 0){
parallel(curr_coord);
}
if (no_wall_left() == 1){
if (nodes[currentleft]->visited == 0){
currentnode->adjacent[i] = nodes[currentleft];
i = available_adjacent(currentnode);
j = available_adjacent(nodes[currentleft]);
nodes[currentleft]->adjacent[j] = currentnode;
}
printf("There is NO wall left ! LEFT IR : %d\n", get_side_ir_dist(LEFT));
}
if (no_wall_right() == 1){
if (nodes[currentright]->visited == 0){
currentnode->adjacent[i] = nodes[currentright];
i = available_adjacent(currentnode);
j = available_adjacent(nodes[currentright]);
nodes[currentright]->adjacent[j] = currentnode;
}
printf("There is NO wall right ! RIGHT IR : %d \n", get_side_ir_dist(RIGHT));
}
printf("Checking ALL for node[%d] wall done!\n", node->name);
}
int checkLeftWall()
{
int noOfChecks = 5;
int distances[noOfChecks];
int final = 0;
int i;
for(i = 0;i<noOfChecks;i++)
{
distances[i] = get_side_ir_dist(LEFT);
final += distances[i];
//printf("%d\n",final );
}
