// Basic Left wall following code
void wallFollowingL()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
//for(i = 0; i < 250; i++)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
printf("i = %i\n", i);
if ( rightFrontIR < 30 )
{ turnRight (90.0, 5);} // super turn right
else if (leftFrontIR > 30 && leftSideIR > 25)
{ set_motors(SPEED-15, SPEED+50);} // super turn left
else if (leftFrontIR > 38)
{ set_motors(SPEED, 30);} // too far, turn left
else if ( leftFrontIR < 18 )
{ set_motors(SPEED*1.5, SPEED);} // turn right
else
{set_motors(SPEED+5, SPEED+5); } // Straight
}
}
// Basic Left wall following code
void wallFollowingL()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 40){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(SPEED, -SPEED);
}
}
if (leftFrontIR > 30 )
{ set_motors(SPEED-10, SPEED+10); } //turn left
else if (leftFrontIR < 15 )
{ set_motors (SPEED+10, SPEED-10); } //turn right
else
{set_motors(SPEED, SPEED); } // Straight
}
}
int no_wall_front(){
int i = 0;
double wall = 0;
for (i = 0; i < 25; i++)
wall += get_us_dist();
return (wall/25 < 40 ) ? 0 : 1;
}
// Basic Right wall following code
void wallFollowingR()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
{
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
if (distAhead < 45){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(20, -20);
}
}
else if (rightFrontIR > 30 && rightSideIR > 25)
{ set_motors(SPEED+30, SPEED-8); } //turn right
else if (rightFrontIR < 20 )
{ set_motors (SPEED-10, SPEED+25); } //turn left
else
{set_motors(SPEED+8, SPEED+8); } // Straight
}
}
void adjustStart()
{
turnToDirection(ABOUT_TURN);
int backDistFromWall = get_us_dist();
int targetDistSide = 27;
while(fabs(backDistFromWall - 27) > 1)
{
printf("%d\n",backDistFromWall);
if(backDistFromWall > 27)
{
set_motors(5,5);
}
else
{
set_motors(-5,-5);
}
backDistFromWall = get_us_dist();
calcPos();
}
turnToDirection(LEFT_TURN);
int sideDistFromWall = get_us_dist();
while(fabs(sideDistFromWall - targetDistSide) > 1)
{
printf("%d\n",sideDistFromWall);
if(sideDistFromWall > targetDistSide)
{
set_motors(5,5);
}
else
{
set_motors(-5,-5);
}
sideDistFromWall = get_us_dist();
calcPos();
}
turnToDirection(LEFT_TURN);
}
// 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);
}
// Basic Right wall following code
void wallFollowingR()
{
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR, distAhead, i;
while (1)
//for(i = 0; i < 170; i++)
{
if (thetaAcc > 40)
{ break; }
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
distAhead = get_us_dist();
int lbump, rbump;
check_bumpers(&lbump, &rbump);
if (lbump){
set_motors(SPEED+20, SPEED-20);
}
if (rbump){
set_motors(SPEED-20, SPEED+20);
}
if (distAhead < 35){ //in the L shape
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, SPEED);
}
else {
set_motors(+20,-20);
}
}
else if (rightFrontIR > 30)
{ set_motors(SPEED+10, SPEED-20); } //turn right
else if (rightFrontIR < 20)
{ set_motors (SPEED-25, SPEED+15); } //turn left
else
{set_motors(SPEED+7, SPEED+7); } // Straight
calculateTheta();
}
}
void moveForwards(int maxspd, int minspd, int dist)
{
//speeds up, moves the robot forwards a certain distance, and slows down.
int left1, left2, right1, right2;
get_motor_encoders(&left1, &right1);
while(1)
{
get_motor_encoders(&left2, &right2);
int dtravelled = ((left2 - left1) + (right2 - right1)) / 2;
if(dtravelled < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(minspd + dtravelled / 2, minspd + dtravelled / 2);
}
else if(dtravelled < dist - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(maxspd, maxspd);
}
else if(dtravelled < dist)
{
//decelerate phase
set_motors(minspd + (dist - dtravelled) / 2, minspd + (dist - dtravelled) / 2);
}
else
{
break;
}
positioncalc();
int ultrasound = get_us_dist();
if(ultrasound < FRONT_WALL_MIN_DIST) { break; }
}
set_motors(0, 0);
}
char* getPath(int startx, int starty, int destx, int desty)
{
//moves the robot from a square to another (in the grid where start = (0, 0)), following left wall, and records the path taken.
int left, right, index = 0, currentx = startx, currenty = starty; //stores current position of robot in the grid in currentx and currenty
int idealBearing = getIdealBearing();
char* path = malloc(sizeof(char) * PATH_MAX_LENGTH);
while (currentx != destx || currenty != desty)
{
get_front_ir_dists(&left, &right);
int front = get_us_dist();
/*if(front < MAX_DIST_TO_WALL)
{
checkFrontWall(front);
}*/
//checks accurancy of the robot's calculated current position vs its real position
log_trail();
set_point(xpos, ypos);
if(left > MAX_DIST_TO_WALL)
{
//turn left
turnLeft(PHASE1_SPEED, PHASE1_SLOW, 90);
idealBearing -= 90;
if(idealBearing < 0) { idealBearing += 360; }
path[index] = 'L';
}
else if(front > MAX_DIST_TO_WALL)
{
//go straight
path[index] = 'S';
}
else if(right > MAX_DIST_TO_WALL)
{
//turn right
turnRight(PHASE1_SPEED, PHASE1_SLOW, 90);
idealBearing += 90;
if(idealBearing >= 360) { idealBearing -= 360; }
path[index] = 'R';
}
else
{
//turn around
turnRight(PHASE1_SPEED, PHASE1_SLOW, 180);
idealBearing += 180;
if(idealBearing >= 360) { idealBearing -= 360; }
path[index] = 'B';
}
fixBearing(idealBearing);
moveForwards(PHASE1_SPEED, PHASE1_SLOW, SQUARE_DIST * CM_TO_ENCODER);
//uses robot's idealBearing to figure out which square in the grid the robot is currently in after moving
switch(idealBearing)
{
case 0: currenty++; break;
case 90: currentx++; break;
case 180: currenty--; break;
case 270: currentx--; break;
default: printf("Error: unexpected value for idealBearing\n"); break;
}
index++;
}
path[index] = '\0';
return path;
}
void findAdjacentNodes(int currentNode, int orientation, int adjacentNodes[]) {
// Special case if called from the starting cell
if (currentNode == 0) {
adjacentNodes[0] = -1;
adjacentNodes[1] = 1;
adjacentNodes[2] = -1;
return;
}
int sampleSize = 40;
int frontSampleSize = 5;
int leftFrontSample[sampleSize];
int rightFrontSample[sampleSize];
int frontSample[frontSampleSize];
// Take a sample of measurements from the left, right and front
int i;
for (i = 0; i < sampleSize; i++) {
leftFrontSample[i] = get_front_ir_dist(LEFT);
rightFrontSample[i] = get_front_ir_dist(RIGHT);
usleep(500);
}
for (i = 0; i < frontSampleSize; i++) {
frontSample[i] = get_us_dist();
usleep(500);
}
// Take the median of those measurements to avoid misreads
int left = getMedian(leftFrontSample, sampleSize);
int front = getMedian(frontSample, frontSampleSize);
int right = getMedian(rightFrontSample, sampleSize);
adjacentNodes[0] = -1;
adjacentNodes[1] = -1;
adjacentNodes[2] = -1;
// No wall on the left
if (left >= 35) {
switch (orientation) {
case 0:
adjacentNodes[0] = currentNode - 1;
break;
case 1:
adjacentNodes[0] = currentNode + 4;
break;
case 2:
adjacentNodes[0] = currentNode + 1;
break;
case 3:
adjacentNodes[0] = currentNode - 4;
break;
}
}
// No wall in front
if (front >= 40) {
switch (orientation) {
case 0:
adjacentNodes[1] = currentNode + 4;
break;
case 1:
adjacentNodes[1] = currentNode + 1;
break;
case 2:
adjacentNodes[1] = currentNode - 4;
break;
case 3:
adjacentNodes[1] = currentNode - 1;
break;
}
}
// No wall on the right
if (right >= 35) {
switch (orientation) {
case 0:
adjacentNodes[2] = currentNode + 1;
break;
case 1:
adjacentNodes[2] = currentNode - 4;
break;
case 2:
adjacentNodes[2] = currentNode - 1;
break;
case 3:
adjacentNodes[2] = currentNode + 4;
break;
}
}
// Print what cells are on the left, right and front, and which of them are not walled off
printf("\n");
printf("Finding adjacent nodes \n");
printf("Left: %d; Front: %d; Right %d \n", left, front, right);
printf("Left: %d; Front: %d; Right %d \n", adjacentNodes[0], adjacentNodes[1], adjacentNodes[2]);
}
void correctFront(int currentNode, Location* currentLocation, int* orientation, Location mazeCoords[]) {
int correctionSpeed = 5;
// Get the distance of the wall in front from the robot (if there is a wall)
double frontMeasurement = (double) get_us_dist() - OFFSET_US;
// No wall in front detected OR there is significant error in the bearing
if (frontMeasurement > (60.0 - ROBOTWIDTH)) {
//printf("No wall in front \n");
return;
}
// Distance of robot's assumed current location from the center of the current node4
double deltaCenterX = currentLocation->x - mazeCoords[currentNode].x;
double deltaCenterY = currentLocation->y - mazeCoords[currentNode].y;
//printf("deltaCenterX = %f; deltaCenterY = %f \n", deltaCenterX, deltaCenterY);
// Calculate what the distance from the front wall is supposed to be with the given the location
double targetDistance = 30.0 - ((ROBOTWIDTH + 1)/2);
switch (*orientation) {
// North
case 0:
targetDistance -= deltaCenterY;
//currentLocation->bearing = 0;
break;
// East
case 1:
targetDistance -= deltaCenterX;
//currentLocation->bearing = (M_PI / 2.0);
break;
// South
case 2:
targetDistance += deltaCenterY;
//currentLocation->bearing = M_PI ;
break;
// West
case 3:
targetDistance += deltaCenterX;
//currentLocation->bearing = ((3.0 * M_PI) / 2.0);
break;
}
double frontError = targetDistance - frontMeasurement;
//printf("Front Measurement: %f; Target Distance: %f \n", frontMeasurement, targetDistance);
// If frontError within bounds, don't bother correcting
if (fabs(frontError) < 1) {
return;
}
while (fabs(frontError) > 1) {
//set_point(currentLocation->x, currentLocation->y);
//log_trail();
if (frontError >= 0) {
set_motors(-correctionSpeed, -correctionSpeed);
} else {
set_motors(correctionSpeed, correctionSpeed);
}
frontMeasurement = (double) get_us_dist() - OFFSET_US;
frontError = targetDistance - frontMeasurement;
}
set_motors(0, 0);
usleep(100000);
}
//Basic wall following code
void wallFollowing()
{
//Sensor readings
int rightSideIR, leftSideIR, rightFrontIR, leftFrontIR;
int ultrasound;
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
inputMotors();
while (1)
{
if (thetaAcc > 40)
{ break; }
int lbump, rbump;
check_bumpers(&lbump, &rbump);
if (lbump){
set_motors(SPEED+20, SPEED-20);
}
if (rbump){
set_motors(SPEED-20, SPEED+20);
}
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
get_side_ir_dists(&leftSideIR, &rightSideIR);
ultrasound = get_us_dist();
//Hit wall
if (ultrasound < 30){
int i;
/*
for(i=0; i<20; i++){
set_motors(-SPEED, -SPEED);
ultrasound = get_us_dist();
}
*/
//set_ir_angle(RIGHT, 45);
//set_ir_angle(LEFT, -45);
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//for (i=0; i<15; i++){
if (leftFrontIR - rightFrontIR > 0){
set_motors(-SPEED, -SPEED);
//set_motors(-SPEED, SPEED);
set_motors(-25,25);
}
else{
set_motors(-SPEED, -SPEED);
set_motors(SPEED, -SPEED);
}
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//}
setAngles();
}
/*
//Deadlock
if (ultrasound < 30 && leftFrontIR < 40 && rightFrontIR < 40)
{
int i;
//while ((ultrasound < 80 && leftFrontIR < 45) || (ultrasound < 80 && rightFrontIR < 45))
while (ultrasound < 87)
{ set_motors(-SPEED, -SPEED);
//get_side_ir_dists(&leftFrontIR, &rightSideIR);
ultrasound = get_us_dist();
}
set_ir_angle(RIGHT, 45);
set_ir_angle(LEFT, -45);
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
//printf("%d %d\n", leftFrontIR, rightFrontIR);
if (leftFrontIR - rightFrontIR > 0){
//for (i=0; i<20; i++)
set_motors(-SPEED-20, -SPEED+20);
//turnLeft(50);
}
else if (rightFrontIR - leftFrontIR > 0){
//for (i=0; i<20; i++)
set_motors(-SPEED+20, -SPEED-20);
//turnRight(50);
}
setAngles();
//Make 90 deg left turn
//for (i=0; i<35; i++)
//{ set_motors(SPEED, -SPEED); }
}
*/
else{
if (leftFrontIR > rightFrontIR)
{
set_motors(SPEED-25, SPEED+25);
//addNode(pointer, LEFT);
}
else if (rightFrontIR > leftFrontIR )
{
set_motors(SPEED+25, SPEED-25);
//addNode(pointer, RIGHT);
}
else
{
set_motors(SPEED, SPEED);
//addNode(pointer, STRAIGHT);
}
}
calculateTheta();
}
}
void getIRDist(){
ultrasound = get_us_dist();
get_side_ir_dists(&leftSideIR, &rightSideIR);
get_front_ir_dists(&leftFrontIR, &rightFrontIR);
}
