Commanding a Motor Speed Toque or Position in a KMS reference project

[#1]

Introduction:

KMS has a built in Motion Sequence Builder tool that gives you a very quick way of commanding various motor control scenarios.  If your motor control device requires external control of the speed, position or torque you will need to adapt the provided KMS reference design to your needs. 

I recommend you start by using the Motion Sequence Builder to create a plan and variables that resemble your application. Examples provided include a blender, a ceiling fan and a washing machine for speed control and a security camera panning example for position control.

After completing the KMS GUI motor commissioning and experimenting with your motion plan, you will want to drive your motor within your application without the GUI and outside the bounds of the motion plan.  Now to create your own control system, to commanded speed, torque or position driven from another source. 

This document covers the basics of performing those commands. In these example you will be interacting with the User block.  The User block aggregates the main motor operating states capable of being called by the user. This is intended to provide an easy, consistent code interface for the user to be able to command a specific operating state and perform certain common configurations of the desired state.

Knowing the State of the Motor:

Before you tell the motor to go-to-speed, or go-to-position, or command a torque, you need to know that the motor is ready to accept your command.  I strongly suggest you read Chapter 2 of the KMS API reference manual.  Some of the text below is copied from there.

States that can be accessed via the User block include:

• Idle
• Fault
• Self-commissioning (SCM)
• Inertia Estimation (Inertia)
• PWM Duty Control
• Voltage Control
• Current Control (Current)
• Speed Control (Speed)
• Position Control (Position) [Sensored Position]
• Motion Sequence (Plan)
• Braking (Brake)
• Encoder Alignment (Align) [Sensored Velocity or Sensored Position]

Once in a state you can update the speed or position or torque directly.  Prior to entering the user state that commands speed, position or torque you need to take the input and covert it to the proper units.

Where to put your Control Code:

In the main loop, the code snippet below checks the state of the DC bus and the Fault indicator.

/* DC BUS Charging Indicator */

  if(DRV_getDcbBusVoltage()<DRV_getDcbBusUnderVoltageThreshold()||DRV_getIsFaultActive())

        {

            BUS_VOLTAGE_LEDOFF;

        }

        else

        {

            BUS_VOLTAGE_LEDON;

        }

        /* FAULT Indicator */

        if (DRV_getIsFaultActive()) // Pulled Up Switch is Active Low

        {

            FAULT_LEDON;

        }

        else

        {

            FAULT_LEDOFF;

        }                    

You can put your control code in this loop or better yet you can put your control code in the interrupt service routine that executes at the slow tick rate.

/**

 * @fn      SWI_IRQHandler(void)

 * @brief   Run slow isr task

 *

 * @return  None

 * @ingroup SLOW_ISR

 */

void SWI_IRQHandler(void)

{

    static timestamp_t SlowIsrStartLast = 0;

    /*!< Previous Start of ISR timestamp for measuring period  */

    timestamp_t startCycleCount = GetProfilerCycles();

    CpuUtilization.SlowIsrPeriod = startCycleCount -SlowIsrStartLast;

    SlowIsrStartLast = startCycleCount;

    /* Clear source of ISR */

    NVIC_ClearPendingIRQ(SWI_IRQn);

    /* Update drive module */

    DRV_updateSlowTick();

    /* Diagnostic counter */

    slowTicks++;;‍‍‍‍‍‍

    /* insert control code here  for example test for state*/

    /*LED1 to indicate motor in Plan mode */

    if(user.state == USER_RUN_PLAN)

    {

        LED4_ON;

    }

    else

    {

        LED4_OFF;

    }

Conversion:

The conveyor example below has two inputs derived from the ADC result register. In the next code listing, the inputs values are scaled and converted to LQ format. These controls are demonstrated in the conveyor belt demonstration shown in the video Kinetis Motor Control Made Easy. The two potentiometers shown in the picture are fed into an ADC channel. 

 

   /*poll external outputs*/

    /*GPIO have HW enabled anti-glitch and Pullup*/

    /*1 = on 0 = off */

    userPlanVariables.On_Off=!((bool)GPIO_DRV_ReadPinInput(kGpioOnOff));

    /*high = 1, low = 0 */

    userPlanVariables.Low_High=(bool)GPIO_DRV_ReadPinInput(kGpioHighLow);

    /*in High speed operation need slightly higher bandwidth */

    if(userPlanVariables.Low_High == 0)

    {

        userPlanVariables.Bandwidth_High = _LQ20(70.0);

        userPlanVariables.Bandwidth_Low = _LQ20(10.0);

    }

    else

    {

        userPlanVariables.Bandwidth_High = _LQ20(100.0);

        userPlanVariables.Bandwidth_Low = _LQ20(15.0);

    }

    /*Value Scaled to 0-1 in LQ Format Q12->GlobalLQ*/
 userPlanVariables.Bandwidth_Scale=_LQXtoLQY(adc0Results[0],12,GLOBAL_LQ);
 position.config.lq20Bw_radps = _LQ20mpyLQX( userPlanVariables.Bandwidth_High - userPlanVariables.Bandwidth_Low , 20U, userPlanVariables.Bandwidth_Scale ,GLOBAL_LQ) \
 + userPlanVariables.Bandwidth_Low;
/*Value Scaled to 0-1 in LQ Format with light exponential weighting .5821*(e^x-1)*/
 userPlanVariables.Jerk_Scale=_LQmpy(_LQXtoLQY(_LQ20exp(_LQXtoLQY(adc1Results[0],12,20U)),20,GLOBAL_LQ)-_LQ(1.0),\
 _LQ(0.581992));‍‍‍‍‍‍‍

You cannot just set the target speed like this using and integer value for RPM.  

user.command.targetSpeed = 100;

The KMS code uses QMath in the calculations. Conversion of a float or integer to a LQ type is easy to do using the API calls. Check out Chapter 24 Math for the details on the QMath library.

In KMS all values are normalized fixed-point.  So setting user.command.targetSpeed to 100 sets the target speed to a very small value (<< 1RPM).

You need to convert from RPM into the normalized fixed point used by KMS. 

user.command.targetSpeed = _LQ(100.0 / FULL_SCALE_SPEED_RPM);

In the above line, FULL_SCALE_SPEED_RPM represents the maximum possible speed in the system.  Dividing the input speed of 100.0 by this value will convert your input into a normalized value.  Since KMS is also fixed point, we need to convert this normalized floating-point number to fixed point.  The macro _LQ will convert from floating point into fixed point at the default Qness.  The variable user.command.targetSpeed is defined as an _lq variable.  This means it is a 32-bit container with a Qness of 24 (24 bits after the decimal point).  By default all _lq variables default to Q24 unless otherwise noted in the variable name.  

Commanding Speed:

In the KMS_API RM, the user block is defined. The user.state definitions are in the user.h file.

If you want to idle the motor you would write:

user.state= USER_IDLE;

To set the user state to run speed you would write:

user.state= USER_RUN_SPEED;

Here is a list of all the USER_States for speed control.

/**
 * @enum USER_state_e
 * @brief USER state-machine states
 */
typedef enum
{
 USER_IDLE = 0, /**< 0: USER IDLE state */
 USER_FAULT = 1, /**< 1: USER FAULT state */
 USER_SCM = 2, /**< 2: USER self-commissioning state */
 USER_INERTIA = 3, /**< 3: USER inertia estimation state */
 USER_RUN_DUTY = 4, /**< 4: DSM run PWM duty control state */
 USER_RUN_VOLTAGE = 5, /**< 5: DSM run voltage control state */
 USER_RUN_CURRENT = 6, /**< 6: USER run current control state */
 USER_RUN_SPEED = 7, /**< 7: USER run speed control state */
 USER_RUN_PLAN = 8, /**< 8: USER run a motion plan state */
 USER_BRAKE = 9, /**< 9: USER braking state */
} USER_state_e;‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

If you use the feedback_3ph.c as an example piece of code, the raw ADC reading is converted to a _sq with the use of the function (_sq): tempSQ = (_sq)(adcResults->phaseBCurrent - v->calib.offsetIb);

Likewise, the conversion of the RAW ADC result to LQ is accomplished using the (_lq) function.

In the following code example an ADC input is used to control the speed. The code below, takes the ADC results and converts it to an _lq type by means of the following statement:

adc0Results[0] = (_lq) adc0RawResults[0];

/* code start */
// declare these as global variables in main.c
_lq adcSpeed = 0;
_lq adcSpeedAccum = 0;
_lq adcSpeedAvg = 0;
uint16_t speedUpdateCounter = 0;
 
// this code takes the ADC reading, downshifts it by 4 to remove noise and uses that as a percentage of the maximum applicaton speed (in this case 20krpm)
adcSpeed = _LQmpyLQX((adc0Results[0] >> 4), 8, _LQ(20000.0/FULL_SCALE_SPEED_RPM), 24);
adcSpeed = _LQsat(adcSpeed, _LQ(1.0), _LQ(0.0));
 
// this code averages the adc reading over 125 samples before setting it to user.command.targetSpeed
// it will also handle setting the control mode if the commanded speed is larger than 0
if(adcSpeed > _LQ(0.0))
{
                user.state= USER_RUN_SPEED;
                if(speedUpdateCounter >= 125)
                {
                     speedUpdateCounter = 0;
                     adcSpeedAvg=(adcSpeedAccum/125)&0x00FF0000;
                     user.command.targetSpeed = adcSpeedAvg;
                     adcSpeedAccum = 0;
                }
                else
                {
                     adcSpeedAccum = adcSpeed + adcSpeedAccum;
                     speedUpdateCounter++;
                }
}
else
{
        user.state= USER_IDLE;
        adcSpeedAccum = 0;
        adcSpeedAvg = 0;
        speedUpdateCounter = 0;
}‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

Setting command Values:

Before you change a user.state there are parameters like acceleration and jerk you can modify. Prior to change the state you can update any number of other command values list in the function below. The list gives a quick look at the parameters you can update prior to changing the motor state. Each of the command values listed can be updated prior to this call. As you can see, setting the speed value in the line containing command.targetSpeed is just one of five values sent to the drive command.

After setting these parameters, you command a state change to USER_RUN_SPEED

user.state= USER_RUN_SPEED;

The user state “USER_RUN_SPEED” executes this function which start the motor on the new trajectory.

/**
 * @fn USER_runSpeedState(USER_t *pUSER, SPEED_t *speed, DRV_control_e controlType, \
 * TRF_rVector16_t startupStatorRefCurrent, _sq fwIdRef)
 * @brief Runs the Speed Control USER state
 * @param pUSER - pointer to the USER block
 * @return none
 */
static void USER_runSpeedState(USER_t *pUSER)
{
 /* For trajectory, just send command values to DRV*/
 pUSER->output.drvCommand.targetSpeed = pUSER->command.targetSpeed;
 pUSER->output.drvCommand.limitAcc = pUSER->command.limitAcc;
 pUSER->output.drvCommand.lq20LimitJerk = pUSER->command.lq20LimitJerk;
 pUSER->output.drvCommand.IqRefMax = pUSER->command.IqRefMax;
 pUSER->output.drvCommand.IqRefMin = pUSER->command.IqRefMin;
/* Run trajectory generator in this state */
 pUSER->output.drvCommand.runTrajectory = true;
return;
} /* end of USER_runSpeedState() */‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

Commanding Torque:

If you wanted to command torque (current), then you would initialize the command values identified in the code below. In addition to command .statorRefCurrent, you can specify a command.targetSpeed. 

/**
 * @fn USER_runCurrentState(USER_t *pUSER)
 * @brief Runs the Current control USER state
 * @param pUSER - pointer to the USER block
 * @return none
 */
static void USER_runCurrentState(USER_t *pUSER)
{
 pUSER->output.drvCommand.targetSpeed = pUSER->command.targetSpeed;
 pUSER->output.drvCommand.limitAcc = pUSER->command.limitAcc;
 pUSER->output.drvCommand.lq20LimitJerk = pUSER->command.lq20LimitJerk;
 pUSER->output.drvCommand.statorRefCurrent = pUSER->command.statorRefCurrent;
 pUSER->output.drvCommand.enableDCInjection = pUSER->command.enableDCInjection;
return;
} /* end of USER_runCurrentState() */‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

Commanding Position:

When using KMS to control a position, the call to the following function is made.  The motor position control follows a trajectory that you choose prior to the call by setting the command values to the motor drive state machine. In the position control reference project include file "user.h" there are two more USER state-machine states added to the list of USER_STATES -> USER_RUN_POSITION and USER_ALIGN.

/**
 * @enum    USER_state_e
 * @brief   USER state-machine states
 */
typedef enum
{
    USER_IDLE         = 0, /**< 0: USER IDLE state */
    USER_FAULT        = 1, /**< 1: USER FAULT state */
    USER_SCM          = 2, /**< 2: USER self-commissioning state */
    USER_INERTIA      = 3, /**< 3: USER inertia estimation state */
    USER_RUN_DUTY     = 4, /**< 4: DSM run PWM duty control state */
    USER_RUN_VOLTAGE  = 5, /**< 5: DSM run voltage control state */
    USER_RUN_CURRENT  = 6, /**< 6: USER run current control state */
    USER_RUN_SPEED    = 7, /**< 7: USER run position-controlled speed state */
    USER_RUN_PLAN     = 8, /**< 8: USER run a motion plan state */
    USER_BRAKE        = 9, /**< 9: USER braking state */
    USER_RUN_POSITION = 10, /**< 10: USER run position control state */
    USER_ALIGN        = 11  /**< 11: USER force motor into alignment */
} USER_state_e;
‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

To command a position, set the command values and then set the state to USER_RUN_POSITION

user.state= USER_RUN_POSITION;

This will result in a trajectory to be executed. 

/**
 * @fn USER runSpeedState(USER_t *pUSER,SPEED_t *speed,DRV_control_e controlType,
 * TRF_rVector16_t startupStatorRefCurrent, _sq fwIdRef)
 * @brief Runs the Speed Control USER state
 * @param pUSER - pointer to the USER block
 * @return none
 */
static void USER_runPositionState(USER_t *pUSER)
{
 /* For trajectory, just send command values to DRV*/
 pUSER->output.drvCommand.runTrajectory = pUSER->command.runTrajectory;
 pUSER->output.drvCommand.posStepInt_mrev = pUSER->command.posStepInt_mrev;
 pUSER->output.drvCommand.posStepFrac_mrev = pUSER->command.posStepFrac_mrev;
 pUSER->output.drvCommand.limitVel = pUSER->command.limitVel;
 pUSER->output.drvCommand.limitAcc = pUSER->command.limitAcc;
 pUSER->output.drvCommand.limitDec = pUSER->command.limitDec;
 pUSER->output.drvCommand.lq20LimitJerk = pUSER->command.lq20LimitJerk;
 pUSER->output.drvCommand.IqRefMax = pUSER->command.IqRefMax;
 pUSER->output.drvCommand.IqRefMin = pUSER->command.IqRefMin;
/* Run trajectory generator in position mode */
 pUSER->output.drvCommand.velocityMode = false;
return;
} /* end of USER_runSpeedState() */‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

Below is a bare metal code example used to drive the X or Y axis of a position controller.  The UART receives the command and a sync pulse on a GPIO starts the trajectory profile. This is not very robust code with very little command format checking.  I encourage you to consider the limit conditions and correctness of the input command values before using them to command the motor position.  This code tends to occasionally miss-fire and send the 3D printer base flying to the end stop.

user.state = USER_RUN_POSITION;
while (true)
{
// LED2 GREEN is DC BUS Charging Indicator, disable if fault
if (DRV_getDcbBusVoltage() < DRV_getDcbBusUnderVoltageThreshold() || DRV_getIsFaultActive()) LED2_OFF; else LED2_ON;

// Expected format: xAAAAAAAABBBBBBBBX
// Wait for initial 'x' or 'y', remember to set this in axis variable
GPIOC_PSOR=(1<<3); //set buffer ready
do {cin=UART1_CharIn();} while (cin!=axis);
// Read AAAAAAAA (dx as a float)
mem=(byte *)&f1; for (i=0;i<4;i++) {*mem=UART1_ByteIn(); mem++;}
// Read BBBBBBBB (dy as a float)
mem=(byte *)&f2; for (i=0;i<4;i++) {*mem=UART1_ByteIn(); mem++;}
// Wait for final 'X' or 'Y': if not an 'X' or 'Y', ignore message
cin=UART1_CharIn();
if (cin==axis-32)
 {
 GPIOC_PCOR=(1<<3);
 UART_CharOut(axis);
 // Valid Message, go ahead and execute
 // ROTATION: extract integer part of rotation
 value=(int)f1;
 user.command.posStepInt_mrev=value;
 //UART_TextOut("IntRotation=");
 UART_DbleOut(value,4,1);
 // ROTATION: extract fractional part of rotation
 user.command.posStepFrac_mrev=_LQ(f1-value);
 //UART_TextOut("FracRotation=");
 UART_DbleOut(f1-value,4,4);
 // SPEED: extract integer part of rotation
 f2=f2/4000.0;
 user.command.limitVel=_LQ(f2);
 //UART_TextOut("SpeedRPM=");
 UART_DbleOut(f2,4,4);
 GPIOC_PSOR=(1<<6);
 // Wait for Sync
 while (((GPIOE_PDIR&(1<<6))==0));
 GPIOC_PCOR=(1<<6);
 flag=0; done=1; user.command.runTrajectory=1;
 while (done==1) {UART_CharOut('1');}
 GPIOC_PSOR=(1<<6);
 }
}//while (true)‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍‍

Conclusion:

Four steps are needed:

1. Check the existing state of the motor, managing faults and trajectories.
2. Convert the commanded inputs to the fixed-point values needed by KMS.
3. Set the configuration parameters, like acceleration, jerk and speed.
4. Change the user state to the desired state, Speed, Torque, Position.

Philip Drake

Systems Applications Engineer