Lab 5a using DRV8312-69M-kit

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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.
 * --/COPYRIGHT--*/

 
 
//! \file   solutions/instaspin_motion/src/ctrl.c
//! \brief  Contains the various functions related to the controller (CTRL) object
//!
//! (C) Copyright 2011, Texas Instruments, Inc.


// **************************************************************************
// the includes

#include <math.h>


// drivers


// modules
#include "sw/modules/dlog/src/32b/dlog4ch.h"
#include "sw/modules/math/src/32b/math.h"


// platforms
#include "ctrl.h"
#include "drv.h"
#include "user.h"


#ifdef FLASH
#pragma CODE_SECTION(CTRL_run,"ramfuncs");
#pragma CODE_SECTION(CTRL_setup,"ramfuncs");
#endif


// **************************************************************************
// the defines


// **************************************************************************
// the globals


// **************************************************************************
// the function prototypes

void CTRL_getGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   _iq *pKp,_iq *pKi,_iq *pKd)
{

  *pKp = CTRL_getKp(handle,ctrlType);
  *pKi = CTRL_getKi(handle,ctrlType);
  *pKd = CTRL_getKd(handle,ctrlType);

  return;    
} // end of CTRL_getGains() function


void CTRL_getIab_filt_pu(CTRL_Handle handle,MATH_vec2 *pIab_filt_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_filt_pu->value[0] = obj->Iab_filt.value[0];
  pIab_filt_pu->value[1] = obj->Iab_filt.value[1];

  return;
} // end of CTRL_getIab_filt_pu() function


void CTRL_getIab_in_pu(CTRL_Handle handle,MATH_vec2 *pIab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_in_pu->value[0] = obj->Iab_in.value[0];
  pIab_in_pu->value[1] = obj->Iab_in.value[1];

  return;
} // end of CTRL_getIab_in_pu() function


void CTRL_getIdq_in_pu(CTRL_Handle handle,MATH_vec2 *pIdq_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_in_pu->value[0] = obj->Idq_in.value[0];
  pIdq_in_pu->value[1] = obj->Idq_in.value[1];

  return;
} // end of CTRL_getIdq_in_pu() function


void CTRL_getIdq_ref_pu(CTRL_Handle handle,MATH_vec2 *pIdq_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_ref_pu->value[0] = obj->Idq_ref.value[0];
  pIdq_ref_pu->value[1] = obj->Idq_ref.value[1];

  return;
} // end of CTRL_getIdq_ref_pu() function


_iq CTRL_getMagCurrent_pu(CTRL_Handle handle)
{

  return(CTRL_getIdRated_pu(handle));
} // end of CTRL_getMagCurrent_pu() function


_iq CTRL_getMaximumSpeed_pu(CTRL_Handle handle)
{

  return(CTRL_getSpd_max_pu(handle));
} // end of CTRL_getMaximumSpeed_pu() function


void CTRL_getVab_in_pu(CTRL_Handle handle,MATH_vec2 *pVab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_in_pu->value[0] = obj->Vab_in.value[0];
  pVab_in_pu->value[1] = obj->Vab_in.value[1];

  return;
} // end of CTRL_getVab_in_pu() function


void CTRL_getVab_out_pu(CTRL_Handle handle,MATH_vec2 *pVab_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_out_pu->value[0] = obj->Vab_out.value[0];
  pVab_out_pu->value[1] = obj->Vab_out.value[1];

  return;
} // end of CTRL_getVab_out_pu() function


void CTRL_getVdq_out_pu(CTRL_Handle handle,MATH_vec2 *pVdq_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVdq_out_pu->value[0] = obj->Vdq_out.value[0];
  pVdq_out_pu->value[1] = obj->Vdq_out.value[1];

  return;
} // end of CTRL_getVdq_out_pu() function


void CTRL_getWaitTimes(CTRL_Handle handle,uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      pWaitTimes[stateCnt] = obj->waitTimes[stateCnt];
    }

  return;
} // end of CTRL_getWaitTimes() function


void CTRL_run(CTRL_Handle handle,DRV_Handle drvHandle,
              const DRV_AdcData_t *pAdcData,
              DRV_PwmData_t *pPwmData)
{
  uint_least16_t count_isr = CTRL_getCount_isr(handle);
  uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle);


  // if needed, run the controller
  if(count_isr >= numIsrTicksPerCtrlTick)
    {
      CTRL_State_e ctrlState = CTRL_getState(handle);

      // reset the isr count
      CTRL_resetCounter_isr(handle);

      // increment the state counter
      CTRL_incrCounter_state(handle);

      // increment the trajectory count
      CTRL_incrCounter_traj(handle);

      // run the appropriate controller
      if(ctrlState == CTRL_State_OnLine)
        {
    	  CTRL_Obj *obj = (CTRL_Obj *)handle;

          // increment the current count
          CTRL_incrCounter_current(handle);

          // increment the speed count
          CTRL_incrCounter_speed(handle);

          if(EST_getState(obj->estHandle) >= EST_State_MotorIdentified)
            {
              // run the online controller
              CTRL_runOnLine_User(handle,pAdcData,pPwmData);
            }
          else
            {
              // run the online controller
              CTRL_runOnLine(handle,pAdcData,pPwmData);
            }
        }
      else if(ctrlState == CTRL_State_OffLine)
        {
          // run the offline controller
          CTRL_runOffLine(handle,drvHandle,pAdcData,pPwmData);
        }
    }
  else
    {
      // increment the isr count
      CTRL_incrCounter_isr(handle);
    }

  return;
} // end of CTRL_run() function


void CTRL_setGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   const _iq Kp,const _iq Ki,const _iq Kd)
{

  CTRL_setKp(handle,ctrlType,Kp);
  CTRL_setKi(handle,ctrlType,Ki);
  CTRL_setKd(handle,ctrlType,Kd);

  return;    
} // end of CTRL_setGains() function


void CTRL_setMagCurrent_pu(CTRL_Handle handle,const _iq magCurrent_pu)
{

  CTRL_setIdRated_pu(handle,magCurrent_pu);

  return;    
} // end of CTRL_setMagCurrent_pu() function


void CTRL_setMaximumSpeed_pu(CTRL_Handle handle,const _iq maxSpeed_pu)
{

  CTRL_setSpd_max_pu(handle,maxSpeed_pu);

  return;    
} // end of CTRL_setMaximumSpeed_pu() function


void CTRL_setParams(CTRL_Handle handle,USER_Params *pUserParams)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq Kp,Ki,Kd;
  _iq outMin,outMax;
  _iq maxModulation;

  MATH_vec2 Iab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_ref_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_in_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vdq_out_pu = {_IQ(0.0),_IQ(0.0)};


  // assign the motor type
  CTRL_setMotorParams(handle,pUserParams->motor_type,
                      pUserParams->motor_numPolePairs,
                      pUserParams->motor_ratedFlux,
                      pUserParams->motor_Ls_d,
                      pUserParams->motor_Ls_q,
                      pUserParams->motor_Rr,
                      pUserParams->motor_Rs);


  // assign other controller parameters
  CTRL_setNumIsrTicksPerCtrlTick(handle,pUserParams->numIsrTicksPerCtrlTick);
  CTRL_setNumCtrlTicksPerCurrentTick(handle,pUserParams->numCtrlTicksPerCurrentTick);
  CTRL_setNumCtrlTicksPerSpeedTick(handle,pUserParams->numCtrlTicksPerSpeedTick);
  CTRL_setNumCtrlTicksPerTrajTick(handle,pUserParams->numCtrlTicksPerTrajTick);

  CTRL_setCtrlFreq_Hz(handle,pUserParams->ctrlFreq_Hz);
  CTRL_setTrajFreq_Hz(handle,pUserParams->trajFreq_Hz);
  CTRL_setTrajPeriod_sec(handle,_IQ(1.0/pUserParams->trajFreq_Hz));

  CTRL_setCtrlPeriod_sec(handle,pUserParams->ctrlPeriod_sec);

  CTRL_setMaxVsMag_pu(handle,_IQ(pUserParams->maxVsMag_pu));

  CTRL_setIab_in_pu(handle,&Iab_out_pu);
  CTRL_setIdq_in_pu(handle,&Idq_out_pu);
  CTRL_setIdq_ref_pu(handle,&Idq_ref_pu);

  CTRL_setIdRated_pu(handle,_IQ(pUserParams->IdRated/pUserParams->iqFullScaleCurrent_A));

  CTRL_setVab_in_pu(handle,&Vab_in_pu);
  CTRL_setVab_out_pu(handle,&Vab_out_pu);
  CTRL_setVdq_out_pu(handle,&Vdq_out_pu);

  CTRL_setSpd_out_pu(handle,_IQ(0.0));

  CTRL_setRhf(handle,0.0);
  CTRL_setLhf(handle,0.0);
  CTRL_setRoverL(handle,0.0);


  // reset the counters
  CTRL_resetCounter_current(handle);
  CTRL_resetCounter_isr(handle);
  CTRL_resetCounter_speed(handle);
  CTRL_resetCounter_state(handle);
  CTRL_resetCounter_traj(handle);


  // set the wait times for each state
  CTRL_setWaitTimes(handle,&pUserParams->ctrlWaitTime[0]);


  // set flags
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableCtrl(handle,false);
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableOffset(handle,true);
  CTRL_setFlag_enableSpeedCtrl(handle,true);
  CTRL_setFlag_enableUserMotorParams(handle,false);
  CTRL_setFlag_enableDcBusComp(handle,true);


  // initialize the controller error code
  CTRL_setErrorCode(handle,CTRL_ErrorCode_NoError);


  // set the default controller state
  CTRL_setState(handle,CTRL_State_Idle);


  // set the number of current sensors
  CTRL_setupClarke_I(handle,pUserParams->numCurrentSensors);


  // set the number of voltage sensors
  CTRL_setupClarke_V(handle,pUserParams->numVoltageSensors);


  // set the default Id PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Id,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Id,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Id,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp,Ki,Kd);


  // set the default the Iq PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Iq,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Iq,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Iq,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp,Ki,Kd);


  // set the default speed PID controller parameters
  Kp = _IQ(0.02*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A);
  Ki = _IQ(2.0*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A);
  Kd = _IQ(0.0);
  outMin = _IQ(-1.0);
  outMax = _IQ(1.0);

  PID_setGains(obj->pidHandle_spd,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_spd,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_spd,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_spd,Kp,Ki,Kd);


  // set the speed reference
  CTRL_setSpd_ref_pu(handle,_IQ(0.0));


  // set the default Id current trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_Id,_IQ(0.0));


  // set the default the speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_spd,_IQ(0.0));


  // set the default maximum speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxSlope(obj->trajHandle_spdMax,_IQ(0.0)); // not used

  
  // set the default estimator parameters
  CTRL_setEstParams(obj->estHandle,pUserParams);


  // set the maximum modulation for the SVGEN module
  maxModulation = SVGEN_4_OVER_3;
  SVGEN_setMaxModulation(obj->svgenHandle,maxModulation);

  return;
} // end of CTRL_setParams() function


void CTRL_setSpd_ref_pu(CTRL_Handle handle,const _iq spd_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_pu() function


void CTRL_setSpd_ref_krpm(CTRL_Handle handle,const _iq spd_ref_krpm)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq krpm_to_pu_sf = EST_get_krpm_to_pu_sf(obj->estHandle);

  _iq spd_ref_pu = _IQmpy(spd_ref_krpm,krpm_to_pu_sf);

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_krpm() function


void CTRL_setup(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  uint_least16_t count_traj = CTRL_getCount_traj(handle);
  uint_least16_t numCtrlTicksPerTrajTick = CTRL_getNumCtrlTicksPerTrajTick(handle);


  // as needed, update the trajectory
  if(count_traj >= numCtrlTicksPerTrajTick)
    {
      _iq intValue_Id = TRAJ_getIntValue(obj->trajHandle_Id);

      // reset the trajectory count
      CTRL_resetCounter_traj(handle);

      // run the trajectories
      CTRL_runTraj(handle);
    } // end of if(gFlag_traj) block

  return;
} // end of CTRL_setup() function


void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numCurrentSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else if(numCurrentSensors == 2)
    {
      alpha_sf = _IQ(1.0);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_I,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_I,numCurrentSensors);

  return;
} // end of CTRL_setupClarke_I() function


void CTRL_setupClarke_V(CTRL_Handle handle,uint_least8_t numVoltageSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numVoltageSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
 else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_V,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_V,numVoltageSensors);

  return;
} // end of CTRL_setupClarke_V() function


void CTRL_setWaitTimes(CTRL_Handle handle,const uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      obj->waitTimes[stateCnt] = pWaitTimes[stateCnt];
    }

  return;
} // end of CTRL_setWaitTimes() function


bool CTRL_updateState(CTRL_Handle handle)
{
  CTRL_State_e ctrlState = CTRL_getState(handle);
  bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
  bool stateChanged = false;


  if(flag_enableCtrl)
    {
      uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
      uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);


      // check for errors
      CTRL_checkForErrors(handle);


      if(counter_ctrlState >= waitTime)
        {
          // reset the counter
          CTRL_resetCounter_state(handle);


          if(ctrlState == CTRL_State_OnLine)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              _iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);

              // update the estimator state
              bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);

              if(flag_estStateChanged)
                {
                  // setup the controller
                  CTRL_setupCtrl(handle);

                  // setup the trajectory
                  CTRL_setupTraj(handle);
                }

              if(EST_isOnLine(obj->estHandle))
                {
                  // setup the estimator for online state
                  CTRL_setupEstOnLineState(handle);
                }

              if(EST_isLockRotor(obj->estHandle) || 
                 (EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
                {
                  // set the enable controller flag to false
                  CTRL_setFlag_enableCtrl(handle,false);

                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_Idle);
                }
            }
          else if(ctrlState == CTRL_State_OffLine)
            {
              // set the next controller state
              CTRL_setState(handle,CTRL_State_OnLine);
            }
          else if(ctrlState == CTRL_State_Idle)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              bool  flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);

              if(flag_enableUserMotorParams)
                {
                  // initialize the motor parameters using values from the user.h file
                  CTRL_setUserMotorParams(handle);
                }

              if(EST_isIdle(obj->estHandle))
                {
                  // setup the estimator for idle state
                  CTRL_setupEstIdleState(handle);

                  if(EST_isMotorIdentified(obj->estHandle))
                    {
                      if(CTRL_getFlag_enableOffset(handle))
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OffLine);
                        }
                      else
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OnLine);
                        }
                    }
                  else
                    {
                      // set the next controller state
                      CTRL_setState(handle,CTRL_State_OffLine);
                    }
                }
              else if(EST_isLockRotor(obj->estHandle))
                {
                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_OnLine);
                }
            }
        }  // if(counter_ctrlState >= waitTime) loop
    } 
  else
    {
      CTRL_Obj *obj = (CTRL_Obj *)handle;

      // set the next controller state
      CTRL_setState(handle,CTRL_State_Idle);

      // set the estimator to idle
      if(!EST_isLockRotor(obj->estHandle))
        {
          if(EST_isMotorIdentified(obj->estHandle))
            {
              EST_setIdle(obj->estHandle);
            }
          else
            {
              EST_setIdle_all(obj->estHandle);

              EST_setRs_pu(obj->estHandle,_IQ30(0.0));
            }
        }
    }


  // check to see if the state changed
  if(ctrlState != CTRL_getState(handle))
    {
      stateChanged = true;
    }

  return(stateChanged);
} // end of CTRL_updateState() function

// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, LineStream Technologies Incorporated
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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 names of Texas Instruments Incorporated, LineStream
 *    Technologies 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.
 * --/COPYRIGHT--*/
//! \file   solutions/instaspin_motion/src/proj_lab05f.c
//! \brief  Comparing performance of PI velocity controller & SpinTAC velocity controller
//!
//! (C) Copyright 2012, LineStream Technologies, Inc.
//! (C) Copyright 2011, Texas Instruments, Inc.

//! \defgroup PROJ_LAB05f PROJ_LAB05f
//@{

//! \defgroup PROJ_LAB05f_OVERVIEW Project Overview
//!
//! Comparing performance of PI velocity controller & SpinTAC velocity controller
//!

// **************************************************************************
// the includes

// system includes
#include <math.h>
#include "main.h"

#ifdef FLASH
#pragma CODE_SECTION(mainISR,"ramfuncs");
#endif

// Include header files used in the main function


// **************************************************************************
// the defines

#define LED_BLINK_FREQ_Hz   5


// **************************************************************************
// the globals

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

DRV_Handle drvHandle;

USER_Params gUserParams;

DRV_PwmData_t gPwmData;

DRV_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

#ifdef FAST_ROM_V1p6
CTRL_Obj *controller_obj;
#else
CTRL_Obj ctrl;				//v1p7 format
#endif

ST_Obj st_obj;
ST_Handle stHandle;

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;

// Graphing Variables
volatile bool Graph_Flag_Enable_update = false;
volatile unsigned int Graph_Counter = 0;
volatile unsigned short Graph_Tick_Counter = 0;
volatile unsigned short Graph_Tick_Counter_Value = 10;

#define GRAPH_SIZE 600        // Having more buffered points to show

//#define GRAPH_SIZE 300
volatile _iq Graph_Data[GRAPH_SIZE];
volatile _iq Graph_Data2[GRAPH_SIZE];
volatile _iq SpeedRef_krpm_previous = _IQ(0.0);

#ifdef FLASH
// Used for running BackGround in flash, and ISR in RAM
extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
#endif


#ifdef DRV8301_SPI
// Watch window interface to the 8301 SPI
DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
#endif


// **************************************************************************
// the functions

void main(void)
{
  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

  // Only used if running from FLASH
  // Note that the variable FLASH is defined by the project
  #ifdef FLASH
  // Copy time critical code and Flash setup code to RAM
  // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
  // symbols are created by the linker. Refer to the linker files.
  memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
  #endif

  uint_least8_t graphCounter = 0;

  // initialize the driver
  drvHandle = DRV_init(&drv,sizeof(drv));


  // initialize the user parameters
  USER_setParams(&gUserParams);


  // set the driver parameters
  DRV_setParams(drvHandle,&gUserParams);


  // initialize the controller
#ifdef FAST_ROM_V1p6
  ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber);  		//v1p6 format (06xF and 06xM devices)
  controller_obj = (CTRL_Obj *)ctrlHandle;
#else
  ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl));	//v1p7 format default
#endif


  // set the default controller parameters
  CTRL_setParams(ctrlHandle,&gUserParams);


  // setup faults
  DRV_setupFaults(drvHandle);


  // initialize the interrupt vector table
  DRV_initIntVectorTable(drvHandle);


  // enable the ADC interrupts
  DRV_enableAdcInts(drvHandle);


  // enable global interrupts
  DRV_enableGlobalInts(drvHandle);


  // enable debug interrupts
  DRV_enableDebugInt(drvHandle);


  // disable the PWM
  DRV_disablePwm(drvHandle);


  // enable DC bus compensation
  CTRL_setFlag_enableDcBusComp(ctrlHandle, true);


  // initialize the SpinTAC Components
  stHandle = ST_init(&st_obj, sizeof(st_obj));
  
  
  // setup the SpinTAC Components
  ST_setupVelCtl(stHandle);

  
  // initialize the Graph Variables
  for(graphCounter=0; graphCounter<GRAPH_SIZE; graphCounter++) {
	  Graph_Data[graphCounter] = 0;
	  Graph_Data2[graphCounter] = 0;
  }  


#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  DRV_enable8301(drvHandle);
  // initialize the DRV8301 interface
  DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars);
#endif


  for(;;)
  {
    // Waiting for enable system flag to be set
    while(!(gMotorVars.Flag_enableSys));

    Flag_Latch_softwareUpdate = true;

    // Dis-able the Library internal PI.  Iq has no reference now
    CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);

    // loop while the enable system flag is true
    while(gMotorVars.Flag_enableSys)
      {
        CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
        ST_Obj *stObj = (ST_Obj *)stHandle;

        // increment counters
        gCounter_updateGlobals++;

        // enable/disable the use of motor parameters being loaded from user.h
        CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);

        // enable/disable Rs recalibration during motor startup
        EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);

        // enable/disable automatic calculation of bias values
        CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);


        if(CTRL_isError(ctrlHandle))
          {
            // set the enable controller flag to false
            CTRL_setFlag_enableCtrl(ctrlHandle,false);

            // set the enable system flag to false
            gMotorVars.Flag_enableSys = false;

            // disable the PWM
            DRV_disablePwm(drvHandle);
          }
        else
          {
            // update the controller state
            bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);

            // enable or disable the control
            CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);

            if(flag_ctrlStateChanged)
              {
                CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);

                if(ctrlState == CTRL_State_OffLine)
                  {
                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_OnLine)
                  {
                    if(gMotorVars.Flag_enableOffsetcalc == true)
                    {
                      // update the ADC bias values
                      DRV_updateAdcBias(drvHandle);
                    }
                    else
                    {
                      // set the current bias
                      DRV_setBias(drvHandle,DRV_SensorType_Current,0,_IQ(I_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,1,_IQ(I_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,2,_IQ(I_C_offset));

                      // set the voltage bias
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,0,_IQ(V_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,1,_IQ(V_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,2,_IQ(V_C_offset));
                    }

                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_Idle)
                  {
                    // disable the PWM
                    DRV_disablePwm(drvHandle);
                    gMotorVars.Flag_Run_Identify = false;
                  }
              }
          }


        if(EST_isMotorIdentified(obj->estHandle))
          {
            // set the current ramp
            EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
            gMotorVars.Flag_MotorIdentified = true;

            // set the speed reference
            CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);

            // set the speed acceleration
            CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));

            // enable the SpinTAC Speed Controller
            STVELCTL_setEnable(stObj->velCtlHandle, true);

            if(EST_getState(obj->estHandle) != EST_State_OnLine)
            {
            	// if the estimator is not running, place SpinTAC into reset
            	STVELCTL_setEnable(stObj->velCtlHandle, false);
            }
			
			// select the SpinTAC Speed Controller (true) or the PI Speed Controller (false)
            CTRL_setFlag_enableSpeedCtrl(ctrlHandle, !gMotorVars.SpinTAC.VelCtlEnb);

            if(Flag_Latch_softwareUpdate)
            {
              Flag_Latch_softwareUpdate = false;

#ifdef FAST_ROM_V1p6
              if(CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true)
              {
                  // call this function to fix 1p6
                  softwareUpdate1p6(ctrlHandle);
              }
#endif

              calcPIgains(ctrlHandle);

              // initialize the watch window kp and ki current values with pre-calculated values
              gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
              gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);

              // initialize the watch window kp and ki values with pre-calculated values
              gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
              gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
			  
			  // initialize the watch window Bw value with the default value
              gMotorVars.SpinTAC.VelCtlBwScale = STVELCTL_getBandwidthScale(stObj->velCtlHandle);
            }
          }
        else
          {
            Flag_Latch_softwareUpdate = true;

            // the estimator sets the maximum current slope during identification
            gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
          }


        // when appropriate, update the global variables
        if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
          {
            // reset the counter
            gCounter_updateGlobals = 0;

            updateGlobalVariables_motor(ctrlHandle, stHandle);
          }

      } // end of while(gFlag_enableSys) loop


    // disable the PWM
    DRV_disablePwm(drvHandle);

    // set the default controller parameters (Reset the control to re-identify the motor)
    CTRL_setParams(ctrlHandle,&gUserParams);

    // setup the SpinTAC Components
    ST_setupVelCtl(stHandle);

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{

  static uint16_t stCnt = 0;

  // DPM - toggle status LED
  if(gLEDcnt++ > (uint_least32_t)(USER_PWM_FREQ_kHz * 1000 / LED_BLINK_FREQ_Hz)) {
    DRV_toggleLed(drvHandle,(GPIO_Number_e)DRV_Gpio_LED2);
    gLEDcnt = 0;
  }


  // acknowledge the ADC interrupt
  DRV_acqAdcInt(drvHandle,ADC_IntNumber_1);


  // convert the ADC data
  DRV_readAdcData(drvHandle,&gAdcData);


  // Run the SpinTAC Components
  if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
	  ST_runVelCtl(stHandle, ctrlHandle);
	  stCnt = 1;
  }


  // run the controller
  CTRL_run(ctrlHandle,drvHandle,&gAdcData,&gPwmData);


  // write the PWM compare values
  DRV_writePwmData(drvHandle,&gPwmData);

  if (!(SpeedRef_krpm_previous == gMotorVars.SpeedRef_krpm)) {
  	  Graph_Flag_Enable_update = true;
  	  Graph_Counter = 0;
  	  SpeedRef_krpm_previous = gMotorVars.SpeedRef_krpm;
  }

  gMotorVars.Ia = CTRL_getIa_in_pu(ctrlHandle);

  //gMotorVars.Iq_A = _IQmpy(CTRL_getIq_in_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));

  // store graph value
  if(Graph_Flag_Enable_update) {
  	  if(Graph_Counter >= GRAPH_SIZE) {
  		  Graph_Flag_Enable_update = false;
  	  }
  	  else {
  		  if(Graph_Tick_Counter == 0) {
  		      Graph_Data[Graph_Counter] = gMotorVars.Speed_krpm;

 		      Graph_Data2[Graph_Counter] = gMotorVars.Ia;

 //		  Graph_Data2[Graph_Counter] = gMotorVars.Iq_A;
  		      Graph_Counter++;
  		      Graph_Tick_Counter = Graph_Tick_Counter_Value - 1;
  		  }
  		  else {
  			  Graph_Tick_Counter--;
  		  }
  	  }
   }


  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle sthandle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  ST_Obj *stObj = (ST_Obj *)sthandle;


  // get the speed estimate
  gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);

  // get the torque estimate
  gMotorVars.Torque_lbin = EST_getTorque_lbin(obj->estHandle);

  // get the magnetizing current
  gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);

  // get the rotor resistance
  gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);

  // get the stator resistance
  gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);

  // get the stator inductance in the direct coordinate direction
  gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);

  // get the stator inductance in the quadrature coordinate direction
  gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);

  // get the flux
  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);

  // get the controller state
  gMotorVars.CtrlState = CTRL_getState(handle);

  // get the estimator state
  gMotorVars.EstState = EST_getState(obj->estHandle);
  
  if((gMotorVars.CtrlState == CTRL_State_OnLine) && (gMotorVars.Flag_MotorIdentified == true) && (Flag_Latch_softwareUpdate == false))
    {
      // set the kp and ki speed values from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_spd,gMotorVars.Kp_spd);
      CTRL_setKi(handle,CTRL_Type_PID_spd,gMotorVars.Ki_spd);

      // set the kp and ki current values for Id and Iq from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_Id,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Id,gMotorVars.Ki_Idq);
      CTRL_setKp(handle,CTRL_Type_PID_Iq,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Iq,gMotorVars.Ki_Idq);
	}

  // gets the Velocity Controller status
  gMotorVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle);
	
  // get the inertia setting
  gMotorVars.SpinTAC.Inertia_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // get the friction setting
  gMotorVars.SpinTAC.Friction_Aperkrpm = _IQmpy(STVELCTL_getFriction(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // set the SpinTAC (ST) bandwidth scale
  STVELCTL_setBandwidthScale(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBwScale);

  // gets the SpinTAC (ST) bandwidth
  gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);

  // get the Velocity Controller error
  gMotorVars.SpinTAC.VelCtlErrorID = STVELCTL_getErrorID(stObj->velCtlHandle);

  // enable/disable the forced angle
  EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);

  // enable or disable power warp
  CTRL_setFlag_enableEpl(handle,gMotorVars.Flag_enableEpl);

#ifdef DRV8301_SPI
  DRV_8301_SpiInterface(drvHandle,&gDrvSpi8301Vars);
#endif

  return;
} // end of updateGlobalVariables_motor() function

#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{

  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t fullScaleInductance = EST_getFullScaleInductance(obj->estHandle);
  float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
  int_least8_t lShift = ceil(log(obj->motorParams.Ls_d/(Ls_coarse_max*fullScaleInductance))/log(2.0));
  uint_least8_t Ls_qFmt = 30 - lShift;
  float_t L_max = fullScaleInductance * pow(2.0,lShift);
  _iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d / L_max);
  _iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q / L_max);


  // store the results
  EST_setLs_d_pu(obj->estHandle,Ls_d_pu);
  EST_setLs_q_pu(obj->estHandle,Ls_q_pu);
  EST_setLs_qFmt(obj->estHandle,Ls_qFmt);

  return;
} // end of softwareUpdate1p6() function
#endif

void calcPIgains(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t Ls_d = EST_getLs_d_H(obj->estHandle);
  float_t Ls_q = EST_getLs_q_H(obj->estHandle);
  float_t Rs = EST_getRs_Ohm(obj->estHandle);
  float_t RoverLs_d = Rs/Ls_d;
  float_t RoverLs_q = Rs/Ls_q;
  float_t fullScaleCurrent = EST_getFullScaleCurrent(obj->estHandle);
  float_t fullScaleVoltage = EST_getFullScaleVoltage(obj->estHandle);
  float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
  _iq Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);
  _iq Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);
  _iq Kd = _IQ(0.0);

  // set the Id controller gains
  PID_setKi(obj->pidHandle_Id,Ki_Id);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd);

  // set the Iq controller gains
  PID_setKi(obj->pidHandle_Iq,Ki_Iq);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd);

  return;
} // end of calcPIgains() function


void ST_runVelCtl(ST_Handle sthandle, CTRL_Handle handle)
{

    _iq speedFeedback, iqReference, gSpdError, gUp, gUi;
    ST_Obj *stObj = (ST_Obj *)sthandle;
    CTRL_Obj *ctrlObj = (CTRL_Obj *)handle;

    // Get the mechanical speed in pu
    speedFeedback = EST_getFm_pu(ctrlObj->estHandle);

	// Run the SpinTAC Controller
	// Note that the library internal ramp generator is used to set the speed reference
    STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
	STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0));	// Internal ramp generator does not provide Acceleration Reference
	STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
	STVELCTL_run(stObj->velCtlHandle);

	// select SpinTAC Velocity Controller
	iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);

	if(gMotorVars.SpinTAC.VelCtlEnb == true) {
		// Set the Iq reference that came out of SpinTAC Velocity Control
		CTRL_setIq_ref_pu(ctrlHandle, iqReference);
		// Update internal PI integrator, if running SpinTAC
		gSpdError = TRAJ_getIntValue(ctrlObj->trajHandle_spd) - speedFeedback;
		gUp = _IQmpy(gSpdError, CTRL_getKp(handle, CTRL_Type_PID_spd));
		gUi = _IQmpy(gSpdError, CTRL_getKi(handle, CTRL_Type_PID_spd));
		CTRL_setUi(handle, CTRL_Type_PID_spd, iqReference - gUp - gUi);
	}
}
//@} //defgroup
// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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.
 * --/COPYRIGHT--*/

 
 
//! \file   solutions/instaspin_foc/src/ctrl.c
//! \brief  Contains the various functions related to the controller (CTRL) object
//!
//! (C) Copyright 2011, Texas Instruments, Inc.


// **************************************************************************
// the includes

#include <math.h>


// drivers


// modules
#include "sw/modules/dlog/src/32b/dlog4ch.h"
#include "sw/modules/math/src/32b/math.h"


// platforms
#include "ctrl.h"
#include "hal.h"
#include "user.h"


#ifdef FLASH
#pragma CODE_SECTION(CTRL_run,"ramfuncs");
#pragma CODE_SECTION(CTRL_setup,"ramfuncs");
#endif


// **************************************************************************
// the defines


// **************************************************************************
// the globals


// **************************************************************************
// the function prototypes

void CTRL_getGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   _iq *pKp,_iq *pKi,_iq *pKd)
{

  *pKp = CTRL_getKp(handle,ctrlType);
  *pKi = CTRL_getKi(handle,ctrlType);
  *pKd = CTRL_getKd(handle,ctrlType);

  return;    
} // end of CTRL_getGains() function


void CTRL_getIab_filt_pu(CTRL_Handle handle,MATH_vec2 *pIab_filt_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_filt_pu->value[0] = obj->Iab_filt.value[0];
  pIab_filt_pu->value[1] = obj->Iab_filt.value[1];

  return;
} // end of CTRL_getIab_filt_pu() function


void CTRL_getIab_in_pu(CTRL_Handle handle,MATH_vec2 *pIab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_in_pu->value[0] = obj->Iab_in.value[0];
  pIab_in_pu->value[1] = obj->Iab_in.value[1];

  return;
} // end of CTRL_getIab_in_pu() function


void CTRL_getIdq_in_pu(CTRL_Handle handle,MATH_vec2 *pIdq_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_in_pu->value[0] = obj->Idq_in.value[0];
  pIdq_in_pu->value[1] = obj->Idq_in.value[1];

  return;
} // end of CTRL_getIdq_in_pu() function


void CTRL_getIdq_ref_pu(CTRL_Handle handle,MATH_vec2 *pIdq_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_ref_pu->value[0] = obj->Idq_ref.value[0];
  pIdq_ref_pu->value[1] = obj->Idq_ref.value[1];

  return;
} // end of CTRL_getIdq_ref_pu() function


_iq CTRL_getMagCurrent_pu(CTRL_Handle handle)
{

  return(CTRL_getIdRated_pu(handle));
} // end of CTRL_getMagCurrent_pu() function


_iq CTRL_getMaximumSpeed_pu(CTRL_Handle handle)
{

  return(CTRL_getSpd_max_pu(handle));
} // end of CTRL_getMaximumSpeed_pu() function


void CTRL_getVab_in_pu(CTRL_Handle handle,MATH_vec2 *pVab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_in_pu->value[0] = obj->Vab_in.value[0];
  pVab_in_pu->value[1] = obj->Vab_in.value[1];

  return;
} // end of CTRL_getVab_in_pu() function


void CTRL_getVab_out_pu(CTRL_Handle handle,MATH_vec2 *pVab_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_out_pu->value[0] = obj->Vab_out.value[0];
  pVab_out_pu->value[1] = obj->Vab_out.value[1];

  return;
} // end of CTRL_getVab_out_pu() function


void CTRL_getVdq_out_pu(CTRL_Handle handle,MATH_vec2 *pVdq_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVdq_out_pu->value[0] = obj->Vdq_out.value[0];
  pVdq_out_pu->value[1] = obj->Vdq_out.value[1];

  return;
} // end of CTRL_getVdq_out_pu() function


void CTRL_getWaitTimes(CTRL_Handle handle,uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      pWaitTimes[stateCnt] = obj->waitTimes[stateCnt];
    }

  return;
} // end of CTRL_getWaitTimes() function


void CTRL_run(CTRL_Handle handle,HAL_Handle halHandle,
              const HAL_AdcData_t *pAdcData,
              HAL_PwmData_t *pPwmData)
{
  uint_least16_t count_isr = CTRL_getCount_isr(handle);
  uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle);


  // if needed, run the controller
  if(count_isr >= numIsrTicksPerCtrlTick)
    {
      CTRL_State_e ctrlState = CTRL_getState(handle);

      // reset the isr count
      CTRL_resetCounter_isr(handle);

      // increment the state counter
      CTRL_incrCounter_state(handle);

      // increment the trajectory count
      CTRL_incrCounter_traj(handle);

      // run the appropriate controller
      if(ctrlState == CTRL_State_OnLine)
        {
    	  CTRL_Obj *obj = (CTRL_Obj *)handle;

          // increment the current count
          CTRL_incrCounter_current(handle);

          // increment the speed count
          CTRL_incrCounter_speed(handle);

          if(EST_getState(obj->estHandle) >= EST_State_MotorIdentified)
            {
              // run the online controller
              CTRL_runOnLine_User(handle,pAdcData,pPwmData);
            }
          else
            {
              // run the online controller
              CTRL_runOnLine(handle,pAdcData,pPwmData);
            }
        }
      else if(ctrlState == CTRL_State_OffLine)
        {
          // run the offline controller
          CTRL_runOffLine(handle,halHandle,pAdcData,pPwmData);
        }
    }
  else
    {
      // increment the isr count
      CTRL_incrCounter_isr(handle);
    }

  return;
} // end of CTRL_run() function


void CTRL_setGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   const _iq Kp,const _iq Ki,const _iq Kd)
{

  CTRL_setKp(handle,ctrlType,Kp);
  CTRL_setKi(handle,ctrlType,Ki);
  CTRL_setKd(handle,ctrlType,Kd);

  return;    
} // end of CTRL_setGains() function


void CTRL_setMagCurrent_pu(CTRL_Handle handle,const _iq magCurrent_pu)
{

  CTRL_setIdRated_pu(handle,magCurrent_pu);

  return;    
} // end of CTRL_setMagCurrent_pu() function


void CTRL_setMaximumSpeed_pu(CTRL_Handle handle,const _iq maxSpeed_pu)
{

  CTRL_setSpd_max_pu(handle,maxSpeed_pu);

  return;    
} // end of CTRL_setMaximumSpeed_pu() function


void CTRL_setParams(CTRL_Handle handle,USER_Params *pUserParams)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq Kp,Ki,Kd;
  _iq outMin,outMax;
  _iq maxModulation;

  MATH_vec2 Iab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_ref_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_in_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vdq_out_pu = {_IQ(0.0),_IQ(0.0)};


  // assign the motor type
  CTRL_setMotorParams(handle,pUserParams->motor_type,
                      pUserParams->motor_numPolePairs,
                      pUserParams->motor_ratedFlux,
                      pUserParams->motor_Ls_d,
                      pUserParams->motor_Ls_q,
                      pUserParams->motor_Rr,
                      pUserParams->motor_Rs);


  // assign other controller parameters
  CTRL_setNumIsrTicksPerCtrlTick(handle,pUserParams->numIsrTicksPerCtrlTick);
  CTRL_setNumCtrlTicksPerCurrentTick(handle,pUserParams->numCtrlTicksPerCurrentTick);
  CTRL_setNumCtrlTicksPerSpeedTick(handle,pUserParams->numCtrlTicksPerSpeedTick);
  CTRL_setNumCtrlTicksPerTrajTick(handle,pUserParams->numCtrlTicksPerTrajTick);

  CTRL_setCtrlFreq_Hz(handle,pUserParams->ctrlFreq_Hz);
  CTRL_setTrajFreq_Hz(handle,pUserParams->trajFreq_Hz);
  CTRL_setTrajPeriod_sec(handle,_IQ(1.0/pUserParams->trajFreq_Hz));

  CTRL_setCtrlPeriod_sec(handle,pUserParams->ctrlPeriod_sec);

  CTRL_setMaxVsMag_pu(handle,_IQ(pUserParams->maxVsMag_pu));

  CTRL_setIab_in_pu(handle,&Iab_out_pu);
  CTRL_setIdq_in_pu(handle,&Idq_out_pu);
  CTRL_setIdq_ref_pu(handle,&Idq_ref_pu);

  CTRL_setIdRated_pu(handle,_IQ(pUserParams->IdRated/pUserParams->iqFullScaleCurrent_A));

  CTRL_setVab_in_pu(handle,&Vab_in_pu);
  CTRL_setVab_out_pu(handle,&Vab_out_pu);
  CTRL_setVdq_out_pu(handle,&Vdq_out_pu);

  CTRL_setSpd_out_pu(handle,_IQ(0.0));

  CTRL_setRhf(handle,0.0);
  CTRL_setLhf(handle,0.0);
  CTRL_setRoverL(handle,0.0);


  // reset the counters
  CTRL_resetCounter_current(handle);
  CTRL_resetCounter_isr(handle);
  CTRL_resetCounter_speed(handle);
  CTRL_resetCounter_state(handle);
  CTRL_resetCounter_traj(handle);


  // set the wait times for each state
  CTRL_setWaitTimes(handle,&pUserParams->ctrlWaitTime[0]);


  // set flags
  CTRL_setFlag_enablePowerWarp(handle,false);
  CTRL_setFlag_enableCtrl(handle,false);
  CTRL_setFlag_enableOffset(handle,true);
  CTRL_setFlag_enableSpeedCtrl(handle,true);
  CTRL_setFlag_enableUserMotorParams(handle,false);
  CTRL_setFlag_enableDcBusComp(handle,true);


  // initialize the controller error code
  CTRL_setErrorCode(handle,CTRL_ErrorCode_NoError);


  // set the default controller state
  CTRL_setState(handle,CTRL_State_Idle);


  // set the number of current sensors
  CTRL_setupClarke_I(handle,pUserParams->numCurrentSensors);


  // set the number of voltage sensors
  CTRL_setupClarke_V(handle,pUserParams->numVoltageSensors);


  // set the default Id PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Id,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Id,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Id,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp,Ki,Kd);


  // set the default the Iq PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Iq,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Iq,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Iq,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp,Ki,Kd);


  // set the default speed PID controller parameters
  Kp = _IQ(0.02*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A);
  Ki = _IQ(2.0*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A);
  Kd = _IQ(0.0);
  outMin = _IQ(-1.0);
  outMax = _IQ(1.0);

  PID_setGains(obj->pidHandle_spd,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_spd,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_spd,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_spd,Kp,Ki,Kd);


  // set the speed reference
  CTRL_setSpd_ref_pu(handle,_IQ(0.0));


  // set the default Id current trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxDelta(obj->trajHandle_Id,_IQ(0.0));


  // set the default the speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxDelta(obj->trajHandle_spd,_IQ(0.0));


  // set the default maximum speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxDelta(obj->trajHandle_spdMax,_IQ(0.0)); // not used

  
  // set the default estimator parameters
  CTRL_setEstParams(obj->estHandle,pUserParams);


  // set the maximum modulation for the SVGEN module
  maxModulation = SVGEN_4_OVER_3;
  SVGEN_setMaxModulation(obj->svgenHandle,maxModulation);

  return;
} // end of CTRL_setParams() function


void CTRL_setSpd_ref_pu(CTRL_Handle handle,const _iq spd_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_pu() function


void CTRL_setSpd_ref_krpm(CTRL_Handle handle,const _iq spd_ref_krpm)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq krpm_to_pu_sf = EST_get_krpm_to_pu_sf(obj->estHandle);

  _iq spd_ref_pu = _IQmpy(spd_ref_krpm,krpm_to_pu_sf);

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_krpm() function


void CTRL_setup(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  uint_least16_t count_traj = CTRL_getCount_traj(handle);
  uint_least16_t numCtrlTicksPerTrajTick = CTRL_getNumCtrlTicksPerTrajTick(handle);


  // as needed, update the trajectory
  if(count_traj >= numCtrlTicksPerTrajTick)
    {
      _iq intValue_Id = TRAJ_getIntValue(obj->trajHandle_Id);

      // reset the trajectory count
      CTRL_resetCounter_traj(handle);

      // run the trajectories
      CTRL_runTraj(handle);
    } // end of if(gFlag_traj) block

  return;
} // end of CTRL_setup() function


void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numCurrentSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else if(numCurrentSensors == 2)
    {
      alpha_sf = _IQ(1.0);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_I,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_I,numCurrentSensors);

  return;
} // end of CTRL_setupClarke_I() function


void CTRL_setupClarke_V(CTRL_Handle handle,uint_least8_t numVoltageSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numVoltageSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
 else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_V,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_V,numVoltageSensors);

  return;
} // end of CTRL_setupClarke_V() function


void CTRL_setWaitTimes(CTRL_Handle handle,const uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      obj->waitTimes[stateCnt] = pWaitTimes[stateCnt];
    }

  return;
} // end of CTRL_setWaitTimes() function


bool CTRL_updateState(CTRL_Handle handle)
{
  CTRL_State_e ctrlState = CTRL_getState(handle);
  bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
  bool stateChanged = false;


  if(flag_enableCtrl)
    {
      uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
      uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);


      // check for errors
      CTRL_checkForErrors(handle);


      if(counter_ctrlState >= waitTime)
        {
          // reset the counter
          CTRL_resetCounter_state(handle);


          if(ctrlState == CTRL_State_OnLine)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              _iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);

              // update the estimator state
              bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);

              if(flag_estStateChanged)
                {
                  // setup the controller
                  CTRL_setupCtrl(handle);

                  // setup the trajectory
                  CTRL_setupTraj(handle);
                }

              if(EST_isOnLine(obj->estHandle))
                {
                  // setup the estimator for online state
                  CTRL_setupEstOnLineState(handle);
                }

              if(EST_isLockRotor(obj->estHandle) || 
                 (EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
                {
                  // set the enable controller flag to false
                  CTRL_setFlag_enableCtrl(handle,false);

                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_Idle);
                }
            }
          else if(ctrlState == CTRL_State_OffLine)
            {
              // set the next controller state
              CTRL_setState(handle,CTRL_State_OnLine);
            }
          else if(ctrlState == CTRL_State_Idle)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              bool  flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);

              if(flag_enableUserMotorParams)
                {
                  // initialize the motor parameters using values from the user.h file
                  CTRL_setUserMotorParams(handle);
                }

              if(EST_isIdle(obj->estHandle))
                {
                  // setup the estimator for idle state
                  CTRL_setupEstIdleState(handle);

                  if(EST_isMotorIdentified(obj->estHandle))
                    {
                      if(CTRL_getFlag_enableOffset(handle))
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OffLine);
                        }
                      else
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OnLine);
                        }
                    }
                  else
                    {
                      // set the next controller state
                      CTRL_setState(handle,CTRL_State_OffLine);
                    }
                }
              else if(EST_isLockRotor(obj->estHandle))
                {
                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_OnLine);
                }
            }
        }  // if(counter_ctrlState >= waitTime) loop
    } 
  else
    {
      CTRL_Obj *obj = (CTRL_Obj *)handle;

      // set the next controller state
      CTRL_setState(handle,CTRL_State_Idle);

      // set the estimator to idle
      if(!EST_isLockRotor(obj->estHandle))
        {
          if(EST_isMotorIdentified(obj->estHandle))
            {
              EST_setIdle(obj->estHandle);
            }
          else
            {
              EST_setIdle_all(obj->estHandle);

              EST_setRs_pu(obj->estHandle,_IQ30(0.0));
            }
        }
    }


  // check to see if the state changed
  if(ctrlState != CTRL_getState(handle))
    {
      stateChanged = true;
    }

  return(stateChanged);
} // end of CTRL_updateState() function

// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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.
 * --/COPYRIGHT--*/
//! \file   solutions/instaspin_foc/src/proj_lab10a.c
//! \brief Space Vector Over-Modulation
//!
//! (C) Copyright 2011, Texas Instruments, Inc.

//! \defgroup PROJ_LAB10a PROJ_LAB10a
//@{

//! \defgroup PROJ_LAB10a_OVERVIEW Project Overview
//!
//! Experimentation with Space Vector Over-Modulation
//!

// **************************************************************************
// the includes

// system includes
#include <math.h>
#include "main.h"

#ifdef FLASH
#pragma CODE_SECTION(mainISR,"ramfuncs");
#endif

// Include header files used in the main function


// **************************************************************************
// the defines

#define LED_BLINK_FREQ_Hz   5


// **************************************************************************
// the globals

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

HAL_Handle halHandle;

USER_Params gUserParams;

HAL_PwmData_t gPwmData = {_IQ(0.0), _IQ(0.0), _IQ(0.0)};

HAL_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

CTRL_Obj *controller_obj;

#ifndef FAST_ROM_V1p6
CTRL_Obj ctrl;				//v1p7 format
#endif

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;


#ifdef FLASH
// Used for running BackGround in flash, and ISR in RAM
extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
#endif


SVGENCURRENT_Obj svgencurrent;
SVGENCURRENT_Handle svgencurrentHandle;


#ifdef DRV8301_SPI
// Watch window interface to the 8301 SPI
DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
#endif

// **************************************************************************
// the functions

void main(void)
{
  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

  // Only used if running from FLASH
  // Note that the variable FLASH is defined by the project
  #ifdef FLASH
  // Copy time critical code and Flash setup code to RAM
  // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
  // symbols are created by the linker. Refer to the linker files.
  memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
  #endif


  // initialize the hardware abstraction layer
  halHandle = HAL_init(&hal,sizeof(hal));


  // check for errors in user parameters
  USER_checkForErrors(&gUserParams);


  // store user parameter error in global variable
  gMotorVars.UserErrorCode = USER_getErrorCode(&gUserParams);


  // do not allow code execution if there is a user parameter error
  if(gMotorVars.UserErrorCode != USER_ErrorCode_NoError)
    {
      for(;;)
        {
          gMotorVars.Flag_enableSys = false;
        }
    }


  // initialize the user parameters
  USER_setParams(&gUserParams);


  // set the hardware abstraction layer parameters
  HAL_setParams(halHandle,&gUserParams);


  // initialize the controller
#ifdef FAST_ROM_V1p6
  ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber);  		//v1p6 format (06xF and 06xM devices)
#else
  ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl));	//v1p7 format default
#endif

  controller_obj = (CTRL_Obj *)ctrlHandle;


  {
    CTRL_Version version;

    // get the version number
    CTRL_getVersion(ctrlHandle,&version);

    gMotorVars.CtrlVersion = version;
  }


  // set the default controller parameters
  CTRL_setParams(ctrlHandle,&gUserParams);


  // Initialize and setup the 100% SVM generator
  svgencurrentHandle = SVGENCURRENT_init(&svgencurrent,sizeof(svgencurrent));

  {
    float_t minWidth_microseconds = 2.0;
    uint16_t minWidth_counts = (uint16_t)(minWidth_microseconds * USER_SYSTEM_FREQ_MHz / 2.0) + HAL_PWM_DBFED_CNT;

    SVGENCURRENT_setMinWidth(svgencurrentHandle, minWidth_counts);
  }


  // setup faults
  HAL_setupFaults(halHandle);


  // initialize the interrupt vector table
  HAL_initIntVectorTable(halHandle);


  // enable the ADC interrupts
  HAL_enableAdcInts(halHandle);


  // enable global interrupts
  HAL_enableGlobalInts(halHandle);


  // enable debug interrupts
  HAL_enableDebugInt(halHandle);


  // disable the PWM
  HAL_disablePwm(halHandle);


#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  HAL_enableDrv(halHandle);
  // initialize the DRV8301 interface
  HAL_setupDrvSpi(halHandle,&gDrvSpi8301Vars);
#endif


  // enable DC bus compensation
  CTRL_setFlag_enableDcBusComp(ctrlHandle, true);


  for(;;)
  {
    // Waiting for enable system flag to be set
    while(!(gMotorVars.Flag_enableSys));


    // set the current bias
    HAL_setBias(halHandle,HAL_SensorType_Current,0,_IQ(I_A_offset));
    HAL_setBias(halHandle,HAL_SensorType_Current,1,_IQ(I_B_offset));
    HAL_setBias(halHandle,HAL_SensorType_Current,2,_IQ(I_C_offset));

    // set the voltage bias
    HAL_setBias(halHandle,HAL_SensorType_Voltage,0,_IQ(V_A_offset));
    HAL_setBias(halHandle,HAL_SensorType_Voltage,1,_IQ(V_B_offset));
    HAL_setBias(halHandle,HAL_SensorType_Voltage,2,_IQ(V_C_offset));

    // Enable the Library internal PI.  Iq is referenced by the speed PI now
    CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);

    // loop while the enable system flag is true
    while(gMotorVars.Flag_enableSys)
      {
        CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;

        // increment counters
        gCounter_updateGlobals++;

        // enable/disable the use of motor parameters being loaded from user.h
        CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);

        // enable/disable Rs recalibration during motor startup
        EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);

        // enable/disable automatic calculation of bias values
        CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);


        if(CTRL_isError(ctrlHandle))
          {
            // set the enable controller flag to false
            CTRL_setFlag_enableCtrl(ctrlHandle,false);

            // set the enable system flag to false
            gMotorVars.Flag_enableSys = false;

            // disable the PWM
            HAL_disablePwm(halHandle);
          }
        else
          {
            // update the controller state
            bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);

            // enable or disable the control
            CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);

            if(flag_ctrlStateChanged)
              {
                CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);

                if(ctrlState == CTRL_State_OffLine)
                  {
                    // enable the PWM
                    HAL_enablePwm(halHandle);
                  }
                else if(ctrlState == CTRL_State_OnLine)
                  {
                    // update the ADC bias values
                    HAL_updateAdcBias(halHandle);


                    // enable the PWM
                    HAL_enablePwm(halHandle);
                  }
                else if(ctrlState == CTRL_State_Idle)
                  {
                    // disable the PWM
                    HAL_disablePwm(halHandle);
                    gMotorVars.Flag_Run_Identify = false;
                  }
              }
          }


        if(EST_isMotorIdentified(obj->estHandle))
          {
            _iq Id_squared_pu = _IQmpy(CTRL_getId_ref_pu(ctrlHandle),CTRL_getId_ref_pu(ctrlHandle));


            //Set the maximum current controller output for the Iq and Id current controllers to enable
            //over-modulation.
            //An input into the SVM above 2/SQRT(3) = 1.1547 is in the over-modulation region.  An input of 1.1547 is where
            //the crest of the sinewave touches the 100% duty cycle.  At an input of 4/3, the SVM generator
            //produces a trapezoidal waveform touching every corner of the hexagon
            CTRL_setMaxVsMag_pu(ctrlHandle,gMotorVars.OverModulation);

            // set the current ramp
            EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
            gMotorVars.Flag_MotorIdentified = true;

            // set the speed reference
            CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);

            // set the speed acceleration
            CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));
            if(Flag_Latch_softwareUpdate)
            {
              Flag_Latch_softwareUpdate = false;

#ifdef FAST_ROM_V1p6
              if(CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true)
              {
                  // call this function to fix 1p6
                  softwareUpdate1p6(ctrlHandle);
              }
#endif

              calcPIgains(ctrlHandle);

              // initialize the watch window kp and ki current values with pre-calculated values
              gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
              gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);
            }

          }
        else
          {
            Flag_Latch_softwareUpdate = true;

            // initialize the watch window kp and ki values with pre-calculated values
            gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
            gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);


            // the estimator sets the maximum current slope during identification
            gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
          }


        // when appropriate, update the global variables
        if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
          {
            // reset the counter
            gCounter_updateGlobals = 0;

            updateGlobalVariables_motor(ctrlHandle);
          }


        // update Kp and Ki gains
        updateKpKiGains(ctrlHandle);

        // enable/disable the forced angle
        EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);

        // enable or disable power warp
        CTRL_setFlag_enablePowerWarp(ctrlHandle,gMotorVars.Flag_enablePowerWarp);

#ifdef DRV8301_SPI
        HAL_writeDrvData(halHandle,&gDrvSpi8301Vars);

        HAL_readDrvData(halHandle,&gDrvSpi8301Vars);
#endif

      } // end of while(gFlag_enableSys) loop


    // disable the PWM
    HAL_disablePwm(halHandle);

    // set the default controller parameters (Reset the control to re-identify the motor)
    CTRL_setParams(ctrlHandle,&gUserParams);
    gMotorVars.Flag_Run_Identify = false;

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{
  // toggle status LED
  if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
  {
    HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
    gLEDcnt = 0;
  }


  // acknowledge the ADC interrupt
  HAL_acqAdcInt(halHandle,ADC_IntNumber_1);


  // convert the ADC data
  HAL_readAdcData(halHandle,&gAdcData);


  // run the current reconstruction algorithm
  SVGENCURRENT_RunRegenCurrent(svgencurrentHandle, (MATH_vec3 *)(gAdcData.I.value));


  // run the controller
  CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);


  // write the PWM compare values
  HAL_writePwmData(halHandle,&gPwmData);


  // run the current ignore algorithm
  {
    uint16_t pwmValue_1 = HAL_readPwmCmpA(halHandle,PWM_Number_1);
    uint16_t pwmValue_2 = HAL_readPwmCmpA(halHandle,PWM_Number_2);
    uint16_t pwmValue_3 = HAL_readPwmCmpA(halHandle,PWM_Number_3);

    // run the current ignore algorithm
    SVGENCURRENT_RunIgnoreShunt(svgencurrentHandle,pwmValue_1,pwmValue_2,pwmValue_3);
  }

    gMotorVars.Ia = CTRL_getIa_in_pu(ctrlHandle);

  // Set trigger point in the middle of the low side pulse
  HAL_setTrigger(halHandle,SVGENCURRENT_getMinWidth(svgencurrentHandle));


  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  // get the speed estimate
  gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);

  // get the real time speed reference coming out of the speed trajectory generator
  gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));

  // get the torque estimate
  gMotorVars.Torque_lbin = calcTorque_lbin(handle);

  // get the magnetizing current
  gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);

  // get the rotor resistance
  gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);

  // get the stator resistance
  gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);

  // get the stator inductance in the direct coordinate direction
  gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);

  // get the stator inductance in the quadrature coordinate direction
  gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);

  // get the flux
  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);

  // get the controller state
  gMotorVars.CtrlState = CTRL_getState(handle);

  // get the estimator state
  gMotorVars.EstState = EST_getState(obj->estHandle);

  // read Vd and Vq vectors per units
  gMotorVars.Vd = CTRL_getVd_out_pu(ctrlHandle);
  gMotorVars.Vq = CTRL_getVq_out_pu(ctrlHandle);

  // calculate vector Vs in per units
  gMotorVars.Vs = _IQsqrt(_IQmpy(gMotorVars.Vd, gMotorVars.Vd) + _IQmpy(gMotorVars.Vq, gMotorVars.Vq));

  // read Id and Iq vectors in amps
  gMotorVars.Id_A = _IQmpy(CTRL_getId_in_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
  gMotorVars.Iq_A = _IQmpy(CTRL_getIq_in_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));

  // calculate vector Is in amps
  gMotorVars.Is_A = _IQsqrt(_IQmpy(gMotorVars.Id_A, gMotorVars.Id_A) + _IQmpy(gMotorVars.Iq_A, gMotorVars.Iq_A));

  // Get the DC buss voltage
  gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));

  return;
} // end of updateGlobalVariables_motor() function


void updateKpKiGains(CTRL_Handle handle)
{
  if((gMotorVars.CtrlState == CTRL_State_OnLine) && (gMotorVars.Flag_MotorIdentified == true) && (Flag_Latch_softwareUpdate == false))
    {
      // set the kp and ki speed values from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_spd,gMotorVars.Kp_spd);
      CTRL_setKi(handle,CTRL_Type_PID_spd,gMotorVars.Ki_spd);

      // set the kp and ki current values for Id and Iq from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_Id,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Id,gMotorVars.Ki_Idq);
      CTRL_setKp(handle,CTRL_Type_PID_Iq,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Iq,gMotorVars.Ki_Idq);
	}

  return;
} // end of updateKpKiGains() function


#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t fullScaleInductance = EST_getFullScaleInductance(obj->estHandle);
  float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
  int_least8_t lShift = ceil(log(obj->motorParams.Ls_d_H/(Ls_coarse_max*fullScaleInductance))/log(2.0));
  uint_least8_t Ls_qFmt = 30 - lShift;
  float_t L_max = fullScaleInductance * pow(2.0,lShift);
  _iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d_H / L_max);
  _iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q_H / L_max);


  // store the results
  EST_setLs_d_pu(obj->estHandle,Ls_d_pu);
  EST_setLs_q_pu(obj->estHandle,Ls_q_pu);
  EST_setLs_qFmt(obj->estHandle,Ls_qFmt);

  return;
} // end of softwareUpdate1p6() function
#endif

void calcPIgains(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t Ls_d = EST_getLs_d_H(obj->estHandle);
  float_t Ls_q = EST_getLs_q_H(obj->estHandle);
  float_t Rs = EST_getRs_Ohm(obj->estHandle);
  float_t RoverLs_d = Rs/Ls_d;
  float_t RoverLs_q = Rs/Ls_q;
  float_t fullScaleCurrent = EST_getFullScaleCurrent(obj->estHandle);
  float_t fullScaleVoltage = EST_getFullScaleVoltage(obj->estHandle);
  float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
  _iq Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);
  _iq Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);
  _iq Kd = _IQ(0.0);

  // set the Id controller gains
  PID_setKi(obj->pidHandle_Id,Ki_Id);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd);

  // set the Iq controller gains
  PID_setKi(obj->pidHandle_Iq,Ki_Iq);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd);

  return;
} // end of calcPIgains() function

float_t calcTorque_Nm(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  float_t Id_A = _IQtoF(PID_getFbackValue(obj->pidHandle_Id)) * USER_IQ_FULL_SCALE_CURRENT_A;
  float_t Iq_A = _IQtoF(PID_getFbackValue(obj->pidHandle_Iq)) * USER_IQ_FULL_SCALE_CURRENT_A;
  float_t Polepairs = (float_t)USER_MOTOR_NUM_POLE_PAIRS;
  float_t Flux_Wb = EST_getFlux_Wb(obj->estHandle);
  float_t Lsd_H = EST_getLs_d_H(obj->estHandle);
  float_t Lsq_H = EST_getLs_q_H(obj->estHandle);

  return((Flux_Wb * Iq_A + (Lsd_H - Lsq_H) * Id_A * Iq_A) * Polepairs * 1.5);
} // end of calcTorque_Nm() function


float_t calcTorque_lbin(CTRL_Handle handle)
{

  return(calcTorque_Nm(handle) * MATH_Nm_TO_lbin_SF);
} // end of calcTorque_lbin() function


//@} //defgroup
// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, LineStream Technologies Incorporated
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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 names of Texas Instruments Incorporated, LineStream
 *    Technologies 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.
 * --/COPYRIGHT--*/
//! \file   solutions/instaspin_motion/src/proj_lab05c.c
//! \brief  InstaSPIN-MOTION Inertia Identification
//!
//! (C) Copyright 2012, LineStream Technologies, Inc.
//! (C) Copyright 2011, Texas Instruments, Inc.

//! \defgroup PROJ_LAB05c PROJ_LAB05c
//@{

//! \defgroup PROJ_LAB05c_OVERVIEW Project Overview
//!
//! InstaSPIN-MOTION Inertia Identification
//!

// **************************************************************************
// the includes

// system includes
#include <math.h>
#include "main.h"

#ifdef FLASH
#pragma CODE_SECTION(mainISR,"ramfuncs");
#endif

// Include header files used in the main function


// **************************************************************************
// the defines

#define LED_BLINK_FREQ_Hz   5


// **************************************************************************
// the globals

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

DRV_Handle drvHandle;

USER_Params gUserParams;

DRV_PwmData_t gPwmData;

DRV_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

#ifdef FAST_ROM_V1p6
CTRL_Obj *controller_obj;
#else
CTRL_Obj ctrl;				//v1p7 format
#endif

ST_Obj st_obj;
ST_Handle stHandle;

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;

#ifdef FLASH
// Used for running BackGround in flash, and ISR in RAM
extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
#endif


#ifdef DRV8301_SPI
// Watch window interface to the 8301 SPI
DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
#endif

// **************************************************************************
// the functions

void main(void)
{
  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

  // Only used if running from FLASH
  // Note that the variable FLASH is defined by the project
  #ifdef FLASH
  // Copy time critical code and Flash setup code to RAM
  // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
  // symbols are created by the linker. Refer to the linker files.
  memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
  #endif

  // initialize the driver
  drvHandle = DRV_init(&drv,sizeof(drv));


  // initialize the user parameters
  USER_setParams(&gUserParams);


  // set the driver parameters
  DRV_setParams(drvHandle,&gUserParams);


  // initialize the controller
#ifdef FAST_ROM_V1p6
  ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber);  		//v1p6 format (06xF and 06xM devices)
  controller_obj = (CTRL_Obj *)ctrlHandle;
#else
  ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl));	//v1p7 format default
#endif


  // set the default controller parameters
  CTRL_setParams(ctrlHandle,&gUserParams);


  // setup faults
  DRV_setupFaults(drvHandle);


  // initialize the interrupt vector table
  DRV_initIntVectorTable(drvHandle);


  // enable the ADC interrupts
  DRV_enableAdcInts(drvHandle);


  // enable global interrupts
  DRV_enableGlobalInts(drvHandle);


  // enable debug interrupts
  DRV_enableDebugInt(drvHandle);


  // disable the PWM
  DRV_disablePwm(drvHandle);


  // enable DC bus compensation
  CTRL_setFlag_enableDcBusComp(ctrlHandle, true);


  // initialize the SpinTAC Components
  stHandle = ST_init(&st_obj, sizeof(st_obj));
  
  
  // setup the SpinTAC Components
  ST_setupVelId(stHandle);


#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  DRV_enable8301(drvHandle);
  // initialize the DRV8301 interface
  DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars);
#endif


  for(;;)
  {
    // Waiting for enable system flag to be set
    while(!(gMotorVars.Flag_enableSys));

    Flag_Latch_softwareUpdate = true;

    // Dis-able the Library internal PI.  Iq has no reference now
    CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);

    // loop while the enable system flag is true
    while(gMotorVars.Flag_enableSys)
      {
        CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
        ST_Obj *stObj = (ST_Obj *)stHandle;

        // increment counters
        gCounter_updateGlobals++;

        // enable/disable the use of motor parameters being loaded from user.h
        CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);

        // enable/disable Rs recalibration during motor startup
        EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);

        // enable/disable automatic calculation of bias values
        CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);


        if(CTRL_isError(ctrlHandle))
          {
            // set the enable controller flag to false
            CTRL_setFlag_enableCtrl(ctrlHandle,false);

            // set the enable system flag to false
            gMotorVars.Flag_enableSys = false;

            // disable the PWM
            DRV_disablePwm(drvHandle);
          }
        else
          {
            // update the controller state
            bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);

            // enable or disable the control
            CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);

            if(flag_ctrlStateChanged)
              {
                CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);

                if(ctrlState == CTRL_State_OffLine)
                  {
                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_OnLine)
                  {
                    if(gMotorVars.Flag_enableOffsetcalc == true)
                    {
                      // update the ADC bias values
                      DRV_updateAdcBias(drvHandle);
                    }
                    else
                    {
                      // set the current bias
                      DRV_setBias(drvHandle,DRV_SensorType_Current,0,_IQ(I_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,1,_IQ(I_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,2,_IQ(I_C_offset));

                      // set the voltage bias
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,0,_IQ(V_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,1,_IQ(V_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,2,_IQ(V_C_offset));
                    }

                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_Idle)
                  {
                    // disable the PWM
                    DRV_disablePwm(drvHandle);
                    gMotorVars.Flag_Run_Identify = false;
                  }
              }
          }


        if(EST_isMotorIdentified(obj->estHandle))
          {
            // set the current ramp
            EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
            gMotorVars.Flag_MotorIdentified = true;

            // set the speed reference
            CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);

            // set the speed acceleration
            CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));

            if(Flag_Latch_softwareUpdate)
            {
              Flag_Latch_softwareUpdate = false;

#ifdef FAST_ROM_V1p6
              if(CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true)
              {
                  // call this function to fix 1p6
                  softwareUpdate1p6(ctrlHandle);
              }
#endif

              calcPIgains(ctrlHandle);

              // initializes the value for GoalSpeed and TorqueRampTime
              gMotorVars.SpinTAC.VelIdGoalSpeed_krpm = _IQmpy(STVELID_getVelocityPositive(stObj->velIdHandle), _IQ(ST_SPEED_KRPM_PER_PU));
              gMotorVars.SpinTAC.VelIdTorqueRampTime_sec = STVELID_getTorqueRampTime_sec(stObj->velIdHandle);

            }
          }
        else
          {
            Flag_Latch_softwareUpdate = true;

            // the estimator sets the maximum current slope during identification
            gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
          }


        // when appropriate, update the global variables
        if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
          {
            // reset the counter
            gCounter_updateGlobals = 0;

            updateGlobalVariables_motor(ctrlHandle, stHandle);
          }

      } // end of while(gFlag_enableSys) loop


    // disable the PWM
    DRV_disablePwm(drvHandle);

    // set the default controller parameters (Reset the control to re-identify the motor)
    CTRL_setParams(ctrlHandle,&gUserParams);

    // setup the SpinTAC Components
    ST_setupVelId(stHandle);

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{

  static uint16_t stCnt = 0;

  // DPM - toggle status LED
  if(gLEDcnt++ > (uint_least32_t)(USER_PWM_FREQ_kHz * 1000 / LED_BLINK_FREQ_Hz)) {
    DRV_toggleLed(drvHandle,(GPIO_Number_e)DRV_Gpio_LED2);
    gLEDcnt = 0;
  }


  // acknowledge the ADC interrupt
  DRV_acqAdcInt(drvHandle,ADC_IntNumber_1);


  // convert the ADC data
  DRV_readAdcData(drvHandle,&gAdcData);


  // Run the SpinTAC Components
  if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
	  ST_runVelId(stHandle, ctrlHandle);
	  stCnt = 1;
  }


  // run the controller
  CTRL_run(ctrlHandle,drvHandle,&gAdcData,&gPwmData);


  // write the PWM compare values
  DRV_writePwmData(drvHandle,&gPwmData);


  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle sthandle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  ST_Obj *stObj = (ST_Obj *)sthandle;


  // get the speed estimate
  gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);

  // get the torque estimate
  gMotorVars.Torque_lbin = EST_getTorque_lbin(obj->estHandle);

  // get the magnetizing current
  gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);

  // get the rotor resistance
  gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);

  // get the stator resistance
  gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);

  // get the stator inductance in the direct coordinate direction
  gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);

  // get the stator inductance in the quadrature coordinate direction
  gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);

  // get the flux
  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);

  // get the controller state
  gMotorVars.CtrlState = CTRL_getState(handle);

  // get the estimator state
  gMotorVars.EstState = EST_getState(obj->estHandle);

  // get Velocity Identify status
  gMotorVars.SpinTAC.VelIdStatus = STVELID_getStatus(stObj->velIdHandle);

  // get the inertia estimate
  gMotorVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STVELID_getInertiaEstimate(stObj->velIdHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // get the friction estimate
  gMotorVars.SpinTAC.FrictionEstimate_Aperkrpm = _IQmpy(STVELID_getFrictionEstimate(stObj->velIdHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // get the Velocity Identify error
  gMotorVars.SpinTAC.VelIdErrorID = STVELID_getErrorID(stObj->velIdHandle);

  // enable/disable the forced angle
  EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);

  // enable or disable power warp
  CTRL_setFlag_enableEpl(handle,gMotorVars.Flag_enableEpl);

#ifdef DRV8301_SPI
  DRV_8301_SpiInterface(drvHandle,&gDrvSpi8301Vars);
#endif

  return;
} // end of updateGlobalVariables_motor() function

#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{

  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t fullScaleInductance = EST_getFullScaleInductance(obj->estHandle);
  float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
  int_least8_t lShift = ceil(log(obj->motorParams.Ls_d/(Ls_coarse_max*fullScaleInductance))/log(2.0));
  uint_least8_t Ls_qFmt = 30 - lShift;
  float_t L_max = fullScaleInductance * pow(2.0,lShift);
  _iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d / L_max);
  _iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q / L_max);


  // store the results
  EST_setLs_d_pu(obj->estHandle,Ls_d_pu);
  EST_setLs_q_pu(obj->estHandle,Ls_q_pu);
  EST_setLs_qFmt(obj->estHandle,Ls_qFmt);

  return;
} // end of softwareUpdate1p6() function
#endif

void calcPIgains(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t Ls_d = EST_getLs_d_H(obj->estHandle);
  float_t Ls_q = EST_getLs_q_H(obj->estHandle);
  float_t Rs = EST_getRs_Ohm(obj->estHandle);
  float_t RoverLs_d = Rs/Ls_d;
  float_t RoverLs_q = Rs/Ls_q;
  float_t fullScaleCurrent = EST_getFullScaleCurrent(obj->estHandle);
  float_t fullScaleVoltage = EST_getFullScaleVoltage(obj->estHandle);
  float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
  _iq Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);
  _iq Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);
  _iq Kd = _IQ(0.0);

  // set the Id controller gains
  PID_setKi(obj->pidHandle_Id,Ki_Id);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd);

  // set the Iq controller gains
  PID_setKi(obj->pidHandle_Iq,Ki_Iq);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd);

  return;
} // end of calcPIgains() function


void ST_runVelId(ST_Handle sthandle, CTRL_Handle handle)
{

    _iq speedFeedback, iqReference;
    ST_Obj *stObj = (ST_Obj *)sthandle;
    CTRL_Obj *ctrlObj = (CTRL_Obj *)handle;

    // Get the mechanical speed in pu
    speedFeedback = EST_getFm_pu(ctrlObj->estHandle);

	if(gMotorVars.SpinTAC.VelIdRun == true) {
		// if beginning the SpinTAC Identify process
		// set the speed reference to zero
		gMotorVars.SpeedRef_krpm = 0;
		// wait until the actual speed is zero
		if((_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU)) && (STVELID_getEnable(stObj->velIdHandle) == false)) {
			CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
			gMotorVars.Flag_enableForceAngle = true;
			EST_setFlag_enableForceAngle(ctrlObj->estHandle, gMotorVars.Flag_enableForceAngle);
			// set the GoalSpeed
			STVELID_setVelocityPositive(stObj->velIdHandle, _IQmpy(gMotorVars.SpinTAC.VelIdGoalSpeed_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
			// set the Torque Ramp Time
			STVELID_setTorqueRampTime_sec(stObj->velIdHandle, gMotorVars.SpinTAC.VelIdTorqueRampTime_sec);
			// Enable SpinTAC Inertia Identify
			STVELID_setEnable(stObj->velIdHandle, true);
			// Set a positive speed reference to FAST to provide direction information
			gMotorVars.SpeedRef_krpm = _IQ(0.001);
			CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
		}
	}

	// Run SpinTAC Inertia Identify
	STVELID_setVelocityFeedback(stObj->velIdHandle, speedFeedback);
	STVELID_run(stObj->velIdHandle);

	if(STVELID_getDone(stObj->velIdHandle) == true) {
		// If inertia identification is successful, update the inertia setting of SpinTAC Velocity Controller
		// EXAMPLE: STVELCTL_setInertia(stObj->velCtlHandle, STVELID_getInertiaEstimate(stObj->velIdHandle));
		gMotorVars.SpinTAC.VelIdRun = false;
		// return the speed reference to zero
		gMotorVars.SpeedRef_krpm = gMotorVars.StopSpeedRef_krpm;
		CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
		CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
	}
	else if((STVELID_getErrorID(stObj->velIdHandle) != false) && (STVELID_getErrorID(stObj->velIdHandle) != ST_ID_INCOMPLETE_ERROR)) {
	// if not done & in error, wait until speed is less than 1RPM to exit
		if(_IQabs(speedFeedback) < _IQ(ST_MIN_ID_SPEED_PU)) {
			gMotorVars.SpinTAC.VelIdRun = false;
			// return the speed reference to zero
			gMotorVars.SpeedRef_krpm = gMotorVars.StopSpeedRef_krpm;
			CTRL_setFlag_enableSpeedCtrl(ctrlHandle, true);
			CTRL_setSpd_ref_krpm(ctrlHandle, gMotorVars.SpeedRef_krpm);
		}
	}

	// select SpinTAC Identify
	iqReference = STVELID_getTorqueReference(stObj->velIdHandle);

	// Set the Iq reference that came out of SpinTAC Velocity Control
	CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
//@} //defgroup
// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, LineStream Technologies Incorporated
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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 names of Texas Instruments Incorporated, LineStream
 *    Technologies 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.
 * --/COPYRIGHT--*/
//! \file   solutions/instaspin_motion/src/proj_lab05f.c
//! \brief  Comparing performance of PI velocity controller & SpinTAC velocity controller
//!
//! (C) Copyright 2012, LineStream Technologies, Inc.
//! (C) Copyright 2011, Texas Instruments, Inc.

//! \defgroup PROJ_LAB05f PROJ_LAB05f
//@{

//! \defgroup PROJ_LAB05f_OVERVIEW Project Overview
//!
//! Comparing performance of PI velocity controller & SpinTAC velocity controller
//!

// **************************************************************************
// the includes

// system includes
#include <math.h>
#include "main.h"

#ifdef FLASH
#pragma CODE_SECTION(mainISR,"ramfuncs");
#endif

// Include header files used in the main function


// **************************************************************************
// the defines

#define LED_BLINK_FREQ_Hz   5


// **************************************************************************
// the globals

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

DRV_Handle drvHandle;

USER_Params gUserParams;

DRV_PwmData_t gPwmData;

DRV_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

#ifdef FAST_ROM_V1p6
CTRL_Obj *controller_obj;
#else
CTRL_Obj ctrl;				//v1p7 format
#endif

ST_Obj st_obj;
ST_Handle stHandle;

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;

// Graphing Variables
volatile bool Graph_Flag_Enable_update = false;
volatile unsigned int Graph_Counter = 0;
volatile unsigned short Graph_Tick_Counter = 0;
volatile unsigned short Graph_Tick_Counter_Value = 10;
#define GRAPH_SIZE 300
volatile _iq Graph_Data[GRAPH_SIZE];
volatile _iq Graph_Data2[GRAPH_SIZE];
volatile _iq SpeedRef_krpm_previous = _IQ(0.0);

#ifdef FLASH
// Used for running BackGround in flash, and ISR in RAM
extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;
#endif


#ifdef DRV8301_SPI
// Watch window interface to the 8301 SPI
DRV_SPI_8301_Vars_t gDrvSpi8301Vars;
#endif


// **************************************************************************
// the functions

void main(void)
{
  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

  // Only used if running from FLASH
  // Note that the variable FLASH is defined by the project
  #ifdef FLASH
  // Copy time critical code and Flash setup code to RAM
  // The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
  // symbols are created by the linker. Refer to the linker files.
  memCopy((uint16_t *)&RamfuncsLoadStart,(uint16_t *)&RamfuncsLoadEnd,(uint16_t *)&RamfuncsRunStart);
  #endif

  uint_least8_t graphCounter = 0;

  // initialize the driver
  drvHandle = DRV_init(&drv,sizeof(drv));


  // initialize the user parameters
  USER_setParams(&gUserParams);


  // set the driver parameters
  DRV_setParams(drvHandle,&gUserParams);


  // initialize the controller
#ifdef FAST_ROM_V1p6
  ctrlHandle = CTRL_initCtrl(ctrlNumber, estNumber);  		//v1p6 format (06xF and 06xM devices)
  controller_obj = (CTRL_Obj *)ctrlHandle;
#else
  ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl));	//v1p7 format default
#endif


  // set the default controller parameters
  CTRL_setParams(ctrlHandle,&gUserParams);


  // setup faults
  DRV_setupFaults(drvHandle);


  // initialize the interrupt vector table
  DRV_initIntVectorTable(drvHandle);


  // enable the ADC interrupts
  DRV_enableAdcInts(drvHandle);


  // enable global interrupts
  DRV_enableGlobalInts(drvHandle);


  // enable debug interrupts
  DRV_enableDebugInt(drvHandle);


  // disable the PWM
  DRV_disablePwm(drvHandle);


  // enable DC bus compensation
  CTRL_setFlag_enableDcBusComp(ctrlHandle, true);


  // initialize the SpinTAC Components
  stHandle = ST_init(&st_obj, sizeof(st_obj));
  
  
  // setup the SpinTAC Components
  ST_setupVelCtl(stHandle);

  
  // initialize the Graph Variables
  for(graphCounter=0; graphCounter<GRAPH_SIZE; graphCounter++) {
	  Graph_Data[graphCounter] = 0;
	  Graph_Data2[graphCounter] = 0;
  }  


#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  DRV_enable8301(drvHandle);
  // initialize the DRV8301 interface
  DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars);
#endif


  for(;;)
  {
    // Waiting for enable system flag to be set
    while(!(gMotorVars.Flag_enableSys));

    Flag_Latch_softwareUpdate = true;

    // Dis-able the Library internal PI.  Iq has no reference now
    CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);

    // loop while the enable system flag is true
    while(gMotorVars.Flag_enableSys)
      {
        CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
        ST_Obj *stObj = (ST_Obj *)stHandle;

        // increment counters
        gCounter_updateGlobals++;

        // enable/disable the use of motor parameters being loaded from user.h
        CTRL_setFlag_enableUserMotorParams(ctrlHandle,gMotorVars.Flag_enableUserParams);

        // enable/disable Rs recalibration during motor startup
        EST_setFlag_enableRsRecalc(obj->estHandle,gMotorVars.Flag_enableRsRecalc);

        // enable/disable automatic calculation of bias values
        CTRL_setFlag_enableOffset(ctrlHandle,gMotorVars.Flag_enableOffsetcalc);


        if(CTRL_isError(ctrlHandle))
          {
            // set the enable controller flag to false
            CTRL_setFlag_enableCtrl(ctrlHandle,false);

            // set the enable system flag to false
            gMotorVars.Flag_enableSys = false;

            // disable the PWM
            DRV_disablePwm(drvHandle);
          }
        else
          {
            // update the controller state
            bool flag_ctrlStateChanged = CTRL_updateState(ctrlHandle);

            // enable or disable the control
            CTRL_setFlag_enableCtrl(ctrlHandle, gMotorVars.Flag_Run_Identify);

            if(flag_ctrlStateChanged)
              {
                CTRL_State_e ctrlState = CTRL_getState(ctrlHandle);

                if(ctrlState == CTRL_State_OffLine)
                  {
                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_OnLine)
                  {
                    if(gMotorVars.Flag_enableOffsetcalc == true)
                    {
                      // update the ADC bias values
                      DRV_updateAdcBias(drvHandle);
                    }
                    else
                    {
                      // set the current bias
                      DRV_setBias(drvHandle,DRV_SensorType_Current,0,_IQ(I_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,1,_IQ(I_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Current,2,_IQ(I_C_offset));

                      // set the voltage bias
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,0,_IQ(V_A_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,1,_IQ(V_B_offset));
                      DRV_setBias(drvHandle,DRV_SensorType_Voltage,2,_IQ(V_C_offset));
                    }

                    // enable the PWM
                    DRV_enablePwm(drvHandle);
                  }
                else if(ctrlState == CTRL_State_Idle)
                  {
                    // disable the PWM
                    DRV_disablePwm(drvHandle);
                    gMotorVars.Flag_Run_Identify = false;
                  }
              }
          }


        if(EST_isMotorIdentified(obj->estHandle))
          {
            // set the current ramp
            EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
            gMotorVars.Flag_MotorIdentified = true;

            // set the speed reference
            CTRL_setSpd_ref_krpm(ctrlHandle,gMotorVars.SpeedRef_krpm);

            // set the speed acceleration
            CTRL_setMaxAccel_pu(ctrlHandle,_IQmpy(MAX_ACCEL_KRPMPS_SF,gMotorVars.MaxAccel_krpmps));

            // enable the SpinTAC Speed Controller
            STVELCTL_setEnable(stObj->velCtlHandle, true);

            if(EST_getState(obj->estHandle) != EST_State_OnLine)
            {
            	// if the estimator is not running, place SpinTAC into reset
            	STVELCTL_setEnable(stObj->velCtlHandle, false);
            }
			
			// select the SpinTAC Speed Controller (true) or the PI Speed Controller (false)
            CTRL_setFlag_enableSpeedCtrl(ctrlHandle, !gMotorVars.SpinTAC.VelCtlEnb);

            if(Flag_Latch_softwareUpdate)
            {
              Flag_Latch_softwareUpdate = false;

#ifdef FAST_ROM_V1p6
              if(CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true)
              {
                  // call this function to fix 1p6
                  softwareUpdate1p6(ctrlHandle);
              }
#endif

              calcPIgains(ctrlHandle);

              // initialize the watch window kp and ki current values with pre-calculated values
              gMotorVars.Kp_Idq = CTRL_getKp(ctrlHandle,CTRL_Type_PID_Id);
              gMotorVars.Ki_Idq = CTRL_getKi(ctrlHandle,CTRL_Type_PID_Id);

              // initialize the watch window kp and ki values with pre-calculated values
              gMotorVars.Kp_spd = CTRL_getKp(ctrlHandle,CTRL_Type_PID_spd);
              gMotorVars.Ki_spd = CTRL_getKi(ctrlHandle,CTRL_Type_PID_spd);
			  
			  // initialize the watch window Bw value with the default value
              gMotorVars.SpinTAC.VelCtlBwScale = STVELCTL_getBandwidthScale(stObj->velCtlHandle);
            }
          }
        else
          {
            Flag_Latch_softwareUpdate = true;

            // the estimator sets the maximum current slope during identification
            gMaxCurrentSlope = EST_getMaxCurrentSlope_pu(obj->estHandle);
          }


        // when appropriate, update the global variables
        if(gCounter_updateGlobals >= NUM_MAIN_TICKS_FOR_GLOBAL_VARIABLE_UPDATE)
          {
            // reset the counter
            gCounter_updateGlobals = 0;

            updateGlobalVariables_motor(ctrlHandle, stHandle);
          }

      } // end of while(gFlag_enableSys) loop


    // disable the PWM
    DRV_disablePwm(drvHandle);

    // set the default controller parameters (Reset the control to re-identify the motor)
    CTRL_setParams(ctrlHandle,&gUserParams);

    // setup the SpinTAC Components
    ST_setupVelCtl(stHandle);

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{

  static uint16_t stCnt = 0;

  // DPM - toggle status LED
  if(gLEDcnt++ > (uint_least32_t)(USER_PWM_FREQ_kHz * 1000 / LED_BLINK_FREQ_Hz)) {
    DRV_toggleLed(drvHandle,(GPIO_Number_e)DRV_Gpio_LED2);
    gLEDcnt = 0;
  }


  // acknowledge the ADC interrupt
  DRV_acqAdcInt(drvHandle,ADC_IntNumber_1);


  // convert the ADC data
  DRV_readAdcData(drvHandle,&gAdcData);


  // Run the SpinTAC Components
  if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
	  ST_runVelCtl(stHandle, ctrlHandle);
	  stCnt = 1;
  }


  // run the controller
  CTRL_run(ctrlHandle,drvHandle,&gAdcData,&gPwmData);


  // write the PWM compare values
  DRV_writePwmData(drvHandle,&gPwmData);

  if (!(SpeedRef_krpm_previous == gMotorVars.SpeedRef_krpm)) {
  	  Graph_Flag_Enable_update = true;
  	  Graph_Counter = 0;
  	  SpeedRef_krpm_previous = gMotorVars.SpeedRef_krpm;
  }

  gMotorVars.Iq_A = _IQmpy(CTRL_getIq_in_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));

  // store graph value
  if(Graph_Flag_Enable_update) {
  	  if(Graph_Counter >= GRAPH_SIZE) {
  		  Graph_Flag_Enable_update = false;
  	  }
  	  else {
  		  if(Graph_Tick_Counter == 0) {
  		      Graph_Data[Graph_Counter] = gMotorVars.Speed_krpm;
  		      Graph_Data2[Graph_Counter] = gMotorVars.Iq_A;
  		      Graph_Counter++;
  		      Graph_Tick_Counter = Graph_Tick_Counter_Value - 1;
  		  }
  		  else {
  			  Graph_Tick_Counter--;
  		  }
  	  }
   }


  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle sthandle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  ST_Obj *stObj = (ST_Obj *)sthandle;


  // get the speed estimate
  gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);

  // get the torque estimate
  gMotorVars.Torque_lbin = EST_getTorque_lbin(obj->estHandle);

  // get the magnetizing current
  gMotorVars.MagnCurr_A = EST_getIdRated(obj->estHandle);

  // get the rotor resistance
  gMotorVars.Rr_Ohm = EST_getRr_Ohm(obj->estHandle);

  // get the stator resistance
  gMotorVars.Rs_Ohm = EST_getRs_Ohm(obj->estHandle);

  // get the stator inductance in the direct coordinate direction
  gMotorVars.Lsd_H = EST_getLs_d_H(obj->estHandle);

  // get the stator inductance in the quadrature coordinate direction
  gMotorVars.Lsq_H = EST_getLs_q_H(obj->estHandle);

  // get the flux
  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);

  // get the controller state
  gMotorVars.CtrlState = CTRL_getState(handle);

  // get the estimator state
  gMotorVars.EstState = EST_getState(obj->estHandle);
  
  if((gMotorVars.CtrlState == CTRL_State_OnLine) && (gMotorVars.Flag_MotorIdentified == true) && (Flag_Latch_softwareUpdate == false))
    {
      // set the kp and ki speed values from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_spd,gMotorVars.Kp_spd);
      CTRL_setKi(handle,CTRL_Type_PID_spd,gMotorVars.Ki_spd);

      // set the kp and ki current values for Id and Iq from the watch window
      CTRL_setKp(handle,CTRL_Type_PID_Id,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Id,gMotorVars.Ki_Idq);
      CTRL_setKp(handle,CTRL_Type_PID_Iq,gMotorVars.Kp_Idq);
      CTRL_setKi(handle,CTRL_Type_PID_Iq,gMotorVars.Ki_Idq);
	}

  // gets the Velocity Controller status
  gMotorVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle);
	
  // get the inertia setting
  gMotorVars.SpinTAC.Inertia_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // get the friction setting
  gMotorVars.SpinTAC.Friction_Aperkrpm = _IQmpy(STVELCTL_getFriction(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

  // set the SpinTAC (ST) bandwidth scale
  STVELCTL_setBandwidthScale(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBwScale);

  // gets the SpinTAC (ST) bandwidth
  gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);

  // get the Velocity Controller error
  gMotorVars.SpinTAC.VelCtlErrorID = STVELCTL_getErrorID(stObj->velCtlHandle);

  // enable/disable the forced angle
  EST_setFlag_enableForceAngle(obj->estHandle,gMotorVars.Flag_enableForceAngle);

  // enable or disable power warp
  CTRL_setFlag_enableEpl(handle,gMotorVars.Flag_enableEpl);

#ifdef DRV8301_SPI
  DRV_8301_SpiInterface(drvHandle,&gDrvSpi8301Vars);
#endif

  return;
} // end of updateGlobalVariables_motor() function

#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{

  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t fullScaleInductance = EST_getFullScaleInductance(obj->estHandle);
  float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
  int_least8_t lShift = ceil(log(obj->motorParams.Ls_d/(Ls_coarse_max*fullScaleInductance))/log(2.0));
  uint_least8_t Ls_qFmt = 30 - lShift;
  float_t L_max = fullScaleInductance * pow(2.0,lShift);
  _iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d / L_max);
  _iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q / L_max);


  // store the results
  EST_setLs_d_pu(obj->estHandle,Ls_d_pu);
  EST_setLs_q_pu(obj->estHandle,Ls_q_pu);
  EST_setLs_qFmt(obj->estHandle,Ls_qFmt);

  return;
} // end of softwareUpdate1p6() function
#endif

void calcPIgains(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  float_t Ls_d = EST_getLs_d_H(obj->estHandle);
  float_t Ls_q = EST_getLs_q_H(obj->estHandle);
  float_t Rs = EST_getRs_Ohm(obj->estHandle);
  float_t RoverLs_d = Rs/Ls_d;
  float_t RoverLs_q = Rs/Ls_q;
  float_t fullScaleCurrent = EST_getFullScaleCurrent(obj->estHandle);
  float_t fullScaleVoltage = EST_getFullScaleVoltage(obj->estHandle);
  float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
  _iq Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);
  _iq Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  _iq Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);
  _iq Kd = _IQ(0.0);

  // set the Id controller gains
  PID_setKi(obj->pidHandle_Id,Ki_Id);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd);

  // set the Iq controller gains
  PID_setKi(obj->pidHandle_Iq,Ki_Iq);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd);

  return;
} // end of calcPIgains() function


void ST_runVelCtl(ST_Handle sthandle, CTRL_Handle handle)
{

    _iq speedFeedback, iqReference, gSpdError, gUp, gUi;
    ST_Obj *stObj = (ST_Obj *)sthandle;
    CTRL_Obj *ctrlObj = (CTRL_Obj *)handle;

    // Get the mechanical speed in pu
    speedFeedback = EST_getFm_pu(ctrlObj->estHandle);

	// Run the SpinTAC Controller
	// Note that the library internal ramp generator is used to set the speed reference
    STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
	STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0));	// Internal ramp generator does not provide Acceleration Reference
	STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
	STVELCTL_run(stObj->velCtlHandle);

	// select SpinTAC Velocity Controller
	iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);

	if(gMotorVars.SpinTAC.VelCtlEnb == true) {
		// Set the Iq reference that came out of SpinTAC Velocity Control
		CTRL_setIq_ref_pu(ctrlHandle, iqReference);
		// Update internal PI integrator, if running SpinTAC
		gSpdError = TRAJ_getIntValue(ctrlObj->trajHandle_spd) - speedFeedback;
		gUp = _IQmpy(gSpdError, CTRL_getKp(handle, CTRL_Type_PID_spd));
		gUi = _IQmpy(gSpdError, CTRL_getKi(handle, CTRL_Type_PID_spd));
		CTRL_setUi(handle, CTRL_Type_PID_spd, iqReference - gUp - gUi);
	}
}
//@} //defgroup
// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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.
 * --/COPYRIGHT--*/

 
 
//! \file   solutions/instaspin_motion/src/ctrl.c
//! \brief  Contains the various functions related to the controller (CTRL) object
//!
//! (C) Copyright 2011, Texas Instruments, Inc.


// **************************************************************************
// the includes

#include <math.h>


// drivers


// modules
#include "sw/modules/dlog/src/32b/dlog4ch.h"
#include "sw/modules/math/src/32b/math.h"


// platforms
#include "ctrl.h"
#include "drv.h"
#include "user.h"


#ifdef FLASH
#pragma CODE_SECTION(CTRL_run,"ramfuncs");
#pragma CODE_SECTION(CTRL_setup,"ramfuncs");
#endif


// **************************************************************************
// the defines


// **************************************************************************
// the globals


// **************************************************************************
// the function prototypes

void CTRL_getGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   _iq *pKp,_iq *pKi,_iq *pKd)
{

  *pKp = CTRL_getKp(handle,ctrlType);
  *pKi = CTRL_getKi(handle,ctrlType);
  *pKd = CTRL_getKd(handle,ctrlType);

  return;    
} // end of CTRL_getGains() function


void CTRL_getIab_filt_pu(CTRL_Handle handle,MATH_vec2 *pIab_filt_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_filt_pu->value[0] = obj->Iab_filt.value[0];
  pIab_filt_pu->value[1] = obj->Iab_filt.value[1];

  return;
} // end of CTRL_getIab_filt_pu() function


void CTRL_getIab_in_pu(CTRL_Handle handle,MATH_vec2 *pIab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_in_pu->value[0] = obj->Iab_in.value[0];
  pIab_in_pu->value[1] = obj->Iab_in.value[1];

  return;
} // end of CTRL_getIab_in_pu() function


void CTRL_getIdq_in_pu(CTRL_Handle handle,MATH_vec2 *pIdq_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_in_pu->value[0] = obj->Idq_in.value[0];
  pIdq_in_pu->value[1] = obj->Idq_in.value[1];

  return;
} // end of CTRL_getIdq_in_pu() function


void CTRL_getIdq_ref_pu(CTRL_Handle handle,MATH_vec2 *pIdq_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_ref_pu->value[0] = obj->Idq_ref.value[0];
  pIdq_ref_pu->value[1] = obj->Idq_ref.value[1];

  return;
} // end of CTRL_getIdq_ref_pu() function


_iq CTRL_getMagCurrent_pu(CTRL_Handle handle)
{

  return(CTRL_getIdRated_pu(handle));
} // end of CTRL_getMagCurrent_pu() function


_iq CTRL_getMaximumSpeed_pu(CTRL_Handle handle)
{

  return(CTRL_getSpd_max_pu(handle));
} // end of CTRL_getMaximumSpeed_pu() function


void CTRL_getVab_in_pu(CTRL_Handle handle,MATH_vec2 *pVab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_in_pu->value[0] = obj->Vab_in.value[0];
  pVab_in_pu->value[1] = obj->Vab_in.value[1];

  return;
} // end of CTRL_getVab_in_pu() function


void CTRL_getVab_out_pu(CTRL_Handle handle,MATH_vec2 *pVab_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_out_pu->value[0] = obj->Vab_out.value[0];
  pVab_out_pu->value[1] = obj->Vab_out.value[1];

  return;
} // end of CTRL_getVab_out_pu() function


void CTRL_getVdq_out_pu(CTRL_Handle handle,MATH_vec2 *pVdq_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVdq_out_pu->value[0] = obj->Vdq_out.value[0];
  pVdq_out_pu->value[1] = obj->Vdq_out.value[1];

  return;
} // end of CTRL_getVdq_out_pu() function


void CTRL_getWaitTimes(CTRL_Handle handle,uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      pWaitTimes[stateCnt] = obj->waitTimes[stateCnt];
    }

  return;
} // end of CTRL_getWaitTimes() function


void CTRL_run(CTRL_Handle handle,DRV_Handle drvHandle,
              const DRV_AdcData_t *pAdcData,
              DRV_PwmData_t *pPwmData)
{
  uint_least16_t count_isr = CTRL_getCount_isr(handle);
  uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle);


  // if needed, run the controller
  if(count_isr >= numIsrTicksPerCtrlTick)
    {
      CTRL_State_e ctrlState = CTRL_getState(handle);

      // reset the isr count
      CTRL_resetCounter_isr(handle);

      // increment the state counter
      CTRL_incrCounter_state(handle);

      // increment the trajectory count
      CTRL_incrCounter_traj(handle);

      // run the appropriate controller
      if(ctrlState == CTRL_State_OnLine)
        {
    	  CTRL_Obj *obj = (CTRL_Obj *)handle;

          // increment the current count
          CTRL_incrCounter_current(handle);

          // increment the speed count
          CTRL_incrCounter_speed(handle);

          if(EST_getState(obj->estHandle) >= EST_State_MotorIdentified)
            {
              // run the online controller
              CTRL_runOnLine_User(handle,pAdcData,pPwmData);
            }
          else
            {
              // run the online controller
              CTRL_runOnLine(handle,pAdcData,pPwmData);
            }
        }
      else if(ctrlState == CTRL_State_OffLine)
        {
          // run the offline controller
          CTRL_runOffLine(handle,drvHandle,pAdcData,pPwmData);
        }
    }
  else
    {
      // increment the isr count
      CTRL_incrCounter_isr(handle);
    }

  return;
} // end of CTRL_run() function


void CTRL_setGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   const _iq Kp,const _iq Ki,const _iq Kd)
{

  CTRL_setKp(handle,ctrlType,Kp);
  CTRL_setKi(handle,ctrlType,Ki);
  CTRL_setKd(handle,ctrlType,Kd);

  return;    
} // end of CTRL_setGains() function


void CTRL_setMagCurrent_pu(CTRL_Handle handle,const _iq magCurrent_pu)
{

  CTRL_setIdRated_pu(handle,magCurrent_pu);

  return;    
} // end of CTRL_setMagCurrent_pu() function


void CTRL_setMaximumSpeed_pu(CTRL_Handle handle,const _iq maxSpeed_pu)
{

  CTRL_setSpd_max_pu(handle,maxSpeed_pu);

  return;    
} // end of CTRL_setMaximumSpeed_pu() function


void CTRL_setParams(CTRL_Handle handle,USER_Params *pUserParams)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq Kp,Ki,Kd;
  _iq outMin,outMax;
  _iq maxModulation;

  MATH_vec2 Iab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_ref_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_in_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vdq_out_pu = {_IQ(0.0),_IQ(0.0)};


  // assign the motor type
  CTRL_setMotorParams(handle,pUserParams->motor_type,
                      pUserParams->motor_numPolePairs,
                      pUserParams->motor_ratedFlux,
                      pUserParams->motor_Ls_d,
                      pUserParams->motor_Ls_q,
                      pUserParams->motor_Rr,
                      pUserParams->motor_Rs);


  // assign other controller parameters
  CTRL_setNumIsrTicksPerCtrlTick(handle,pUserParams->numIsrTicksPerCtrlTick);
  CTRL_setNumCtrlTicksPerCurrentTick(handle,pUserParams->numCtrlTicksPerCurrentTick);
  CTRL_setNumCtrlTicksPerSpeedTick(handle,pUserParams->numCtrlTicksPerSpeedTick);
  CTRL_setNumCtrlTicksPerTrajTick(handle,pUserParams->numCtrlTicksPerTrajTick);

  CTRL_setCtrlFreq_Hz(handle,pUserParams->ctrlFreq_Hz);
  CTRL_setTrajFreq_Hz(handle,pUserParams->trajFreq_Hz);
  CTRL_setTrajPeriod_sec(handle,_IQ(1.0/pUserParams->trajFreq_Hz));

  CTRL_setCtrlPeriod_sec(handle,pUserParams->ctrlPeriod_sec);

  CTRL_setMaxVsMag_pu(handle,_IQ(pUserParams->maxVsMag_pu));

  CTRL_setIab_in_pu(handle,&Iab_out_pu);
  CTRL_setIdq_in_pu(handle,&Idq_out_pu);
  CTRL_setIdq_ref_pu(handle,&Idq_ref_pu);

  CTRL_setIdRated_pu(handle,_IQ(pUserParams->IdRated/pUserParams->iqFullScaleCurrent_A));

  CTRL_setVab_in_pu(handle,&Vab_in_pu);
  CTRL_setVab_out_pu(handle,&Vab_out_pu);
  CTRL_setVdq_out_pu(handle,&Vdq_out_pu);

  CTRL_setSpd_out_pu(handle,_IQ(0.0));

  CTRL_setRhf(handle,0.0);
  CTRL_setLhf(handle,0.0);
  CTRL_setRoverL(handle,0.0);


  // reset the counters
  CTRL_resetCounter_current(handle);
  CTRL_resetCounter_isr(handle);
  CTRL_resetCounter_speed(handle);
  CTRL_resetCounter_state(handle);
  CTRL_resetCounter_traj(handle);


  // set the wait times for each state
  CTRL_setWaitTimes(handle,&pUserParams->ctrlWaitTime[0]);


  // set flags
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableCtrl(handle,false);
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableOffset(handle,true);
  CTRL_setFlag_enableSpeedCtrl(handle,true);
  CTRL_setFlag_enableUserMotorParams(handle,false);
  CTRL_setFlag_enableDcBusComp(handle,true);


  // initialize the controller error code
  CTRL_setErrorCode(handle,CTRL_ErrorCode_NoError);


  // set the default controller state
  CTRL_setState(handle,CTRL_State_Idle);


  // set the number of current sensors
  CTRL_setupClarke_I(handle,pUserParams->numCurrentSensors);


  // set the number of voltage sensors
  CTRL_setupClarke_V(handle,pUserParams->numVoltageSensors);


  // set the default Id PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Id,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Id,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Id,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp,Ki,Kd);


  // set the default the Iq PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Iq,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Iq,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Iq,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp,Ki,Kd);


  // set the default speed PID controller parameters
  Kp = _IQ(0.02*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A);
  Ki = _IQ(2.0*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A);
  Kd = _IQ(0.0);
  outMin = _IQ(-1.0);
  outMax = _IQ(1.0);

  PID_setGains(obj->pidHandle_spd,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_spd,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_spd,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_spd,Kp,Ki,Kd);


  // set the speed reference
  CTRL_setSpd_ref_pu(handle,_IQ(0.0));


  // set the default Id current trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_Id,_IQ(0.0));


  // set the default the speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_spd,_IQ(0.0));


  // set the default maximum speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxSlope(obj->trajHandle_spdMax,_IQ(0.0)); // not used

  
  // set the default estimator parameters
  CTRL_setEstParams(obj->estHandle,pUserParams);


  // set the maximum modulation for the SVGEN module
  maxModulation = SVGEN_4_OVER_3;
  SVGEN_setMaxModulation(obj->svgenHandle,maxModulation);

  return;
} // end of CTRL_setParams() function


void CTRL_setSpd_ref_pu(CTRL_Handle handle,const _iq spd_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_pu() function


void CTRL_setSpd_ref_krpm(CTRL_Handle handle,const _iq spd_ref_krpm)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq krpm_to_pu_sf = EST_get_krpm_to_pu_sf(obj->estHandle);

  _iq spd_ref_pu = _IQmpy(spd_ref_krpm,krpm_to_pu_sf);

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_krpm() function


void CTRL_setup(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  uint_least16_t count_traj = CTRL_getCount_traj(handle);
  uint_least16_t numCtrlTicksPerTrajTick = CTRL_getNumCtrlTicksPerTrajTick(handle);


  // as needed, update the trajectory
  if(count_traj >= numCtrlTicksPerTrajTick)
    {
      _iq intValue_Id = TRAJ_getIntValue(obj->trajHandle_Id);

      // reset the trajectory count
      CTRL_resetCounter_traj(handle);

      // run the trajectories
      CTRL_runTraj(handle);
    } // end of if(gFlag_traj) block

  return;
} // end of CTRL_setup() function


void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numCurrentSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else if(numCurrentSensors == 2)
    {
      alpha_sf = _IQ(1.0);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_I,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_I,numCurrentSensors);

  return;
} // end of CTRL_setupClarke_I() function


void CTRL_setupClarke_V(CTRL_Handle handle,uint_least8_t numVoltageSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numVoltageSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
 else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_V,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_V,numVoltageSensors);

  return;
} // end of CTRL_setupClarke_V() function


void CTRL_setWaitTimes(CTRL_Handle handle,const uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      obj->waitTimes[stateCnt] = pWaitTimes[stateCnt];
    }

  return;
} // end of CTRL_setWaitTimes() function


bool CTRL_updateState(CTRL_Handle handle)
{
  CTRL_State_e ctrlState = CTRL_getState(handle);
  bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
  bool stateChanged = false;


  if(flag_enableCtrl)
    {
      uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
      uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);


      // check for errors
      CTRL_checkForErrors(handle);


      if(counter_ctrlState >= waitTime)
        {
          // reset the counter
          CTRL_resetCounter_state(handle);


          if(ctrlState == CTRL_State_OnLine)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              _iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);

              // update the estimator state
              bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);

              if(flag_estStateChanged)
                {
                  // setup the controller
                  CTRL_setupCtrl(handle);

                  // setup the trajectory
                  CTRL_setupTraj(handle);
                }

              if(EST_isOnLine(obj->estHandle))
                {
                  // setup the estimator for online state
                  CTRL_setupEstOnLineState(handle);
                }

              if(EST_isLockRotor(obj->estHandle) || 
                 (EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
                {
                  // set the enable controller flag to false
                  CTRL_setFlag_enableCtrl(handle,false);

                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_Idle);
                }
            }
          else if(ctrlState == CTRL_State_OffLine)
            {
              // set the next controller state
              CTRL_setState(handle,CTRL_State_OnLine);
            }
          else if(ctrlState == CTRL_State_Idle)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              bool  flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);

              if(flag_enableUserMotorParams)
                {
                  // initialize the motor parameters using values from the user.h file
                  CTRL_setUserMotorParams(handle);
                }

              if(EST_isIdle(obj->estHandle))
                {
                  // setup the estimator for idle state
                  CTRL_setupEstIdleState(handle);

                  if(EST_isMotorIdentified(obj->estHandle))
                    {
                      if(CTRL_getFlag_enableOffset(handle))
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OffLine);
                        }
                      else
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OnLine);
                        }
                    }
                  else
                    {
                      // set the next controller state
                      CTRL_setState(handle,CTRL_State_OffLine);
                    }
                }
              else if(EST_isLockRotor(obj->estHandle))
                {
                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_OnLine);
                }
            }
        }  // if(counter_ctrlState >= waitTime) loop
    } 
  else
    {
      CTRL_Obj *obj = (CTRL_Obj *)handle;

      // set the next controller state
      CTRL_setState(handle,CTRL_State_Idle);

      // set the estimator to idle
      if(!EST_isLockRotor(obj->estHandle))
        {
          if(EST_isMotorIdentified(obj->estHandle))
            {
              EST_setIdle(obj->estHandle);
            }
          else
            {
              EST_setIdle_all(obj->estHandle);

              EST_setRs_pu(obj->estHandle,_IQ30(0.0));
            }
        }
    }


  // check to see if the state changed
  if(ctrlState != CTRL_getState(handle))
    {
      stateChanged = true;
    }

  return(stateChanged);
} // end of CTRL_updateState() function

// end of file

/* --COPYRIGHT--,BSD
 * Copyright (c) 2012, Texas Instruments Incorporated
 * All rights reserved.
 *
 * 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.
 * --/COPYRIGHT--*/

 
 
//! \file   solutions/instaspin_motion/src/ctrl.c
//! \brief  Contains the various functions related to the controller (CTRL) object
//!
//! (C) Copyright 2011, Texas Instruments, Inc.


// **************************************************************************
// the includes

#include <math.h>


// drivers


// modules
#include "sw/modules/dlog/src/32b/dlog4ch.h"
#include "sw/modules/math/src/32b/math.h"


// platforms
#include "ctrl.h"
#include "drv.h"
#include "user.h"


#ifdef FLASH
#pragma CODE_SECTION(CTRL_run,"ramfuncs");
#pragma CODE_SECTION(CTRL_setup,"ramfuncs");
#endif


// **************************************************************************
// the defines


// **************************************************************************
// the globals


// **************************************************************************
// the function prototypes

void CTRL_getGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   _iq *pKp,_iq *pKi,_iq *pKd)
{

  *pKp = CTRL_getKp(handle,ctrlType);
  *pKi = CTRL_getKi(handle,ctrlType);
  *pKd = CTRL_getKd(handle,ctrlType);

  return;    
} // end of CTRL_getGains() function


void CTRL_getIab_filt_pu(CTRL_Handle handle,MATH_vec2 *pIab_filt_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_filt_pu->value[0] = obj->Iab_filt.value[0];
  pIab_filt_pu->value[1] = obj->Iab_filt.value[1];

  return;
} // end of CTRL_getIab_filt_pu() function


void CTRL_getIab_in_pu(CTRL_Handle handle,MATH_vec2 *pIab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIab_in_pu->value[0] = obj->Iab_in.value[0];
  pIab_in_pu->value[1] = obj->Iab_in.value[1];

  return;
} // end of CTRL_getIab_in_pu() function


void CTRL_getIdq_in_pu(CTRL_Handle handle,MATH_vec2 *pIdq_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_in_pu->value[0] = obj->Idq_in.value[0];
  pIdq_in_pu->value[1] = obj->Idq_in.value[1];

  return;
} // end of CTRL_getIdq_in_pu() function


void CTRL_getIdq_ref_pu(CTRL_Handle handle,MATH_vec2 *pIdq_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pIdq_ref_pu->value[0] = obj->Idq_ref.value[0];
  pIdq_ref_pu->value[1] = obj->Idq_ref.value[1];

  return;
} // end of CTRL_getIdq_ref_pu() function


_iq CTRL_getMagCurrent_pu(CTRL_Handle handle)
{

  return(CTRL_getIdRated_pu(handle));
} // end of CTRL_getMagCurrent_pu() function


_iq CTRL_getMaximumSpeed_pu(CTRL_Handle handle)
{

  return(CTRL_getSpd_max_pu(handle));
} // end of CTRL_getMaximumSpeed_pu() function


void CTRL_getVab_in_pu(CTRL_Handle handle,MATH_vec2 *pVab_in_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_in_pu->value[0] = obj->Vab_in.value[0];
  pVab_in_pu->value[1] = obj->Vab_in.value[1];

  return;
} // end of CTRL_getVab_in_pu() function


void CTRL_getVab_out_pu(CTRL_Handle handle,MATH_vec2 *pVab_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVab_out_pu->value[0] = obj->Vab_out.value[0];
  pVab_out_pu->value[1] = obj->Vab_out.value[1];

  return;
} // end of CTRL_getVab_out_pu() function


void CTRL_getVdq_out_pu(CTRL_Handle handle,MATH_vec2 *pVdq_out_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  pVdq_out_pu->value[0] = obj->Vdq_out.value[0];
  pVdq_out_pu->value[1] = obj->Vdq_out.value[1];

  return;
} // end of CTRL_getVdq_out_pu() function


void CTRL_getWaitTimes(CTRL_Handle handle,uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      pWaitTimes[stateCnt] = obj->waitTimes[stateCnt];
    }

  return;
} // end of CTRL_getWaitTimes() function


void CTRL_run(CTRL_Handle handle,DRV_Handle drvHandle,
              const DRV_AdcData_t *pAdcData,
              DRV_PwmData_t *pPwmData)
{
  uint_least16_t count_isr = CTRL_getCount_isr(handle);
  uint_least16_t numIsrTicksPerCtrlTick = CTRL_getNumIsrTicksPerCtrlTick(handle);


  // if needed, run the controller
  if(count_isr >= numIsrTicksPerCtrlTick)
    {
      CTRL_State_e ctrlState = CTRL_getState(handle);

      // reset the isr count
      CTRL_resetCounter_isr(handle);

      // increment the state counter
      CTRL_incrCounter_state(handle);

      // increment the trajectory count
      CTRL_incrCounter_traj(handle);

      // run the appropriate controller
      if(ctrlState == CTRL_State_OnLine)
        {
    	  CTRL_Obj *obj = (CTRL_Obj *)handle;

          // increment the current count
          CTRL_incrCounter_current(handle);

          // increment the speed count
          CTRL_incrCounter_speed(handle);

          if(EST_getState(obj->estHandle) >= EST_State_MotorIdentified)
            {
              // run the online controller
              CTRL_runOnLine_User(handle,pAdcData,pPwmData);
            }
          else
            {
              // run the online controller
              CTRL_runOnLine(handle,pAdcData,pPwmData);
            }
        }
      else if(ctrlState == CTRL_State_OffLine)
        {
          // run the offline controller
          CTRL_runOffLine(handle,drvHandle,pAdcData,pPwmData);
        }
    }
  else
    {
      // increment the isr count
      CTRL_incrCounter_isr(handle);
    }

  return;
} // end of CTRL_run() function


void CTRL_setGains(CTRL_Handle handle,const CTRL_Type_e ctrlType,
                   const _iq Kp,const _iq Ki,const _iq Kd)
{

  CTRL_setKp(handle,ctrlType,Kp);
  CTRL_setKi(handle,ctrlType,Ki);
  CTRL_setKd(handle,ctrlType,Kd);

  return;    
} // end of CTRL_setGains() function


void CTRL_setMagCurrent_pu(CTRL_Handle handle,const _iq magCurrent_pu)
{

  CTRL_setIdRated_pu(handle,magCurrent_pu);

  return;    
} // end of CTRL_setMagCurrent_pu() function


void CTRL_setMaximumSpeed_pu(CTRL_Handle handle,const _iq maxSpeed_pu)
{

  CTRL_setSpd_max_pu(handle,maxSpeed_pu);

  return;    
} // end of CTRL_setMaximumSpeed_pu() function


void CTRL_setParams(CTRL_Handle handle,USER_Params *pUserParams)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq Kp,Ki,Kd;
  _iq outMin,outMax;
  _iq maxModulation;

  MATH_vec2 Iab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Idq_ref_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_in_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vab_out_pu = {_IQ(0.0),_IQ(0.0)};
  MATH_vec2 Vdq_out_pu = {_IQ(0.0),_IQ(0.0)};


  // assign the motor type
  CTRL_setMotorParams(handle,pUserParams->motor_type,
                      pUserParams->motor_numPolePairs,
                      pUserParams->motor_ratedFlux,
                      pUserParams->motor_Ls_d,
                      pUserParams->motor_Ls_q,
                      pUserParams->motor_Rr,
                      pUserParams->motor_Rs);


  // assign other controller parameters
  CTRL_setNumIsrTicksPerCtrlTick(handle,pUserParams->numIsrTicksPerCtrlTick);
  CTRL_setNumCtrlTicksPerCurrentTick(handle,pUserParams->numCtrlTicksPerCurrentTick);
  CTRL_setNumCtrlTicksPerSpeedTick(handle,pUserParams->numCtrlTicksPerSpeedTick);
  CTRL_setNumCtrlTicksPerTrajTick(handle,pUserParams->numCtrlTicksPerTrajTick);

  CTRL_setCtrlFreq_Hz(handle,pUserParams->ctrlFreq_Hz);
  CTRL_setTrajFreq_Hz(handle,pUserParams->trajFreq_Hz);
  CTRL_setTrajPeriod_sec(handle,_IQ(1.0/pUserParams->trajFreq_Hz));

  CTRL_setCtrlPeriod_sec(handle,pUserParams->ctrlPeriod_sec);

  CTRL_setMaxVsMag_pu(handle,_IQ(pUserParams->maxVsMag_pu));

  CTRL_setIab_in_pu(handle,&Iab_out_pu);
  CTRL_setIdq_in_pu(handle,&Idq_out_pu);
  CTRL_setIdq_ref_pu(handle,&Idq_ref_pu);

  CTRL_setIdRated_pu(handle,_IQ(pUserParams->IdRated/pUserParams->iqFullScaleCurrent_A));

  CTRL_setVab_in_pu(handle,&Vab_in_pu);
  CTRL_setVab_out_pu(handle,&Vab_out_pu);
  CTRL_setVdq_out_pu(handle,&Vdq_out_pu);

  CTRL_setSpd_out_pu(handle,_IQ(0.0));

  CTRL_setRhf(handle,0.0);
  CTRL_setLhf(handle,0.0);
  CTRL_setRoverL(handle,0.0);


  // reset the counters
  CTRL_resetCounter_current(handle);
  CTRL_resetCounter_isr(handle);
  CTRL_resetCounter_speed(handle);
  CTRL_resetCounter_state(handle);
  CTRL_resetCounter_traj(handle);


  // set the wait times for each state
  CTRL_setWaitTimes(handle,&pUserParams->ctrlWaitTime[0]);


  // set flags
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableCtrl(handle,false);
  CTRL_setFlag_enableEpl(handle,false);
  CTRL_setFlag_enableOffset(handle,true);
  CTRL_setFlag_enableSpeedCtrl(handle,true);
  CTRL_setFlag_enableUserMotorParams(handle,false);
  CTRL_setFlag_enableDcBusComp(handle,true);


  // initialize the controller error code
  CTRL_setErrorCode(handle,CTRL_ErrorCode_NoError);


  // set the default controller state
  CTRL_setState(handle,CTRL_State_Idle);


  // set the number of current sensors
  CTRL_setupClarke_I(handle,pUserParams->numCurrentSensors);


  // set the number of voltage sensors
  CTRL_setupClarke_V(handle,pUserParams->numVoltageSensors);


  // set the default Id PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Id,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Id,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Id,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Id,Kp,Ki,Kd);


  // set the default the Iq PID controller parameters
  Kp = _IQ(0.1);
  Ki = _IQ(pUserParams->ctrlPeriod_sec/0.004);
  Kd = _IQ(0.0);
  outMin = _IQ(-0.95);
  outMax = _IQ(0.95);

  PID_setGains(obj->pidHandle_Iq,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_Iq,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_Iq,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp,Ki,Kd);


  // set the default speed PID controller parameters
  Kp = _IQ(0.02*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz/pUserParams->iqFullScaleCurrent_A);
  Ki = _IQ(2.0*pUserParams->maxCurrent*pUserParams->iqFullScaleFreq_Hz*pUserParams->ctrlPeriod_sec/pUserParams->iqFullScaleCurrent_A);
  Kd = _IQ(0.0);
  outMin = _IQ(-1.0);
  outMax = _IQ(1.0);

  PID_setGains(obj->pidHandle_spd,Kp,Ki,Kd);
  PID_setUi(obj->pidHandle_spd,_IQ(0.0));
  PID_setMinMax(obj->pidHandle_spd,outMin,outMax);
  CTRL_setGains(handle,CTRL_Type_PID_spd,Kp,Ki,Kd);


  // set the speed reference
  CTRL_setSpd_ref_pu(handle,_IQ(0.0));


  // set the default Id current trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_Id,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_Id,_IQ(0.0));


  // set the default the speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxValue(obj->trajHandle_spd,_IQ(0.0));
  TRAJ_setMaxSlope(obj->trajHandle_spd,_IQ(0.0));


  // set the default maximum speed trajectory module parameters
  TRAJ_setIntValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(0.0));
  TRAJ_setMinValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxValue(obj->trajHandle_spdMax,_IQ(0.0)); // not used
  TRAJ_setMaxSlope(obj->trajHandle_spdMax,_IQ(0.0)); // not used

  
  // set the default estimator parameters
  CTRL_setEstParams(obj->estHandle,pUserParams);


  // set the maximum modulation for the SVGEN module
  maxModulation = SVGEN_4_OVER_3;
  SVGEN_setMaxModulation(obj->svgenHandle,maxModulation);

  return;
} // end of CTRL_setParams() function


void CTRL_setSpd_ref_pu(CTRL_Handle handle,const _iq spd_ref_pu)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_pu() function


void CTRL_setSpd_ref_krpm(CTRL_Handle handle,const _iq spd_ref_krpm)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq krpm_to_pu_sf = EST_get_krpm_to_pu_sf(obj->estHandle);

  _iq spd_ref_pu = _IQmpy(spd_ref_krpm,krpm_to_pu_sf);

  obj->spd_ref = spd_ref_pu;

  return;
} // end of CTRL_setSpd_ref_krpm() function


void CTRL_setup(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  uint_least16_t count_traj = CTRL_getCount_traj(handle);
  uint_least16_t numCtrlTicksPerTrajTick = CTRL_getNumCtrlTicksPerTrajTick(handle);


  // as needed, update the trajectory
  if(count_traj >= numCtrlTicksPerTrajTick)
    {
      _iq intValue_Id = TRAJ_getIntValue(obj->trajHandle_Id);

      // reset the trajectory count
      CTRL_resetCounter_traj(handle);

      // run the trajectories
      CTRL_runTraj(handle);
    } // end of if(gFlag_traj) block

  return;
} // end of CTRL_setup() function


void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numCurrentSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else if(numCurrentSensors == 2)
    {
      alpha_sf = _IQ(1.0);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
  else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_I,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_I,numCurrentSensors);

  return;
} // end of CTRL_setupClarke_I() function


void CTRL_setupClarke_V(CTRL_Handle handle,uint_least8_t numVoltageSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

  // initialize the Clarke transform module for current
  if(numVoltageSensors == 3)
    {
      alpha_sf = _IQ(MATH_ONE_OVER_THREE);
      beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
    }
 else 
    {
      alpha_sf = _IQ(0.0);
      beta_sf = _IQ(0.0);
    }

  // set the parameters
  CLARKE_setScaleFactors(obj->clarkeHandle_V,alpha_sf,beta_sf);
  CLARKE_setNumSensors(obj->clarkeHandle_V,numVoltageSensors);

  return;
} // end of CTRL_setupClarke_V() function


void CTRL_setWaitTimes(CTRL_Handle handle,const uint_least32_t *pWaitTimes)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  uint_least16_t stateCnt;

  for(stateCnt=0;stateCnt<CTRL_numStates;stateCnt++)
    {
      obj->waitTimes[stateCnt] = pWaitTimes[stateCnt];
    }

  return;
} // end of CTRL_setWaitTimes() function


bool CTRL_updateState(CTRL_Handle handle)
{
  CTRL_State_e ctrlState = CTRL_getState(handle);
  bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
  bool stateChanged = false;


  if(flag_enableCtrl)
    {
      uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
      uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);


      // check for errors
      CTRL_checkForErrors(handle);


      if(counter_ctrlState >= waitTime)
        {
          // reset the counter
          CTRL_resetCounter_state(handle);


          if(ctrlState == CTRL_State_OnLine)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              _iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);

              // update the estimator state
              bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);

              if(flag_estStateChanged)
                {
                  // setup the controller
                  CTRL_setupCtrl(handle);

                  // setup the trajectory
                  CTRL_setupTraj(handle);
                }

              if(EST_isOnLine(obj->estHandle))
                {
                  // setup the estimator for online state
                  CTRL_setupEstOnLineState(handle);
                }

              if(EST_isLockRotor(obj->estHandle) || 
                 (EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
                {
                  // set the enable controller flag to false
                  CTRL_setFlag_enableCtrl(handle,false);

                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_Idle);
                }
            }
          else if(ctrlState == CTRL_State_OffLine)
            {
              // set the next controller state
              CTRL_setState(handle,CTRL_State_OnLine);
            }
          else if(ctrlState == CTRL_State_Idle)
            {
              CTRL_Obj *obj = (CTRL_Obj *)handle;
              bool  flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);

              if(flag_enableUserMotorParams)
                {
                  // initialize the motor parameters using values from the user.h file
                  CTRL_setUserMotorParams(handle);
                }

              if(EST_isIdle(obj->estHandle))
                {
                  // setup the estimator for idle state
                  CTRL_setupEstIdleState(handle);

                  if(EST_isMotorIdentified(obj->estHandle))
                    {
                      if(CTRL_getFlag_enableOffset(handle))
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OffLine);
                        }
                      else
                        {
                          // set the next controller state
                          CTRL_setState(handle,CTRL_State_OnLine);
                        }
                    }
                  else
                    {
                      // set the next controller state
                      CTRL_setState(handle,CTRL_State_OffLine);
                    }
                }
              else if(EST_isLockRotor(obj->estHandle))
                {
                  // set the next controller state
                  CTRL_setState(handle,CTRL_State_OnLine);
                }
            }
        }  // if(counter_ctrlState >= waitTime) loop
    } 
  else
    {
      CTRL_Obj *obj = (CTRL_Obj *)handle;

      // set the next controller state
      CTRL_setState(handle,CTRL_State_Idle);

      // set the estimator to idle
      if(!EST_isLockRotor(obj->estHandle))
        {
          if(EST_isMotorIdentified(obj->estHandle))
            {
              EST_setIdle(obj->estHandle);
            }
          else
            {
              EST_setIdle_all(obj->estHandle);

              EST_setRs_pu(obj->estHandle,_IQ30(0.0));
            }
        }
    }


  // check to see if the state changed
  if(ctrlState != CTRL_getState(handle))
    {
      stateChanged = true;
    }

  return(stateChanged);
} // end of CTRL_updateState() function

// end of file