Thread: function to count number of rotations

  1. #1
    Registered User
    Join Date
    Sep 2012
    Posts
    68

    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?

  2. #2
    and the hat of int overfl Salem's Avatar
    Join Date
    Aug 2001
    Location
    The edge of the known universe
    Posts
    39,651
    > 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?
    If you dance barefoot on the broken glass of undefined behaviour, you've got to expect the occasional cut.
    If at first you don't succeed, try writing your phone number on the exam paper.

  3. #3
    Registered User
    Join Date
    Sep 2012
    Posts
    68
    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.
    Last edited by ak47; 11-26-2023 at 11:45 PM.

  4. #4
    and the hat of int overfl Salem's Avatar
    Join Date
    Aug 2001
    Location
    The edge of the known universe
    Posts
    39,651
    OK so far.

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

    Along with the initial rotation angle.
    If you dance barefoot on the broken glass of undefined behaviour, you've got to expect the occasional cut.
    If at first you don't succeed, try writing your phone number on the exam paper.

  5. #5
    Registered User
    Join Date
    Sep 2012
    Posts
    68
    Quote Originally Posted by Salem View Post
    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?

  6. #6
    and the hat of int overfl Salem's Avatar
    Join Date
    Aug 2001
    Location
    The edge of the known universe
    Posts
    39,651
    > 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;
        }
    }
    If you dance barefoot on the broken glass of undefined behaviour, you've got to expect the occasional cut.
    If at first you don't succeed, try writing your phone number on the exam paper.

  7. #7
    Registered User
    Join Date
    Sep 2012
    Posts
    68
    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);
    }

  8. #8
    Registered User
    Join Date
    Sep 2012
    Posts
    68
    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 ?

  9. #9
    and the hat of int overfl Salem's Avatar
    Join Date
    Aug 2001
    Location
    The edge of the known universe
    Posts
    39,651
    > 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.
    If you dance barefoot on the broken glass of undefined behaviour, you've got to expect the occasional cut.
    If at first you don't succeed, try writing your phone number on the exam paper.

  10. #10
    Registered User
    Join Date
    Sep 2012
    Posts
    68
    Thanks Salem, I understand now

Popular pages Recent additions subscribe to a feed

Similar Threads

  1. Fastest way to count the number of 1's in a n-bit number
    By empire0157 in forum C Programming
    Replies: 3
    Last Post: 09-08-2012, 01:22 PM
  2. Replies: 4
    Last Post: 07-13-2012, 12:46 AM
  3. number of rotations in AVL tree
    By c_weed in forum Tech Board
    Replies: 2
    Last Post: 03-10-2012, 03:31 AM
  4. Count number!
    By tx1988 in forum C++ Programming
    Replies: 3
    Last Post: 09-21-2007, 08:23 PM
  5. count the number of token
    By rsanga1 in forum C Programming
    Replies: 0
    Last Post: 10-02-2001, 11:48 AM

Tags for this Thread