// 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
}
}
// 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 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
}
}
// 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();
}
}
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;
}
//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);
}
