0

Here's the run down: I have a small robot with four continuous servo motors and two ultrasonic sensors. It's run by and UNO board with a Sensor Shield V5 on top. My goal is to program the front right and front left sensors to both be returning distances in inches, and have the robot turn in the direction with the most room when one of the sensors returns a distance less than some minimum. I am very confident that the motors and sensors aren't the issue, as testing them by themselves shows no issues. However, when I run the below code the robot stutters, mainly just going backwards but stuttering forwards and occasionally turning for a second or two. Any ideas what's going on?

#include <Servo.h>

Servo FL;
Servo FR;
Servo BL;
Servo BR;

const int FRecho = 11;
const int FRtrig = 10;
const int FLecho = 9;
const int FLtrig = 8;

long durationFL;
long durationFR;
int distanceInchR = 24;
int distanceCmR;
int distanceInchL = 24;
int distanceCmL;

const int minDistanceInch = 18;
const int minDistanceCm;

enum movementStates {
 STILL,
 FORWARD,
 BACK,
 TURNFR,
 TURNFL,
 NOTHING
};

enum movementStates movement;


void setup() {

  FL.attach(4);
  FR.attach(5);
  BL.attach(6);
  BR.attach(7);

  pinMode(FRtrig, OUTPUT);
  pinMode(FRecho, INPUT);
 pinMode(FLtrig, OUTPUT);
  pinMode(FLecho, INPUT);

  movement = NOTHING;

  forward();

}

void loop() {

updateDistanceFL();
updateDistanceFR();

updateDirection();

}


void forward () {
  FL.write(180);
  FR.write(0);
  BL.write(180);
  BR.write(0);
  movement = FORWARD;
}

void turnFR () {  
  FL.write(180);
  FR.write(180);
  BL.write(180);
  BR.write(180);
  movement = TURNFR;
}

void turnFL () {
  FL.write(0);
  FR.write(0);
  BL.write(0);
  BR.write(0);
  movement = TURNFL;
}

void backUp () {
  FL.write(0);
  FR.write(180);
  BL.write(0);
  BR.write(180);
  movement = BACK;
}

void still () {
  FL.write(90);
  FR.write(90);
  BL.write(90);
  BR.write(90);
  movement = STILL;
}

void updateDirection() {

  if (distanceInchR < minDistanceInch && distanceInchL < minDistanceInch) {
    if (movement != BACK) {
      backUp();
    }
    else if (distanceInchR < minDistanceInch) {
     if (movement != TURNFL) {
      turnFL();
     }
    }
    else if (distanceInchL < minDistanceInch) {
     if (movement != TURNFR) {
      turnFR();
     }
    }
    else if (distanceInchR > minDistanceInch && distanceInchL > minDistanceInch)
    if (movement != FORWARD) {
      forward();
    }
  }
    else {
      if (movement != STILL) {
        still();
      }
    }
}



void updateDistanceFR(){
digitalWrite(FRtrig, LOW);
delayMicroseconds(2);
digitalWrite(FRtrig, HIGH);
delayMicroseconds(10);
digitalWrite(FRtrig, LOW);
durationFR = pulseIn(FRecho, HIGH);
distanceCmR= durationFR*0.034/2;
distanceInchR = durationFR*0.0133/2;
  }

void updateDistanceFL(){
digitalWrite(FLtrig, LOW);
delayMicroseconds(2);
digitalWrite(FLtrig, HIGH);
delayMicroseconds(10);
digitalWrite(FLtrig, LOW);
durationFL = pulseIn(FLecho, HIGH);
distanceCmL= durationFL*0.034/2;
distanceInchL = durationFL*0.0133/2;
  }
  • the updateDirection() function contains a bunch of redundant logic tests ... it may not be doing what you expect it to do – jsotola Feb 13 at 22:34
  • Add a delay() in your loop(), your code is too busy checking and update the direction constantly without the time for the motor to move in any direction. Add a status variable to hold the current direction status, if the direction remain the same, there is no need to run updateDirection() again and again. – hcheung Feb 14 at 0:32
2

your updateDirection logic doesn't quite work the way you think it does.

after a bit of code formatting and comments it looks like this:

void updateDirection() {

    if (distanceInchR < minDistanceInch && distanceInchL < minDistanceInch) {
        if (movement != BACK) {
            backUp();
        } else if (distanceInchR < minDistanceInch) {
            //always entered if movement == BACK
            if (movement != TURNFL) {
                turnFL();
            }
        } else if (distanceInchL < minDistanceInch) {
            //never entered
            if (movement != TURNFR) {
                turnFR();
            }
        } else if (distanceInchR > minDistanceInch && distanceInchL > minDistanceInch)
            //never entered
            if (movement != FORWARD) {
                forward();
            }
        //endif distanceInchR < minDistanceInch && distanceInchL < minDistanceInch
    } else {
        // entered when distanceInchR > minDistanceInch || distanceInchL > minDistanceInch
        if (movement != STILL) {
            still();
        }
    }
}

it looks like you had a missing closing brace around the first condition. Causing the only movement functions that get called to be backUp, turnFL and still.

If we add the missing braces then it looks much better:

void updateDirection() {

    if (distanceInchR < minDistanceInch && distanceInchL < minDistanceInch) {
        if (movement != BACK) {
            backUp();
        } 
    } else if (distanceInchR < minDistanceInch) {
        if (movement != TURNFL) {
            turnFL();
        }
    } else if (distanceInchL < minDistanceInch) {
        if (movement != TURNFR) {
            turnFR();
        }
    } else if (distanceInchR > minDistanceInch && distanceInchL > minDistanceInch) {
        if (movement != FORWARD) {
            forward();
        }
    } else {
        if (movement != STILL) {
            still();
        }
    }
}
| improve this answer | |

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.