function to count number of rotations

Hello Experts,
I am trying to write a function that counts the number of rotations of a motor. The function calculates the number of rotations based on the mechanical angle. The mechanical angle is updated in real time.
rotationCounterMA() is called periodically from a state machine.

This is what i have so far:

Code:

---

void rotationCounterMA(MOTOR_Vars_t *pMotor, uint16_t *rotationCounter, Databus_Vars_t *bus)
{
    static int rotationComplete = 0;
    const float32_t ROTATION_THRESHOLD = 0.95;
    static float32_t prevMechTheta;
    float32_t currentMechTheta = 0;
    currentMechTheta = floorf(pMotor->ptrFCL->qep.MechTheta * 100) / 100;  //get the mechanical position and truncate up to two decimal places. Ex: 0.1234 will become 0.12
    switch(bus->lockedDIR_MA)
    {
    case CLOCKWISE:
    {
        //Clockwise rotation (0.0 to 1.0)
        // Check for the rotation completion
        if (currentMechTheta < ROTATION_THRESHOLD && rotationComplete)
        {
            // Reset the flag when MechTheta goes below the threshold after completion
            rotationComplete = 0;
        }


        if (currentMechTheta >= ROTATION_THRESHOLD
                && !rotationComplete)
        {
            //Make sure that the motor is rotating
            if(prevMechTheta != currentMechTheta)
            {
                // Increment the counter when MechTheta crosses the threshold for the first time
                (*rotationCounter)++;
                rotationComplete = 1; // Set the flag to avoid multiple increments for the same rotation
            }
        }
        prevMechTheta = currentMechTheta;
    }
    break;
    case COUNTERCLOCKWISE:
    {
        //TODO To be completed..
       
        //Counter Clockwise rotation (1.0 to 0.0)
       
        // Check for counterclockwise rotation completion
/*        if (pMotor->ptrFCL->qep.MechTheta > (1.0 - ROTATION_THRESHOLD)
                && rotationComplete == 1)
        {
            rotationComplete = 0;
        }


        if (pMotor->ptrFCL->qep.MechTheta <= (1.0 - ROTATION_THRESHOLD)
                && rotationComplete == 0)
        {
            (*rotationCounter)++;
            rotationComplete = -1;
        } */
    }
    break;
    }
}

---

The function for the most part works well, but i found one potential bug:
Assume motor direction as clockwise and Lets say that the starting mechanical angle is 0.6, the moment the angle crosses ROTATION_THRESHOLD , the logic thinks that one rotation is complete, But that is not the case. Only if it crosses 0.6 again is one rotation actually complete.
At the moment there is always one rotation error.
Any idea on how to go about solving this issue?

> pMotor->ptrFCL->qep.MechTheta
What type is this, and what range of values does it have?
Using floating point is a mistake. You're just going to build up cumulative errors.

> The function calculates the number of rotations based on the mechanical angle. The mechanical angle is updated in real time.
> rotationCounterMA() is called periodically from a state machine.
The problem you're going to have is if it's capable of moving more than half a rotation between samples, is figuring out whether it's fast one way or slow the other.

> switch(bus->lockedDIR_MA)
Why is this in bus, not motor?
Same problem here, can the motor change direction more than once between calls?

pMotor->ptrFCL->qep.MechTheta is a float. This is a normalized value and varies from 0.0 to 1.0 only.
0 being the 0th Mechanical degree and 1.0 being the 360th mechanical degree. To avoid the cumulative errors I though truncating the decimals to two places would help reduce the error?

-The problem you're going to have is if it's capable of moving more than half a rotation between samples, is figuring out whether it's fast one way or slow the other.
No the motor is not capable of moving more than half rotation between samples. The sampling is very fast fast, (3.3 KHz) and the maximum motor speed is 333 Hz, so its 10 times fast.

-Why is this in bus, not motor?
-Same problem here, can the motor change direction more than once between calls?
The direction information is coming from another CPU, which transmits the data via a data bus. So the data is read directly from the bus
No, the motor will never change direction(Not allowed to) as long as the motor is running.

OK so far.

You should have a pointer to a structure containing
- rotationCounter
- rotationComplete
- prevMechTheta

Along with the initial rotation angle.

Quote:

---

Originally Posted by Salem

OK so far.

You should have a pointer to a structure containing
- rotationCounter
- rotationComplete
- prevMechTheta

Along with the initial rotation angle.

---

Okay coming back to my logical bug, How do i remove the first rotation bug? Any idea?

> 0 being the 0th Mechanical degree and 1.0 being the 360th mechanical degree.
So 0 and 100 are the same thing in your scaled result.
Having two values for the same thing might be a problem.
Like when is it 0, and when is it 100?

Oh, and I wouldn't bother dividing by 100 again in your code. Just keep it as an integer in the range 0 to 100.

> How do i remove the first rotation bug? Any idea?

Consider two initial positions
0 - some arbitrary direction at the zero point
50 - a point at 180' to the zero point.

All these examples constitute the completion of a rotation.

clockwise

Code:

---

previous    initial    current
90          0          10
40          50          60

---

counter-clockwise

Code:

---

previous    initial    current
10          0          90
60          50          40

---

Then I might try something like this.

Code:

---

#define FLOAT_TO_INT_ANGLE  100

bool clockwise_crossed(int initial, int previous, int current) {
    if ( initial < previous ) initial += FLOAT_TO_INT_ANGLE;
    if ( current < previous ) current += FLOAT_TO_INT_ANGLE;
    return ( previous < initial && initial <= current );
}

typedef struct {
    int initial;
    int previous;
    int count;
} rotate_state_t;

void rotationCounterMA(MOTOR_Vars_t *pMotor, rotate_state_t *state, Databus_Vars_t *bus)
{
    int current = pMotor->ptrFCL->qep.MechTheta * FLOAT_TO_INT_ANGLE;
    switch(bus->lockedDIR_MA)
    {
        case CLOCKWISE:
            if ( clockwise_crossed(state->initial, state->previous, current) ) {
                state->count++;
            }
            state->previous = current;
            break;
    }
}

---

I see, so for the opposite direction i just need to subtract?

Code:

---

bool counterclockwise_crossed(uint16_t initial, uint16_t previous, uint16_t current)
{
    if (initial > previous)
    {
        initial -= FLOAT_TO_INT_ANGLE;
    }


    if (current > previous)
    {
        current -= FLOAT_TO_INT_ANGLE;
    }
    return (previous > initial && initial >= current);
}

---

Also in your example, it is assumed that the motor first moves clockwise then it moves counter- clockwise, But that may not always be the case. What if the motor is instructed to rotate counter-clockwise first ?

> But that may not always be the case. What if the motor is instructed to rotate counter-clockwise first ?
...
> The direction information is coming from another CPU, which transmits the data via a data bus.

I'd say it's just another member of rotate_state_t

The very first call to the function, you record the initial state of everything you care about.
From then on, you can track changes.

Thanks Salem, I understand now :)