main.c

Luminary Micro BLDC motor control software

        // Transition to the RUN state.
        //
        if((g_ulState & STATE_FLAG_FORWARD) == STATE_FLAG_BACKWARD)
        {
            g_ulState = STATE_BACK_RUN;
        }
        else
        {
            g_ulState = STATE_RUN;
        }

        //
        // Set the initial speed as the current speed for the control loop.
        //
        g_ulSpeed = g_ulMeasuredSpeed << 14;
        g_lSpeedIntegrator = (g_ulDutyCycle * 65536) / g_sParameters.lFAdjI ;

        //
        // And return.
        //
        return;
    }
    
    //
    // See if it is time to commute the motor.
    //
    g_ulStartupHallStateCount--;
    if(g_ulStartupHallStateCount == 0)
    {
        //
        // Decrement the count of commutations for this state.
        //
        g_ulStateCount--;

        //
        // Reload the Hall State Counter.
        // 
        g_ulStartupHallStateCount = g_ulStartupHallStateCountReload;
        if(g_ulMeasuredSpeed < g_sParameters.ulMinSpeed)
        {
            g_ulStartupHallStateCountReload--;
            if(g_ulStartupHallStateCountReload < 5)
            {
                g_ulStartupHallStateCountReload = 5;
            }
        }

        //
        // Save the Hall State.
        //
        if((g_ulState & STATE_FLAG_FORWARD) == STATE_FLAG_BACKWARD)
        {
            g_ulStartupHallValue = ucHallSequence[5 - ucHallState];
        }
        else
        {
            g_ulStartupHallValue = ucHallSequence[ucHallState];
        }
        ucHallState += 1;
        ucHallState %= 6;
    }
}

//*****************************************************************************
//
//! Checks for motor drive faults.
//!
//! This function checks for fault conditions that may occur during the
//! operation of the motor drive.  The ambient temperature, DC bus voltage, and
//! motor current are all monitored for fault conditions.
//!
//! \return None.
//
//*****************************************************************************
static void
MainCheckFaults(void)
{
    //
    // See if the ambient temperature is above the maximum value.
    //
    if(g_ucAmbientTemp > g_sParameters.ucMaxTemperature)
    {
        //
        // Emergency stop the motor drive.
        //
        MainEmergencyStop();

        //
        // Indicate an ambient overtemperature fault.
        //
        MainSetFault(FAULT_TEMPERATURE_HIGH);
    }

    //
    // See if the DC bus voltage is below the minimum value.
    //
    if(g_ulBusVoltage < g_sParameters.ulMinVBus)
    {
        //
        // Emergency stop the motor drive.
        //
        MainEmergencyStop();

        //
        // Indicate a DC bus undervoltage fault.
        //
        MainSetFault(FAULT_VBUS_LOW);
    }

    //
    // See if the DC bus voltage is above the maximum value.
    //
    if(g_ulBusVoltage > g_sParameters.ulMaxVBus)
    {
        //
        // Emergency stop the motor drive.
        //
        MainEmergencyStop();

        //
        // Indicate a DC bus overvoltage fault.
        //
        MainSetFault(FAULT_VBUS_HIGH);
    }

    //
    // See if the motor current is below the minimum value.  This check is only
    // performed when the motor is running and is being driven at or above its
    // minimum speed.
    //
    if((g_sParameters.sMinCurrent != 0) &&
       (g_sMotorCurrent < g_sParameters.sMinCurrent) &&
       (g_ulState != STATE_STOPPED))
    {
        //
        // Emergency stop the motor drive.
        //
        MainEmergencyStop();

        //
        // Indicate a motor undercurrent fault.
        //
        MainSetFault(FAULT_CURRENT_LOW);
    }

    //
    // See if the motor current is above the maximum value.
    //
    if((g_sParameters.sMaxCurrent != 0) &&
       (g_sMotorCurrent > g_sParameters.sMaxCurrent))
    {
        //
        // Emergency stop the motor drive.
        //
        MainEmergencyStop();

        //
        // Indicate a motor overcurrent fault.
        //
        MainSetFault(FAULT_CURRENT_HIGH);
    }

    //
    // See if motor is stalled.
    //
    if(g_ulState & STATE_FLAG_RUN)
    {
        if(g_ulMeasuredSpeed == 0)
        {
            g_ulStallDetectCount++;
            if(g_ulStallDetectCount > 1500)
            {
                //
                // Emergency stop the motor drive.
                //
                MainEmergencyStop();

                //
                // Indicate a motor stall fault.
                //
                MainSetFault(FAULT_STALL);
            }
        }
        else
        {
            g_ulStallDetectCount = 0;
        }
    }
    else
    {
        g_ulStallDetectCount = 0;
    }
}

//*****************************************************************************
//
//! Adjusts the motor drive duty cycle based on the rotor speed.
//!
//! This function uses a PI controller to adjust the motor drive duty cycle
//! in order to get the rotor speed to match the target speed.
//!
//! \return Returns the new motor drive duty cycle.
//
//*****************************************************************************
unsigned long
MainSpeedController(void)
{
    long lTempP, lTempI, lError;

    //
    // Compute the error between the current drive speed and the rotor speed.
    // (-MaxSpeed < lError < MaxSpeed)
    //
    lError = (long)(g_ulSpeed >> 14) - (long)g_ulMeasuredSpeed;

    //
    // Add the error to the integrator accumulator, limiting the value of the
    // accumulator.
    //
    g_lSpeedIntegrator += lError;
    if(g_lSpeedIntegrator > g_lSpeedIntegratorMax)
    {
        g_lSpeedIntegrator = g_lSpeedIntegratorMax;
    }
    if(g_lSpeedIntegrator < 0)
    {
        g_lSpeedIntegrator = 0;
    }

    //
    // Perform the actual PI controller computation.
    //
    lTempP = MainLongMul(g_sParameters.lFAdjP, lError);
    lTempI = MainLongMul(g_sParameters.lFAdjI, g_lSpeedIntegrator);
    lError = lTempP + lTempI;

    //
    // Limit the output of the PI controller based on the maximum speed
    // parameter (and don't allow it to go below zero).
    //
    if(lError < 0)
    {
        lError = 0;
    }
    if(lError > 65536)
    {
        lError = 65536;
    }

    //
    // Return the output of the PI controller.
    //
    return(lError);
}

//*****************************************************************************
//
//! Adjusts the motor drive speed based on the target speed.
//!
//! \param ulTarget is the target speed of the motor drive, specified as RPM.
//!
//! This function adjusts the motor drive speed towards a given target
//! speed.  Limitations such as acceleration and deceleration rate, along
//! with precautions such as limiting the deceleration rate to control the DC
//! bus voltage, are handled by this function.
//!
//! \return None.
//
//*****************************************************************************
static void
MainSpeedHandler(unsigned long ulTarget)
{
    unsigned long ulNewValue;

    //
    // Return without doing anything if the target speed has already been
    // reached.
    //
    if(ulTarget == g_ulSpeed)
    {
        return;
    }
    //
    // See if the target speed is greater than the current speed.
    //
    if(ulTarget > g_ulSpeed)
    {
        //
        // Compute the new maximum acceleration rate, based on the present
        // motor current.
        //
        if(g_sMotorCurrent >= (g_sParameters.sAccelCurrent + 200))
        {
            ulNewValue = g_sParameters.usAccel * 128;
        }
        else
        {
            ulNewValue = g_sParameters.usAccel * 128 *
                          (((g_sParameters.sAccelCurrent + 200)) -
                           g_sMotorCurrent);
        }

        //
        // See if the acceleration rate is greater than the requested
        // acceleration rate (i.e. the acceleration rate has been changed).
        //
        if(g_ulAccelRate > (g_sParameters.usAccel << 16))
        {
            //
            // Reduce the acceleration rate to the requested rate.
            //
            g_ulAccelRate = g_sParameters.usAccel << 16;
        }

        //
        // Then, see if the motor current exceeds the current at which the
        // acceleration rate should be reduced, and the newly computed
        // acceleration rate is less than the current rate.
        //
        else if((g_sMotorCurrent > g_sParameters.sAccelCurrent) &&
                (ulNewValue < g_ulAccelRate))
        {
            //
            // Set the acceleration rate to the newly computed acceleration
            // rate.
            //
            g_ulAccelRate = ulNewValue;
        }

        //
        // Otherwise, see if the acceleration rate is less than the requested
        // acceleration rate.
        //
        else if(g_ulAccelRate < (g_sParameters.usAccel << 16))
        {
            //
            // Increase the acceleration rate by 15 RPM, slowly
            // returning it to the desired rate.
            //
            g_ulAccelRate += (15 << 16);
        }

        //
        // Increase the speed fraction by the acceleration rate.
        //
        g_ulSpeedFract += (g_ulAccelRate >> 16);

        //
        // Loop while the fraction is greater than one.
        //
        while(g_ulSpeedFract >= 1000)
        {
            //
            // Increment the speed whole part.
            //
            g_ulSpeedWhole++;

            //
            // Decrement the speed fraction by one.
            //
            g_ulSpeedFract -= 1000;
        }

        //
        // Convert the speed fraction and whole part into a 16.16 motor
        // drive speed.
        //
        g_ulSpeed = ((g_ulSpeedWhole << 14) +
                         ((g_ulSpeedFract << 14) / 1000));

        //
        // See if the speed has exceeded the target speed.
        //
        if(g_ulSpeed >= ulTarget)
        {
            //
            // Set the motor drive speed to the target speed.
            //
            g_ulSpeed = ulTarget;

            //
            // Compute the speed fraction and whole part from the drive
            // speed.
            //
            g_ulSpeedWhole = ulTarget >> 14;
            g_ulSpeedFract = ((ulTarget & 0x3FFF) * 1000) >> 14;

            //
            // Set the motor status to running.
            //
            g_ucMotorStatus = MOTOR_STATUS_RUN;
        }
        else
        {
            //
            // Set the motor status to accelerating.
            //
            g_ucMotorStatus = MOTOR_STATUS_ACCEL;
        }