10 years, 11 months ago.

Latency or Buffering Issue

Hi All,

I'm working with a HiTec 5035HD motor and because of some unusual drift in the motor, I have physically tied it to a MA3 magnetic absolute encoder so I can keep track of the amount of rotation with high precision:

The encoder itself outputs a 250Hz pwm signal where the duty cycle corresponds to the absolute position of the encoder. I'm driving the servo with a standard pwm signal the servo library on the cookbook. The problem I encounter is when I read the pwm signal into the mbed. I use the following code to set the motor position incrementally and read in the current pwm signal (dutcy cycle):

...

Servo panServo(p21);
Servo tiltServo(p26);

DigitalIn pwmInPan(p29);
DigitalIn pwmInTilt(p30);

float currPanServo = 0.0;
float currTiltServo = 0.0;
...

float getPWM(DigitalIn pin)
{  
    Timer localTimer;  
    float d = 0.0;
    float p = 0.0;
    float pw = 0.0;

    while (!pin){}; // wait hi
    while (pin){};  // wait for low to make sure we dont catch it int he middle

    // pulseIn
    while (!pin){}; // wait for high
    localTimer.reset();
    localTimer.start();
    
    while (pin){}; // wait for low
    d = localTimer.read_us(); //provides echo time in microseconds
    
    while (!pin){}; // wait for high again 
    localTimer.stop();   
    p = localTimer.read_us();
    
    pw = d / p;   // duty cycle = (amount of time  high / period time)
    return pw;
}

void SetMotorPosition(float inPanPos, float inTiltPos)
{        
    float panDiff = 0.0;
    float tiltDiff = 0.0;

    float moveMotorVal = .01;
    float encPosThresh = .001;
    
    bool motorPositioned = false;
    bool panPositioned = false;
    bool tiltPositioned = false; 
    while(!motorPositioned)
    {    
        currPanEncoder = getPWM(pwmInPan); 
        panDiff = currPanEncoder - inPanPos;
        if(!panPositioned)
        {
            pc.printf("CurrPanEncoder: %f PanServo: %f \n", currPanEncoder, panServo.read());        
            
            float pDelta = (panDiff * 360.0) * .01;
            pc.printf("PanDelta: %f\n", pDelta);
            if(panDiff > 0.0)
            {
                currPanServo = currPanServo + moveMotorVal;
            }
            else
            {
                currPanServo = currPanServo - moveMotorVal;
            }                
             
            currPanServo = clamp(currPanServo, minPan, maxPan);                
            
            // check to see if it's small enough to some threshold
            if(abs(panDiff) < encPosThresh || (currPanServo <= minPan || currPanServo >= maxPan))
                panPositioned = true;      
            
        }
        
        currTiltEncoder = getPWM(pwmInTilt);
        tiltDiff = currTiltEncoder - inTiltPos;
        if(!tiltPositioned)
        {   
            pc.printf("CurrTiltEncoder: %f TiltServo: %f \n", currTiltEncoder, tiltServo.read());          
            
            float tDelta = (tiltDiff * 360.0) * .01;
            pc.printf("TiltDelta: %f\n", tDelta);               
            if(tiltDiff > 0.0)
            {
                currTiltServo = currTiltServo + moveMotorVal;
            }
            else
            {
                currTiltServo = currTiltServo - moveMotorVal;
            }
            
            currTiltServo = clamp(currTiltServo, minTilt, maxTilt);       
            if(abs(tiltDiff) < encPosThresh || (currTiltServo <= minTilt || currTiltServo >= maxTilt))
                tiltPositioned = true;             
                
        }   
        
        panServo = currPanServo;
        wait(0.01);
        tiltServo = currTiltServo;
        wait(0.01); 
         
             
        if(panPositioned && tiltPositioned)
            motorPositioned = true;
    }    
}

void InitMotors()
{    
    float range = 0.00045;
    float position = 0.5;
    panServo.calibrate(range, 45.0); 
    tiltServo.calibrate(range, 45.0); 
    
    // Init with Long wait time
    // Motor Home position should be at  pan: .6 and tilt: .4
    currPanServo = .7;  // go tot he left for testing pusposes
    panServo = currPanServo;
    wait(0.5);
    currTiltServo = .4;  // go to home position
    tiltServo = currTiltServo;
    wait(0.5);
    
    // Approx 80deg pan field, 70 tilt field
    maxPan = 1.0;
    minPan = 0.2;
    maxTilt = 0.8;
    minTilt = 0.1;

    // Motor is now off to left (for pan, tilt is already home, so send it current value)
    SetMotorPosition(.9335, .6553);
}

and this is my output:

CurrPanEncoder: .841220 PanServo: .70
CurrPanEncoder: .841215 PanServo: .68
CurrPanEncoder: .841218 PanServo: .67
CurrPanEncoder: .841221 PanServo: .66
CurrPanEncoder: .841218 PanServo: .65
CurrPanEncoder: .850110 PanServo: .64
CurrPanEncoder: .858105 PanServo: .63
CurrPanEncoder: .865117 PanServo: .62
CurrPanEncoder: .872413 PanServo: .61
CurrPanEncoder: .881218 PanServo: .60
CurrPanEncoder: .892887 PanServo: .59
CurrPanEncoder: .908112 PanServo: .58
CurrPanEncoder: .915626 PanServo: .57
CurrPanEncoder: .924480 PanServo: .56
CurrPanEncoder: .933016 PanServo: .55

Basically, I tell the motor to move 1 degree at a time and check the encoder value to see the absolute position as a test. For example, I want to be at abs value .9335 (which corresponds to the servo motor being at .60, this was tested and checked manually) and when the absolute position is close enough, I stop the motor. The problem is that by the time it reaches .933 (in above output), the motor has already gone too far (it should be at .60 but it's at .55) because of the delay. As a result, it overshoots greatly. As you can see, there is some delay in getting the correct most update to date encoder value in the beginning and I'm not sure why (the first five values are roughly the same while the motor is moving). I have also replaced the absolute encoder with a faster analog variant (10-bit 2.6Khz, that outputs a voltage between 0-3.3v to give the absolute position) and the same thing happens. I get analog values that look like they are buffered before I get the actual updated position. Upon hooking it up to an oscope, I can't seem to see any kind of delay when I move either encoder. I'm wondering if there is some strange buffering issue or latency issue in the mbed that I'm not taking into account. I don't think the encoders are the problem, as for a 250Hz signal, I am getting new values every 4ms. The other 2.6Khz analog variant I tried should be instantaneous.

Note that the motor values and the encoder values are both between 0-1 but they are not related at all. Is there an internal update frequency that the pins use to check for new data? I was also using an interrupt based approach to check the duty cycle, so that values are updated on interrupt high, low, with no luck.

Does anyone have any suggestions?