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?

The pins run at full clock speed, so they don't add latency. Isn't it just a matter of the response time of your servo which takes some time? Your getPWM routine isn't exactly fast either, but I don't think that is the issue.

At what kind of speed is your servo rotating? If it is slow I don't expect its inertia to be a problem, but if it is rotating fast it might be.

Well, the servo speed is rated at 60 degrees in 1/10th a second without load so it's going reasonably fast because there is almost no load on my setup, but I don't think the motor matters for this particular problem. If the motors move, the only feedback I have is from the encoder and it appears that the encoder is behind somehow. The motors move by giving it a pwm signal and all I want is for the encoder to be sending back updates.

For the getPWM function, it's not ideal, but it gives me values every 4ms (roughly) which is to be expected for a 250Hz signal. Perhaps there is an issue with the encoder I'm not understanding, but even for an analog encoder with a 2.6Khz signal, the same thing happens so that baffles me as well.

Is it possible that the wait command buffers or does something unexpected to the incoming signal?

The wait command just waits for the specified time, if you read an input after the wait command it will return the input value it has after the wait command.

My question about the servo speed was mainly how much latency the servo itself has: if you 'tell' it to go to 60 degrees for example, what is the time between when you tell it to go there and when it arrives there? From the part where you show the output, it stops there because then it thinks it reached its destination, right? If you output a few more encoder values, will it continue to increase for a bit?

Yes, the motor stops because it think its reached its destination. The encoder values will increase to about .975 so that it catches up to where the motor is actually positioned (it's well over the desired position at this point).

How could I measure the motor latency? I see what you're saying in that the problem might be from the motor, it's fairly fast so inertia might be an issue. The only way I could exactly verify how long it takes to get the motor to move 60 degrees is to use the encoder itself to measure it exactly, which is a problem given that it looks like there is latency in reading the values. I can try measuring how long it takes when I issue the move command and when it "thinks" it's gotten to the location.

Didn't even occur to me that it could have been the motor, it makes a lot of sense. Incrementing a degree at a time is one big problem. I probably should be using a dc motor here instead of a servo which has a built in potentiometer and it's own absolute tracking implemented. It's not an on/off move/stop control mechanism like a dc motor.

Maybe I should match the encoder range to the motor range, and move the motor all at once and check to see if has moved x degrees? I'll think about this, I feel like I'm reinventing some form of control/feedback mechanics that's been done a million times. :)

One problem is that the servo will also have a feedback controller, so multiple feedback controllers running over each other doesn't make it easier. Normal feedback is done using a PID controller (wiki it if you aren't familiar with it :) ). If you would want to make such a feedback loop I would at least start with a PI controller (so only the proportional and integral part, and not differential). While not the fastest it is easier to get stable when there is a delay present.

You can get it alot more advanced than that, you could for example also compare the value where it stops moving with the PWM value you send to it. That way you can calculate the drift, and use a feedforward mechanism to compensate for it. But I would start with a PI controller if you want a control loop there.

Hi,

You can get the motor to move and your encoder is reporting back it position so you have enough for a simple control loop. I have skimmed your code but not enough to check for each of these possible problems.

1. Make sure your actual postion vs demand position comparison respects the direction of movement required. This is best served by using absolute values as much as possible and applying your direction carefully in the code structure. Your comparison statements need to be audited for this in particular.

2. A proportional loop might serve you better. This is where you apply the demand to the motor as a mulitiple of the error (your error being the difference between the demand position and the actual position (the multiple, of "gain" can be a decimal value)). Your code keeps the demand the same regardless of the error. Still respect the direction of movement needed!

3. I see you have tried to apply a "dead band" where the motor stops close to the demand postion. Using error will help as a low error provides very small demand but make sure your dead band is large enough and again respect the "direction of correction" at all times. The proper term for the dead band is a permitted "following error".

4. To get any proportional loop working start by running the code with the motor stationary. Read the encoder position and then save that as your demand position. Then "turn on" the position loop. The motor should stay still. If your system is working OK you should feel effort from the motor to return to position if you move it by hand in either direction. Motor effort should increase the further you move off position. Then try changing the demand slowly and the motor should follow. You will have to fiddle with the gain.

5. Once you have this working you need to "profile" your demand as the motor will try to jump if your demand jumps which is almost certainly not the kind of behaviour you want and can cause other issues.

6, To clarify your demand is a virtual position created by your host control (joystick, movement profile etc). This is fed into your code and compared to the actual motor position. The error between the two is then applied as a demand (multiplied by some factor which may be less than 1) to the motor in the correct direction.

7. You will have issues if your motor happens to park at the loop position of your encoder (the roll over spot between 0 and max position). Test at the mid point of your encoder for now and deal with that challenge later.

8. Did I say you need to respect the direction of movement you actually need moment to moment?

Good luck.