Freesteel Blog » Direct Servo Motor PWM and encoders

Direct Servo Motor PWM and encoders

Monday, April 18th, 2016 at 4:43 pm Written by:

This has taken a lot of effort fighting through the device trees on the Beaglebone to make it access the PWM and Quadrature encoder services at the same time, which is the minimum required to effect a stimulus response observation from a DC servo motor powered by an H-bridge.

What happens when you apply a fixed voltage to the DC motor for an eighth of a second, then reverse the voltage for another eighth of a second, and then set the voltage to zero (and try it for many different voltages and directions):

Here’s the graph of motor positions (in Y) over time (in X) for 20 different trials:

motortrialgraphs

The dots correspond to the times when the voltage was turned on, reversed, and turned off. The exact timing of these transitions is not consistent because I was executing this in a dumb bit of C-code on the Beaglebone that was subject to operating system delays. Luckily I’ve logged the realtime of each sample in the file, so it shouldn’t matter to the data.

Here’s at code which changed the voltages at particular points in the logging loop:

int usec = start_time.tv_usec;
*cmpa_reg = 500;  // out of 1000 for a 50% duty cycle that delivers zero volts
int cmpaval = 650;  // 150/500*(30V bench power supply) = 9Volts
for (int j = 0; j < 600; j++) {
    if (j == 10)
        *cmpa_reg = cmpaval; 
    if (j == 200)  
        *cmpa_reg = 1000-cmpaval; // reverse voltage
    if (j == 400)
        *cmpa_reg = 500; // set back to zero
    gettimeofday(&start_time, 0); 
    fprintf(fout, "%d %d\n", (start_time.tv_usec - usec), (*qposcnt_reg - cntstart0)); 
    usleep(400); 
}

We had a lot of problem getting the Beaglebone to receive quadrature encoding and generate PWM code at the same time. The code for doing it using the PWMSS (pulse width modulation subsystem) is here.

Briefly, you need to turn on the eqep quadrature decoder on pins P8.33 and P8.35 like this:

echo bone_eqep1 > /sys/devices/bone_capemgr*/slots

and set up the PWM generation on pin P9.14 like this:

echo am33xx_pwm > /sys/devices/bone_capemgr.9/slots
echo bone_pwm_P9_14 > /sys/devices/bone_capemgr.9/slots
cd /sys/devices/ocp.3/pwm_test_P9_14.12
echo 1 > run
echo 10000 > period
echo 5000 > duty

And then the address mapping (which can be seen in the manual) was like this:

long PWMSS_offset = 0x48302000; 
long PWMSS_size = 0x260; 
long CMPA = 0x200 + 0x12; 
long QPOSCNT = 0x180 + 0x00; 
long QCAPCTL = 0x180 + 0x2C; 

int fd = open("/dev/mem", O_RDWR);
void* pwmssaddr = mmap(0, PWMSS_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, PWMSS_offset);
unsigned short *cmpa_reg = pwmssaddr + CMPA;
unsigned int* qcapctl_reg = pwmssaddr + QCAPCTL; 
int* qposcnt_reg = pwmssaddr + QPOSCNT; 
close(fd); 

I wanted to write it in Python instead of C, but its mmap() function can’t handle writing just a short 2-byte value to the CMPA register rather than a 4-byte word, and when you do the latter, the number just doesn’t get saved. (This took forever to debug.)

Let’s see if we can get any consistent kinematics from this data

The data was loaded using this loop:

fname = "/home/goatchurch/geom3d/pru-servo-driving/motorcurves.txt"
ls = open(fname).readlines()
lss = [ ]
for l in ls:
    if not l.strip():
        lss.append([])
    elif not lss[-1]:
        lss[-1].append(int(l.strip()))
    else:
        usec, qpos = list(map(int, l.split()))[1:]
        lss[-1].append((usec + (1000000 if usec<0 else 0), qpos))
                               # ^^ handle the usec counter clocking past 0

It’s going to be easier to work with velocities rather than positions.

Here’s the graph of velocities from when the voltage is applied to the time just before the voltage is reversed:
motortrialgraphsvelocity0
Everything seems to get up to a speed that’s proportional to the voltage, but it looks like there’s a bit of a hump near the beginning.

Let’s look at the easier side, where the voltage is set back to 0 (actually it’s 50% PWM at 100kHz):
motortrialgraphsvelocity1
The reason this should be easier is that the ending conditions are all the same, but we start with different velocities.

The code for generating one of those velocity profiles (and trimming out the tail) from the quadrature positions is as follows:

    tvseq = [ ]
    for i in range(405, 595):
        # convert time from microseconds to seconds, and 
        # quadrature position (100per rev) to revolutions per second 
        v = 1e-6*(lsss[i+dd][1] - lsss[i-dd][1])/(lsss[i+dd][0] - lsss[i-dd][0])
        tvseq.append((lsss[i][0]*1e-6, v/400))
    while tvseq and abs(tvseq[-1][1])<0.0005:
        tvseq.pop()

I’d like to fit an exponential decay curve to it, which is the solution to the differential equation dv/dt = -lam*v that says the braking force is proportional to the velocity:

def fun(X, tvseq):
    fac, lam = X
    return sum((v-fac*math.exp(-lam*(t-tvseq[0][0])))**2  for t, v in tvseq)

The curve fitting is done as using the minimize function that finds the values of fac and lam that minimize the error function fun():

# cyan for original data
sendactivity(contours=[[(t*10, v)  for t, v in tvseq]], materialnumber=1)

# red for curve fit data
res = scipy.optimize.minimize(fun=fun, x0=(tvseq[0][1],1), args=(tvseq,))
fac, lam = res["x"]
sendactivity(contours=[[(t*10, fac*math.exp(-lam*(t-tvseq[0][0])))  for t, v in tvseq]], materialnumber=2)

motortrialgraphsvelocity1exp

The numbers for the exponentials are pretty consistently around 74 once you get past the lower voltage lower starting speeds at the beginning of the list.

1, 1, 87, 89, 75, 77, 72, 73, 73, 72, 71, 72, 72, 82, 73, 74, 73, 74, 76, 73, 74, 74, 74, 74, 75, 75, 75, 75, 75, 75

This means that when the voltage is set to 50% PWM (or zero net) the motor slows down at the rate of 74revs per second per second times its revs per second at the time. (There has got to be a better way to express this.)

If I was to run this experiment with metal disks screwed onto the motor spindle to give it different angular momentums, I might be able to derive the force and motor momentum variables.

Looking again at the start end, it’s clear that the exponential curves don’t exactly fit.

motortrialgraphsvelocity0exp

Overall the exponents at the different voltages smaller to larger are as follows:

0, 1, 65, 58, 68, 67, 69, 72, 73, 73, 72, 75, 71, 74, 71, 74, 69, 73, 68, 67, 63, 64, 58, 58, 51, 53, 45, 45, 38, 39

So it’s sort of around the 74 mark for some of the time, going bad where the curves fit the worst.

If we divide the trajectory into two parts, as in the picture:
motortrialgraphsvelocity1exp2

We get the following exponents:

First: 0, 1, 1, 1, 1, 1, 1, 1, 78, 79, 74, 78, 76, 74, 74, 74, 71, 76, 71, 73, 77, 77, 81, 79, 85, 87, 92, 88, 99, 98
Second: 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 31, 33, 32, 26, 29, 26

I think there’s some kind of run-away situation here where the initial hit of energy gives the motor a kick so that it spins ahead mechanically before the rest of the system, magnetism, coils, etc, have a chance to catch up. That first part of the trajectory is only 0.04seconds long.

Finally, plotting the limiting speed for each voltage gives this reasonably linear graph (subject to the bad values from the dead spot and stickiness at low voltages):
motortrialgraphsvelocity1lim
This represents about 2.78 revolutions per second per volt.

What about the voltage reversal case?

motortrialgraphsvelocity3exp

Once again, the first part of the motion moves faster than what can be fitted with an exponential curve (which might be the underlying dynamic), but the exponents look follow a familiar pattern

1, 1, 83, 88, 91, 89, 88, 89, 92, 87, 90, 88, 87, 84, 78, 79, 69, 56, 61, 57, 52, 49, 43, 41, 39, 34, 35, 31, 31, 29

(There might be a consistent factor of 400 missing from all of these numbers.)

The conclusion is that there might be a kinematic model for this motor where you calculate the revs per second as 2.78*voltage, subtract this from the current revs per second, multiply by 74, and reckon that this will be the rate of change in revs per second at this exact moment.

Any departure from this free-space model means that there’s something interesting going on, such as the tool is encountering material. If you know the voltage to velocity to energy conversions we might be able to tell how the material is responding.

The long term objective is to replace the expensive no-feedback-black-box Servo Motor Drives that receive step and direction pulses from the Machinekit/Beaglebone controller with a position-encoder-to-PWM generator programmed on the Beaglebone itself factored through an H-bridge.

What a Servo Motor Drive does is receive position commands from the controller in the form of step and direction, and combines them with position measurements from the quadrature position encoder on the servo motor, and delivers a variable positive or negative voltage (actually PWM at about 60kHz on 40 volts) to the servo motor to drive it towards the commanded position.

In educational examples this conventionally done using a PID control which, say, every 1/20th of a second compares the actual position to the commanded position and updates the voltage accordingly. However, this is not exactly what I observed when I analysed one of these Servo Motor Drives with an oscilloscope.

What happens when there is a load?

We tried a few patterns on the polar graph where the pen setup is lifted and lowered by a servo motor, like so:

These experiments produced a position graph like so:

motortrialpolarweight1

The setting of the voltages was as follows:

*cmpa_reg = 500; // halfway zero net voltage
for (j = 0; j < 12000; j++) {
    if (j == 10) 
        *cmpa_reg = 420; 
    if (j == 5000)   // brief 10ms reversal of voltage
        *cmpa_reg = 620; 
    if (j == 5020)
        *cmpa_reg = 420; 
           
    if (j == 8000)   // set back to "zero"
        *cmpa_reg = 500; 

At the point where the voltage is turned off, there’s this consistent 0.06second dwell at the servo motor before it starts to fall:

motortrialpolarweight2
This must be due to the upward velocity of the weight flying up at about 0.3m/s in freefall before it begins to fall after 0.03*g seconds until it reaches the same height 0.06seconds after it had been initially thrown upwards.

Here is a closeup of the 10ms voltage reversal bounce.
motortrialpolarweight3
Remember, the position is of the motor, which starts going the other way as soon as the voltage changes, while the weight might be in freefall. Then there are a few oscillations after it catches it.

It’s not ideal to be testing this with a weight on a string. The kinematics of a rigid machine with ball-screws might be easier to model. However, we’re not going to do that until we have super-fast limit switches and working cut-offs (as well as our quick-blow fuses) as the damage that can be done is far more severe.

Leave a comment

XHTML: You can use these tags: <a href="" title=""> <abbr title=""> <acronym title=""> <blockquote cite=""> <code> <em> <strong>