How to avoid lag of servo motor used with distance sensor in FRDM-KL25Z

Question

I am having trouble with using distance sensor and servo motor at the same time. My project is a radar project. I have a servo motor sweeping around and on top of it I have distance sensor. When I compile and run this code, servo motor lags and does not sweep smoothly like it did without distance sensor. What can I do to avoid lag?

int main() {
    while(1) {
        runServo120();          
    }
}

long echo_duration() { //used for distance sensor
    timer.reset();
    trig = 0;
    wait_us(2);
    trig = 1;
    wait_us(10);
    trig = 0;
    while(echo == 0);
    timer.start();
    while(echo == 1);
    timer.stop();
    return timer.read_us();
}

long distance(){ // used for distance sensor
    long duration = echo_duration();
    if(duration > 30000)
        return -1;
    long distacne_cm = duration /29 / 2 ;

    return distacne_cm;  
}

void runServo120(){ //sweeps 120 degree and calls distance() function
    float angle = 0.00115; 
    for(int i = 0 ; i<50; i++){ //going forward

        anglePin = 1;
        wait(angle);
        anglePin = 0;
        wait(0.02-angle);
        angle = angle + 0.000014;

        if((int)distance() < 30){
            myled = 0;    
        }else{
            myled = 1;    
        }       
    }
    angle = 0.00185;
    for(int i = 0 ; i<50; i++){ //going backward

        anglePin = 1;
        wait(angle);
        anglePin = 0;
        wait(0.02-angle);
        angle = angle - 0.000014;

        if((int)distance() < 30){
            myled = 0;    
        }else{
            myled = 1;    
        }             
    }

}

Answer 1

You have several things going on, all of which contribute to your problems:

1. The "micro servo SG90" is not one of the strongest servos around. It might not be capable of rapidly swinging the mass of your ultrasound sensor around.
2. Your code generates the servo pulses in software with a "wait." When you have only your "sweep" code, it moves smoothly because you are generating pulses all the time at a steady rate. When you put your distance measuring code in there, though, you aren't generating any pulses while you are measuring distances. The servo has no pulse - they can hold position or wander when there's no pulse. Yours probably wanders when there's no pulse.
3. You are measuring distances at each servo position. Each measurement takes some time. Your code expects each measurement can take over 30 milliseconds. If there's nothing close to your sensor, then you will be waiting at least 30 milliseconds at each position. Unless there's something close by, in which case to pause will be shorter.

You need to do a few things:

1. Test your motion code without the distance measuring code - but with the sensor mounted on the servo. If it is smooth, then your servo has enough "muscle" for the job. If it lags and jumps, then it is too weak. You might slow down the motion and see if it gets smoother. If you can't get smooth motion without the distance measuring code, then your servo is too weak.
2. Switch to a timer based library to control the servo. This guy seems to have developed one for your board. There may be better choices out there. That's the first one I found. This keeps a steady train of pulses going to the servo even when you are measuring distances.
3. Realize that each distance measurement will take some time, and it will vary depending on what your sensor can "see." You will never get perfectly smooth motion since you have to measure distance in your motion loop.
4. See if there's a timer and interrupt based library for your sensor. It won't improve the motion problem, but will deliver more reliable distance measurements than your software loop.