speedcalc.h

Go to the documentation of this file.

//#############################################################################
// $TI Release: MotorControl SDK v1.00.00.00 $
// $Release Date: Mon Mar 11 18:37:40 CDT 2019 $
// $Copyright:
// Copyright (C) 2017-2018 Texas Instruments Incorporated - http://www.ti.com/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//   Redistributions of source code must retain the above copyright
//   notice, this list of conditions and the following disclaimer.
//
//   Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the
//   distribution.
//
//   Neither the name of Texas Instruments Incorporated nor the names of
//   its contributors may be used to endorse or promote products derived
//   from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
// $
//#############################################################################
 
#ifndef SPEED_CALC_H
#define SPEED_CALC_H
 
 
//*****************************************************************************
//
// If building with a C++ compiler, make all of the definitions in this header
// have a C binding.
//
//*****************************************************************************
#ifdef __cplusplus
extern "C"
{
#endif
 
//*****************************************************************************
//
//
//*****************************************************************************
 
// the includes
#ifdef __TMS320C28XX_CLA__
#include "libraries/math/src/float/CLAmath.h"
#else
#include <math.h>
#endif
 
#include "libraries/math/include/math.h"
 
//modules
#include "userParams.h"
 
 
typedef struct _SPDCALC_obj_
{
    float32_t  ref;             // Input: reference set-point
    float32_t  fbk;             // Input: feedback
    float32_t  err;             // Error
    float32_t  out;             // Output: controller output
    float32_t  Kp;              // Parameter: proportional loop gain
    float32_t  Ki;              // Parameter: integral gain
    float32_t  Umax;            // Parameter: upper saturation limit
    float32_t  Umin;            // Parameter: lower saturation limit
    float32_t  Up;              // Data: proportional term
    float32_t  Ui;              // Data: integral term
    float32_t  speed_Hz;        // Output freq
    float32_t  thetaDelta;      // Parameter: theta maximum
} SPDCALC_Obj;
 
typedef struct _SPDCALC_obj_ *SPDCALC_Handle;
 
// ***************************************
// extern functions
// ***************************************
SPDCALC_Handle SPDCALC_init(void *pMemory, const size_t numBytes);
 
void SPDCALC_reset(SPDCALC_Handle handle);
 
void SPDCALC_setParams(SPDCALC_Handle handle, const USER_Params *pUserParams);
 
static inline float32_t SPDCALC_getSpeedHz(SPDCALC_Handle handle)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;
 
    return(obj->speed_Hz);
}
 
static inline void SPDCALC_run(SPDCALC_Handle handle, float32_t theta)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;
 
    obj->ref  = theta;
 
    // error cal
    obj->err = obj->ref - obj->fbk;
 
    // roll in the error
    if(obj->err >= MATH_PI)
    {
        obj->err = obj->err - MATH_TWO_PI;
    }
    else if(obj->err <= -MATH_PI)
    {
        obj->err = obj->err + MATH_TWO_PI;
    }
 
    // P and I control
    obj->Up  = obj->Kp * obj->err;              // P control
    obj->Ui += obj->Ki * obj->err;              // I control
    obj->Ui  = __fsat(obj->Ui, obj->Umax, obj->Umin);
 
    // control output
    obj->out = obj->Up + obj->Ui;
    obj->out = __fsat(obj->out, obj->Umax, obj->Umin);      // rad/s
 
    obj->speed_Hz = obj->out * MATH_ONE_OVER_TWO_PI;
 
    // Latest angle feedback estimation --> ( Fbk = integral of speed )
    obj->fbk = obj->fbk + obj->out * obj->thetaDelta;
 
    // roll "Fbk" within -pi to pi
    if(obj->fbk >= MATH_PI)
    {
        obj->fbk = obj->fbk - MATH_TWO_PI;
    }
    else if(obj->fbk <= -MATH_PI)
    {
        obj->fbk = obj->fbk + MATH_TWO_PI;
    }
 
    return;
}
 
//*****************************************************************************
//
// Close the Doxygen group.
//
//*****************************************************************************
 
//*****************************************************************************
//
// Mark the end of the C bindings section for C++ compilers.
//
//*****************************************************************************
#ifdef __cplusplus
}
#endif
 
#endif //end of SPEED_CALC_H definition