Thursday, March 13, 2014

Create a speedometer from Rotary Incremental Encoder measures


This is what you can find in Wikipedia about Rotary Incremental Encoder:
A rotary encoder, also called a shaft encoder, is an electro-mechanical device that converts the angular position or motion of a shaft or axle to an analog or digital code.
There are two main types: absolute and incremental (relative). The output of absolute encoders indicates the current position of the shaft, making them angle transducers. The output of incremental encoders provides information about the motion of the shaft, which is typically further processed elsewhere into information such as speed, distance, and position. The PLC register value increases until it reaches the upper bound, then begin to increase from the lower value (zero).

Rotary encoders are used in many applications that require precise shaft unlimited rotation—including industrial controls, robotics, special purpose photographic lenses,[1] computer input devices (such as optomechanical mice and trackballs), controlled stressrheometers, and rotating radar platforms.

The Encoder Class
The program have to calculate and store the distance in meter made by a “device” connected to the engine, or simply counting the engine revolutions. In order to calculate the distance traveled the procedure has two take care of two factors: the Engine revolutions and a distance per round. The Motor can do many things, for example controlling a Winch. If the software needs to manage the distance done from rope, pulled or pushed by the Winch, it is necessary to use the revolution factor between the Motor and the Winch.

What it should be done is to manage an incremental encoder signal, that is a positional value, coming from motor revolutions. Imagine a disk that rotate and has a code for each position (For example 8192 values per revolution). To get all useful information it is necessary to pass through a PLC or something like this.

This information is stored into a PLC register. The PLC firmware reads this code and increments the position, moving forward or backward the values between the range This means that every sample time, the read value is added to a total amount, and if the new total value is over the upper bound, the new value is translated into the bottom.

For example consider the motor is moving forward at a constant speed. In tables below how PLC firmware gets samples. There is always two samples code taken in this example, just to simplify the comprehension of the mechanism.

Sample
Read Value
Total
PLC Register
1
1000
1000
1000
2
8192
8192
8192
3
1000
9182
9182
4
8192
16384
16384
5
1000
17384
17384
6
8192
24576
24576
7
1000
25576
25576
8
8192
32768
32768
9
1000
33768
33768
10
8192
40968
40968
11
1000
41968
41968
12
8192
49152
49152
13
1000
50152
50152
14
8192
57344
57344
15
1000
58344
58344
16
8192
65536
1
17
1000
1001
1001
18
8192
8193
8193
PLC register (UINT32 – range between 0  and 65535).

Sample
Read Value
Total
PLC Register
1
1000
1000
1000
2
8192
8192
8192
3
1000
9182
9182
4
8192
16384
16384
5
1000
17384
17384
6
8192
24576
24576
7
1000
25576
25576
8
8192
32768
-32768
9
1000
-31768
-31768
10
8192
-24576
-24576
11
1000
-23576
-23576
12
8192
-16384
-16384



PLC register (INT32 – range between –32768 and 32767).

In both cases when the upper bound is reached, the counter begin again from the bottom. The software, objective of this document, needs to read PLC register and analyze the values. The history of these values gives the distance from the starting point and also, if necessary, the total amount distance by going forward and backward.

What we have in a special case of constant speed is something like the graphic in figure:



The PLC register value increases until it reaches the upper bound, then begin to increase from the lower value (zero).

It is very important taking samples with a right frequency, otherwise there is a concrete risk to lose waves fronts, in this case the algorithm doesn’t work at all.

The idea of this class mechanism represented by the Encoder Class, is to store the previous sample, compare it with the new one and if the difference is upper than 8192, dividing the difference with 8192 in order to obtain the revolutions done. With the remainder of the division it recalculate the last sample to store for next analysis.

A particular attention needs to be done when Motor change direction.
The first sample will be the zero position, zero meter, zero kilometer. If the engine change direction the graphic change, in this case the waves will decrease in value starting from 65535 to zero etc..

Some member variables:

 
int m_prev_pulses = 0; //previuos sample stored
int m_prev_rpm = 0; //previous speed stored (useful to evaluate the direction)
bool m_prev_direction = true; //if true means forward else backward

bool m_Initializated = false;  //the first sample has been taken
int m_rounds = 0; //counter: motor rounds (distance = forward + backward)
int m_rounds_rel = 0; //counter: motor rounds relative (distance = forward - backward)

The constructor has two parameters, in order to create a flexible class. It accepts the distance per round (revolution) and the pulse per round.

 
 /// 
/// Configure the starting parameters
///

/// meter per round
/// pulses per round (from Device configuration)
public Encoder(float mpr, int ppr = 8192)
{
     m_mpr = mpr;
     m_ppr = ppr;
}

Let’s explain the main procedure. The AnalyzeSample() procedure has two input parameters, cnt that is the positional code from PLC register, and rpm that is the Engine speed in rpm (revolutions per meter).

In the current example, the samples comes from a INT32 registry so for first thing the procedure increases the cnt value in order to have always positive values. Then it decides to consider the current value or not checking a Speed filter. If the speed is too low it exits.

 

public void AnalyzeSample(int cnt, int rpm)
{
    m_prev_rpm = rpm;
    int iVal = m_Incr + cnt; //Always positive (0 - 65535)
    if (Math.Abs(rpm) <= m_PulseFilter) return; //speed too low - value ignored


It checks the speed in order to evaluate change of direction. The factor variable will determine the increase or decrease of done distance.

 

 bool bForward = rpm < 0; //forward speed < 0 | backward speed > 0
    int iFactor = bForward ? 1 : -1;
    int sample = bForward ? m_Max - iVal : iVal;


Initialization, only with the first sample this code is considered:
 

if (!m_Initializated)
    {
        m_Initializated = true;
        m_prev_pulses = sample;
        m_prev_direction = bForward;
        return;
    }


Change of direction Management:

 

 if (m_prev_direction != bForward)
    {
        m_prev_pulses = m_Max - m_prev_pulses;
        int diff = Math.Abs(m_prev_pulses - sample);
        m_prev_pulses = (sample - diff);
        if (m_prev_pulses < 0)
            m_prev_pulses = m_Max + m_prev_pulses;
        m_prev_direction = bForward;
        return;
    }
 

And finally the difference between the last sample and the current one is done.

 
 if (sample >= m_prev_pulses)
        dSmpi = (sample - m_prev_pulses);
    else
        dSmpi = (m_Max - m_prev_pulses + sample);

 


If the difference is greater than zero, it makes the division:

 

  int iQi = (int)(dSmpi / m_ppr);

    if (iQi <= 0) return; //not rounds

    m_rounds += iQi;
    m_rounds_rel = m_rounds_rel + (iFactor * iQi);

    int iRem = dSmpi - (iQi * m_ppr); //reminder

    m_prev_pulses = sample - iRem;
    if (m_prev_pulses < 0)
        m_prev_pulses = m_Max + m_prev_pulses;

 


here the complete code:

 

public void AnalyzeSample(int cnt, int rpm)
{
    m_prev_rpm = rpm;
    int iVal = m_Incr + cnt; //Always positive (0 - 65535)
    if (Math.Abs(rpm) <= m_PulseFilter) return; //speed too low - value ignored

    bool bForward = rpm > 0; //forward speed > 0 | backward speed < 0
    int iFactor = bForward ? 1 : -1;

    int sample = bForward ? iVal : m_Max - iVal;

    //first sample acquired
    if (!m_Initializated)
    {
        m_Initializated = true;
        m_prev_pulses = sample;
        m_prev_direction = bForward;
        return;
    }

    //Change of direction
    if (m_prev_direction != bForward)
    {
        m_prev_pulses = m_Max - m_prev_pulses;
        int diff = Math.Abs(m_prev_pulses - sample);
        m_prev_pulses = (sample - diff);
        if (m_prev_pulses < 0)
            m_prev_pulses = m_Max + m_prev_pulses;
        m_prev_direction = bForward;
        return;
    }                      

    int dSmpi = 0;
    if (sample >= m_prev_pulses)
        dSmpi = (sample - m_prev_pulses);
    else
        dSmpi = (m_Max - m_prev_pulses + sample);

    m_prev_direction = bForward;

    if (dSmpi <= 0) return;
    int iQi = (int)(dSmpi / m_ppr);

    if (iQi <= 0) return;

    m_rounds += iQi;
    m_rounds_rel = m_rounds_rel + (iFactor * iQi);

    int iRem = dSmpi - (iQi * m_ppr);

    m_prev_pulses = sample - iRem;
    if (m_prev_pulses < 0)
        m_prev_pulses = m_Max + m_prev_pulses;
}