Dynamometer for Stepper Motors

/* --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_lab12b.c
//! \brief  SpinTAC Velocity Controller using a quadrature encoder for feedback
//!
//! (C) Copyright 2012, LineStream Technologies, Inc.
//! (C) Copyright 2011, Texas Instruments, Inc.

//! \defgroup PROJ_LAB12b PROJ_LAB12b
//@{

//! \defgroup PROJ_LAB12b_OVERVIEW Project Overview
//!
//! SpinTAC Velocity Controller using a quadrature encoder for feedback
//!

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

// system includes
#include <math.h>
#include "main.h"
#include<stdio.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

int TORQUE_CTRL = 1; //Edit by Jon Bright:  Used to choose between torque control or speed control. TORQUE_CTRL=1 means torque control. TORQUE_CTRL=0 means speed control

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);

//***Jon Bright 04/03/19 Custom profile variables
_iq Torque_1=0; //Edited by Raisaat, commented by Jon Bright: This variable is for displaying the torque in the GUI.
_iq Torque_2=0; //Edit by Jon Bright: This variable is for displaying the torque in the GUI.
_iq SpeedQEP_krpm_1 = 0; //Edited by Raisaat, commented by Jon Bright: This variable is for displaying the speed in the GUI.
//***********Jon Bright 03/04/19 Global variables needed for inserting torque profile
float estimated_frequency=8000; //Variable used to determine how often the estimated clock in this code runs. Controls the timing speed of the algorithms. The default value right now is 8000Hz.  This is about twice as fast as seconds.  The real value is probably between 4000Hz and 6000Hz, but it may run at different speeds depending on the speed of the code
long Iq_refA_array[2]; //This will hold the max and min torque in the Long format.  The code only interprets the current so this is used to hold those values.
//****Timing
float TOTAL_TIME_updateGlobalVariables=0; //Used to hold the total time in the system based on our estimated clock.
float check_period_updateGlobalVariables=0; //Used to hold the time for a period.  This is used in the torque profile control algorithms
float check_half_period_updateGlobalVariables=0; //Used to hold the time for half of the period.  This is used in the torque profile control algorithms
float count_over_frequency=0; //This is the number of times the code runs through the updateIqRef function.  If you take this over frequency you get seconds.  However we don't know the exact frequency.

//******

float custom_torque_array[100];//Used to hold the torque array for the custom profile. Only 100 spaces available.
long custom_IQ_Iq_refA_array[100];//Used to hold the torque array for the custom profile. Only 100 spaces available.
float custom_time_array[100]; //Used to hold the time array for the custom profile. Only 100 spaces available.
float *custom_time_array_ptr=&custom_time_array[0]; //Pointer for the time array. Needed to move through time arrays
float desired_interval; //This will be the desired interval for the changing of time.
//***End of Custom Profile Variables

long *Iq_RefA_ptr=&Iq_refA_array[0];//Initialization of pointer for the Iq_ref_array. Will be used for all profiles
float Max_Torque=0; //The Max Torque desired for a set profile. Needs to be in Nm.  This will be converted to Iq Current A value by multiplying this value by the Torque_to_Iq_A_multiplier
float Min_Torque=0;//The Max TOrque desired for a set profile. Need to be in Nm. This will be converted to Iq Current A value by multiplying this value by the Torque_to_Iq_A_multiplier
float Max_Iq=0;//Max IQ value for torque profile. This will be taken in the GUI as torque in Nm.
float Min_Iq=0;//Min IQ value for torque profile.  This will be taken in the GUI as torque in Nm.
//int initialize_torque_profile=0; //This is for the GUI.  When it is 1 the Torque_Profile_Populate() function will run. (I dont think this is necessary as the variable that is used instead is initialize_torque)
float Torque_to_Iq_A_multiplier=20;//This constant is for converting torque input from the GUI to the Iq_RefA that the code needs to control the BLDC motor.  If the torque that is measured on the GUI is less than the expected torque than this value can be increased (thereby increasing current) to make it more accurate.  The opposite can be done if hte torque is greater than expected.  This was measured by plotting Torque vs Iq_RefA

float _PI=3.146;//Needed for sine wave
float _2PI=6.92;//Needed for sine wave
_iq24 IQ_slope_ramp_Iq;//Needed for slope of ramp function
_iq24 IQ_neg_slope_ramp_Iq;//Needed for negative slope of the ramp function
_iq24 IQ_PI;//PI is converted to an IQ24 value for calculations
_iq24 IQ_2PI;//2PI is converted to an IQ24 value for calculations
_iq24 IQMax_Iq;//Converts max torque value to IQ value that we will use to update current IqA control
_iq24 IQMin_Iq;//Converts max torque value to IQ value that we will use to update current IqA control
_iq24 Y_long=0; //Value that will be given to gMotor.Vars.Iq_RefA that determines the Iq_A that controls torque
_iq24 IQAvg_Iq; //Needed for sinusoid offset.
_iq24 IQ_2pi_div_period; //Needed for y=a*sin(bx)+c.  b=2pi/period
_iq24 IQSine_Amplitude_Iq; //Needed for y=a*sin(bx)+c.  a=(Max_torque-Min_torque)/2
_iq24 graph_intended_torque_Nm; //Needed to graph what the torque should be based on Iq_RefA times multiplier
float period_GUI=10; //This variable is in the GUI.  When it is changed in the GUI the actual period does not change until Torque_Profile_Populate() runs. It is defaultly set to 10 to prevent a defalut value of Xe-19 or Xe19
float period;//This is the actual period used in the algorithms.  It doesn't change until Torque_Profile_Populate() runs.  The period was made this way because we had a problem with the period changing in the middle of running a profile if we tried to set up the next profile.  Now, no settings change unless Initialize Torque Profile button is pushed
_iq24 IQ_period; //The period in IQ24 value for calculations in the profile algorithms
float half_period;// Needed for some calculations
_iq24 IQ_half_period; //The half period in IQ24 for calculations in the profile algorithms
int Torque_Profile_Description=0; //This determines which profile is active.  0 for step/pulse profile, 1 for ramp profile, 2 for sine profile, 3 for custom profile.  (Note: Constant Torque is a separate function and is not included in the profiles)
int Torque_Profile_Description_GUI=0;//This is the value in the GUI that changes.  When Torque_Profile_Populate() is ran the actual Torque_Profile_Description variable changes
long clock_count=0; //clock count which counts how many times Iq_A is updated.  It is put over frequency, clock_count/estimated_frequency, to get time
int clock_cycles=0; //Counts how many times we change the array location which should be at a rate of period/2
int initialize_torque=0; //Needed to reset torque profiles and change the variables for a new profile
int status_profile_initiated=0; //This is for the GUI.  It just lights up when a new GUI has been initialized
float count_in_one_cycle=0; //Used to count seconds below 1 second. GIves the decimal number so we can count to an order less than seconds.
int clock_count_period=0; //Counts the seconds.  Increments every time clock count >= estimated frequency. Resets every period
int clock_count_half_period=0; //Counts the seconds for half period.  Resets every half period
int time_total=0; //ADD after TEST. Keeps the total time in seconds.  It doesn't reset unlike period and half period
float time_period=0; //Keeps the total time

//Free/Constant Torque Control Variables
int free_torque_ctrl_on=1; //Variable used to control whether constant torque is on or not.  This can be called constant torque profile or free torque profile because the torque will just be whatever value is input to the constant/free torque input
float free_torque_ctrl_Nm=0.0001;//Variable for constant torque control value.  This is defaultly set to .0001 because otherwise it would be a Xe-19 or Xe19 number
//End of free/constant torque control variables
//Safeguards
int error_torque_too_high=0;//If a torque value is input that is above the max_allowed_torque_Nm value hten this LED will turn on and the Torque entered will change to zero
float max_allowed_torque_Nm=.16; //Highest constant torque allowed from the BLDC motor. *************This needs to be changed according to the BLDC motor and the stepper.  You don't want to overpower the stepper as that may cause an error. At least it messed some stuff up for us.  The Teknic BLDC motor that we are using has a max torque of .27Nm, but the stepper is weaker.
_iq24 IQmax_allowed_torque_Iq_A=0; //Highest constant torque allowed in Iq_A form
//End of Safeguards
int Already_initialize_torque=0;//Variable to indicate that torque profile has already been initialized.  This is to prevent the code from constantly running Torque_Profile_Populate()
//Variables for status bar to indicate BLDC motor is running
int motor_Est_State_Rs=0; //This is a variable for the GUI to show the motors run status
int motor_Est_State_On_Line=0; //This is a variable for the GUI to show the motors run status
//****************End of Jon Bright global variables

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

ENC_Handle encHandle;
ENC_Obj enc;

SLIP_Handle slipHandle;
SLIP_Obj slip;

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
#ifdef DRV8305_SPI
// Watch window interface to the 8305 SPI
DRV_SPI_8305_Vars_t gDrvSpi8305Vars;
#endif

_iq gFlux_pu_to_Wb_sf;

_iq gFlux_pu_to_VpHz_sf;

_iq gTorque_Ls_Id_Iq_pu_to_Nm_sf;

_iq gTorque_Flux_Iq_pu_to_Nm_sf;

//int num_x;

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

void updateIqRef(CTRL_Handle);
void Torque_Profile_Populate();
void Torque_Profile_Type();

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

void main(void)
{
  //start = time(NULL);
    uint_least8_t estNumber = 0;
    //Jon Bright edit.  Initializing custom torque profile array. This is to prevent a mistake where profile 3 is selected and it runs somethingit can't comprehend so current goes really high
    int a;
    for (a=0; a<100; a++)
    {
        custom_torque_array[a]=0.0001;
        custom_time_array[a]=0;
    }
    //end of Jon Bright edit
    //Jon Bright edit 04/11/19.  Sets boundary for torque control for this specific motor. It has a max constant torque of .27Nm
    IQmax_allowed_torque_Iq_A=_IQmpy(_IQ24(max_allowed_torque_Nm),_IQ24(Torque_to_Iq_A_multiplier));
    //End of Jon Bright
#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)
  controller_obj = (CTRL_Obj *)ctrlHandle;
#else
  ctrlHandle = CTRL_initCtrl(estNumber,&ctrl,sizeof(ctrl)); //v1p7 format default
#endif


  {
    CTRL_Version version;

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

    gMotorVars.CtrlVersion = version;
  }


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


  // 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);


  // initialize the ENC module
  encHandle = ENC_init(&enc, sizeof(enc));


  // setup the ENC module
  ENC_setup(encHandle, 1, USER_MOTOR_NUM_POLE_PAIRS, USER_MOTOR_ENCODER_LINES, 0, USER_IQ_FULL_SCALE_FREQ_Hz, USER_ISR_FREQ_Hz, 8000.0);


  // initialize the SLIP module
  slipHandle = SLIP_init(&slip, sizeof(slip));


  // setup the SLIP module
  SLIP_setup(slipHandle, _IQ(gUserParams.ctrlPeriod_sec));

  //**************Re-edited by Jon Bright.  Very important that SpinTac components are setup in order for the encoder and the speed control to work.  This is why the if statement is taken away. The following is necessary for reading position
    //if (TORQUE_CTRL == 0)
    //{// initialize the SpinTAC Components
    stHandle = ST_init(&st_obj, sizeof(st_obj));

    // setup the SpinTAC Components
    ST_setupVelCtl(stHandle);
    ST_setupPosConv(stHandle);//}
  //***********end of Jon Bright edit

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

#ifdef DRV8305_SPI
  // turn on the DRV8305 if present
  HAL_enableDrv(halHandle);
  // initialize the DRV8305 interface
  HAL_setupDrvSpi(halHandle,&gDrvSpi8305Vars);
#endif

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


  // compute scaling factors for flux and torque calculations
  gFlux_pu_to_Wb_sf = USER_computeFlux_pu_to_Wb_sf();
  gFlux_pu_to_VpHz_sf = USER_computeFlux_pu_to_VpHz_sf();
  gTorque_Ls_Id_Iq_pu_to_Nm_sf = USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf();
  gTorque_Flux_Iq_pu_to_Nm_sf = USER_computeTorque_Flux_Iq_pu_to_Nm_sf();


  for(;;)
  {
      //******************************************************************************

    // Waiting for enable system flag to be set
    while(!(gMotorVars.Flag_enableSys))
    {
        gMotorVars.IqRef_A=_IQ(0.001);//The initialization of the torque will be 0.001 to prevent a current that the system can't handle.  Also to prevent 0, because the code does not work well when the torque is equal to 0
        //*****The following algorithm is used to initialize the torque when the motor is off.  This was made to prevent free/constant torque ctrl and torque profile to be active at the same time.
        if (TORQUE_CTRL == 1)
        {
            motor_Est_State_Rs=0;//Initializes this GUI status to 0
            motor_Est_State_On_Line=0;//Initializes this GUI status to 0
            if (Already_initialize_torque==0)
            {
                if (initialize_torque==1)//Jon Bright 04/02/19
                {
                    Torque_Profile_Populate();//********Jon Bright 03/26/19 Initializes the torque values
                }
            }
            if (free_torque_ctrl_on==1)
            {
                Y_long=_IQmpy(_IQ24(free_torque_ctrl_Nm),_IQ24(Torque_to_Iq_A_multiplier));

                if (Y_long > IQmax_allowed_torque_Iq_A)
                {
                    free_torque_ctrl_Nm=0;
                    error_torque_too_high=1;
                }
            }
            if (Already_initialize_torque==1)
            {
                if (initialize_torque==1)
                {
                    Torque_Profile_Populate();
                }
            }
        }//End of edit for initialization
    }

    // 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);
        //CTRL_setFlag_enableUserMotorParams(ctrlHandle,1);

        // 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);
            //CTRL_setFlag_enableCtrl(ctrlHandle, 1);

            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)
                  {
                    if(gMotorVars.Flag_enableOffsetcalc == true)
                    {
                      // update the ADC bias values
                      HAL_updateAdcBias(halHandle);
                    }
                    else
                    {
                      // 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));
                    }

                    // Return the bias value for currents
                    gMotorVars.I_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Current,0);
                    gMotorVars.I_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Current,1);
                    gMotorVars.I_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Current,2);

                    // Return the bias value for voltages
                    gMotorVars.V_bias.value[0] = HAL_getBias(halHandle,HAL_SensorType_Voltage,0);
                    gMotorVars.V_bias.value[1] = HAL_getBias(halHandle,HAL_SensorType_Voltage,1);
                    gMotorVars.V_bias.value[2] = HAL_getBias(halHandle,HAL_SensorType_Voltage,2);

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

                if((CTRL_getFlag_enableUserMotorParams(ctrlHandle) == true) &&
                  (ctrlState > CTRL_State_Idle) &&
                  (gMotorVars.CtrlVersion.minor == 6))
                  {
                    // call this function to fix 1p6
                    USER_softwareUpdate1p6(ctrlHandle);
                  }

              }
          }


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


            if (TORQUE_CTRL == 0)//Edited by Raisaat to include TORQUE_CTRL, from Jonathan: If speed Control is the following is active. Speed is set by the speed reference
            {
                        // 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 (TORQUE_CTRL == 1)//Edited by Raisaat to include TORQUE_CTRL, from Jonathan: If torque Control is selected the following is active. SpinTAC speed controller is disabled
            {
                        // disable the SpinTAC Speed Controller
                        STVELCTL_setEnable(stObj->velCtlHandle, false);
            }


            if (TORQUE_CTRL == 0) //Edited by Raisaat to include TORQUE_CTRL, from Jonathan: If speed Control is selected the following is active.. SpinTAC speed controller is enabled
            {
                        // enable the SpinTAC Speed Controller
                        STVELCTL_setEnable(stObj->velCtlHandle, true);
            }
            //#endif

            if(EST_getState(obj->estHandle) != EST_State_OnLine)
            {
                // if the estimator is not running, place SpinTAC into reset
                STVELCTL_setEnable(stObj->velCtlHandle, false);
            }

            if(Flag_Latch_softwareUpdate)
            {
              Flag_Latch_softwareUpdate = false;

              USER_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 Bw value with the default value
              gMotorVars.SpinTAC.VelCtlBw_radps = STVELCTL_getBandwidth_radps(stObj->velCtlHandle);

              // initialize the watch window with maximum and minimum Iq reference
              gMotorVars.SpinTAC.VelCtlOutputMax_A = _IQmpy(STVELCTL_getOutputMaximum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
              gMotorVars.SpinTAC.VelCtlOutputMin_A = _IQmpy(STVELCTL_getOutputMinimum(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
            }
//#endif
          }
        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);
          }


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

        if (TORQUE_CTRL == 0) //************Edited by Raisaat, from Jonathan: If speed Control is selected the following is active.
        {
                // set the SpinTAC (ST) bandwidth scale
                STVELCTL_setBandwidth_radps(stObj->velCtlHandle, gMotorVars.SpinTAC.VelCtlBw_radps);

                // set the maximum and minimum values for Iq reference
                STVELCTL_setOutputMaximums(stObj->velCtlHandle, _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMax_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)), _IQmpy(gMotorVars.SpinTAC.VelCtlOutputMin_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
        }


        if (TORQUE_CTRL == 1)///************Edited by Raisaat, from Jonathan: If torque Control is selected the following is active.
        {
                // update Iq reference
                updateIqRef(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
#ifdef DRV8305_SPI
        HAL_writeDrvData(halHandle,&gDrvSpi8305Vars);

        HAL_readDrvData(halHandle,&gDrvSpi8305Vars);
#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;

    if (TORQUE_CTRL == 0) ///************Edited by Raisaat, from Jonathan: If speed Control is selected the following is active.
    {
    // setup the SpinTAC Components
    ST_setupVelCtl(stHandle);
    ST_setupPosConv(stHandle);
    }


  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{

      static uint16_t stCnt = 0;

  CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;

  // 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;
  }

  count_over_frequency=count_in_one_cycle/estimated_frequency; //Edit by Jon Bright: This counts seconds in decimals for less than one second.  I put this in here to try and cut down on computing time in updateGlobalVariables.


  // compute the electrical angle
  ENC_calcElecAngle(encHandle, HAL_getQepPosnCounts(halHandle));

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

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


   if (TORQUE_CTRL == 0)//Edited by Raisaat, from Jonathan: If speed Control is selected the following is active.
   {
    // Run the SpinTAC Components
       if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
           ST_runPosConv(stHandle, encHandle, ctrlHandle);
           ST_runVelCtl(stHandle, ctrlHandle);
           stCnt = 1;
       }
   }


  if(USER_MOTOR_TYPE == MOTOR_Type_Induction) {
    // update the electrical angle for the SLIP module
    SLIP_setElectricalAngle(slipHandle, ENC_getElecAngle(encHandle));
    // compute the amount of slip
    SLIP_run(slipHandle);


    // run the controller
    CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,SLIP_getMagneticAngle(slipHandle));
  }
  else {
    // run the controller
    CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData,ENC_getElecAngle(encHandle));
  }

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


  // setup the controller
  CTRL_setup(ctrlHandle);

  // if we are forcing alignment, using the Rs Recalculation, align the eQEP angle with the rotor angle
  if((EST_getState(obj->estHandle) == EST_State_Rs) && (USER_MOTOR_TYPE == MOTOR_Type_Pm))
  {
      ENC_setZeroOffset(encHandle, (uint32_t)(HAL_getQepPosnMaximum(halHandle) - HAL_getQepPosnCounts(halHandle)));
  }

  return;
} // end of mainISR() function

//Original functin.  Edits by Jon Bright: I made a lot of edits in this 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 speed from eQEP
  gMotorVars.SpeedQEP_krpm = _IQmpy(STPOSCONV_getVelocityFiltered(stObj->posConvHandle), _IQ(ST_SPEED_KRPM_PER_PU));
  SpeedQEP_krpm_1 = gMotorVars.SpeedQEP_krpm; //Edit by Raisaat:  Captures the current speed so we can display it in the GUI

  // 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));

  //*********Edit by Jon Bright 04/14/19 Status indicator for GUI
  if (gMotorVars.EstState==EST_State_Rs)
  {
      motor_Est_State_Rs=1;
  }
  else if (gMotorVars.EstState!=EST_State_OnLine && gMotorVars.EstState!=EST_State_Rs)
  {
      motor_Est_State_Rs=0;
  }//First progress bar
  if (gMotorVars.EstState==EST_State_OnLine)
  {
      motor_Est_State_On_Line=1;
  }
  else
  {
      motor_Est_State_On_Line=0;
  }//Second progress bar
  //**********End of Jon Bright edit. for Status indicator

  // get the torque estimate
  gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);

  // get the torque estimate
  if (gMotorVars.EstState==EST_State_OnLine) //****Edit by Jon Bright.  I put in this "if" statement because I only want display torque if the motor is on, not during the setup time.
  {
      //*******Edit by Jonathan Bright. Timing to line up torque data with time
      Torque_1 = gMotorVars.Torque_Nm; //Edited by Raisaat, from Jonathan: Stores the torque value so we can use it in the GUI.  You can't use a variable for two or more widgets in hte GUI
      Torque_2 = gMotorVars.Torque_Nm;  //Jon Bright.  For the graph.
      //The timing will be included here because this shows up fastest on the GUI (fastest refresh rate)
      graph_intended_torque_Nm=_IQdiv(gMotorVars.IqRef_A,_IQ24(Torque_to_Iq_A_multiplier)); //Graphing what the torque should be based on Iq_A. For torque error.
      //******Timing
      TOTAL_TIME_updateGlobalVariables=time_total+count_over_frequency; //The total time that will be displayed
      check_period_updateGlobalVariables=clock_count_period+count_over_frequency; //The period time
      check_half_period_updateGlobalVariables=clock_count_half_period+count_over_frequency; //THe half period
      //End of timing
  }

  // 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 in V/Hz in floating point
  gMotorVars.Flux_VpHz = EST_getFlux_VpHz(obj->estHandle);

  // get the flux in Wb in fixed point
  gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);

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

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

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


  if (TORQUE_CTRL == 0) //************Edited by Raisaat, from Jonathan
  {
      // get the Iq reference from the speed controller
      gMotorVars.IqRef_A = _IQmpy(STVELCTL_getTorqueReference(stObj->velCtlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
      if (gMotorVars.IqRef_A>_IQ24(3.2)) //Edit by Jonathan Bright: Safeguard against the current going to high
      {
          gMotorVars.IqRef_A=_IQ24(3.2); //Capped at 3.2 A just for safeguards. The motor should be able to go up to 7 A but for our purposes I capped it at 3.2 A
          error_torque_too_high=1;
      }
      else
      {
          error_torque_too_high=0;
      }
  }//End of edit


  // gets the Velocity Controller status
  gMotorVars.SpinTAC.VelCtlStatus = STVELCTL_getStatus(stObj->velCtlHandle);

  // get the inertia setting
  gMotorVars.SpinTAC.InertiaEstimate_Aperkrpm = _IQmpy(STVELCTL_getInertia(stObj->velCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

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

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

  // get the Position Converter error
  gMotorVars.SpinTAC.PosConvErrorID = STPOSCONV_getErrorID(stObj->posConvHandle);
  //#endif
  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

//#ifndef TORQUE_CTRL
void ST_runPosConv(ST_Handle handle, ENC_Handle encHandle, CTRL_Handle ctrlHandle)
{
    ST_Obj *stObj = (ST_Obj *)handle;

    // get the electrical angle from the ENC module
    STPOSCONV_setElecAngle_erev(stObj->posConvHandle, ENC_getElecAngle(encHandle));

    if(USER_MOTOR_TYPE ==  MOTOR_Type_Induction) {
      // The CurrentVector feedback is only needed for ACIM
      // get the vector of the direct/quadrature current input vector values from CTRL
      STPOSCONV_setCurrentVector(stObj->posConvHandle, CTRL_getIdq_in_addr(ctrlHandle));
    }

    // run the SpinTAC Position Converter
    STPOSCONV_run(stObj->posConvHandle);

    if(USER_MOTOR_TYPE ==  MOTOR_Type_Induction) {
      // The Slip Velocity is only needed for ACIM
      // update the slip velocity in electrical angle per second, Q24
      SLIP_setSlipVelocity(slipHandle, STPOSCONV_getSlipVelocity(stObj->posConvHandle));
    }
}


void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{
    _iq speedFeedback, iqReference;
    ST_Obj *stObj = (ST_Obj *)handle;
    CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;

    // Get the mechanical speed in pu
    speedFeedback = STPOSCONV_getVelocityFiltered(stObj->posConvHandle);

    // 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);

    // Set the Iq reference that came out of SpinTAC Velocity Control
    CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
//#endif

//*****Function by Jon Bright
void Torque_Profile_Populate() //********Jon Bright 03_08_19A function to populate the arrays for the torque profiles.  I want to do math here so that it is not constantly having to do calculations while running the motor/in the update IqRef function
{
    Max_Iq=Max_Torque*Torque_to_Iq_A_multiplier; //Converts Torque to current value we can use for setting the current
    Min_Iq=Min_Torque*Torque_to_Iq_A_multiplier; //Converts Torque to current value we can use for setting the current
    IQMax_Iq=_IQ24(Max_Iq); //Converts Current to IQ24 value we can use for setting the current. This may be able to be deleted as the same thing is done in the if statement below
    IQMin_Iq=_IQ24(Min_Iq); //Converts Current to IQ24 value we can use for setting the current. This may be able to be deleted as the same thing is done in the if statement below
    Torque_Profile_Description=Torque_Profile_Description_GUI; //This is so the profile only changes when the button in the GUI is toggled.  This sets which profile is active
    period=period_GUI; //Changes the period that will be used for the profile
    if (Max_Torque < max_allowed_torque_Nm && Min_Torque < max_allowed_torque_Nm) //If the torque selected is not higher than the max torque allowed the following is active
    {
        IQMax_Iq=_IQmpy(_IQ24(Torque_to_Iq_A_multiplier),_IQ24(Max_Torque));//Added 04/10/19.  Calculates Max current for the max torque in IQ24 format
        IQMin_Iq=_IQmpy(_IQ24(Torque_to_Iq_A_multiplier),_IQ24(Min_Torque));//Added 04/10/19. Calculates Max current for the max torque in IQ24 format
        error_torque_too_high=0;
    }
    else
    {
        error_torque_too_high=1;
        IQMax_Iq=0; //Ensures that the current value will not be higher than what the system is set for.
        IQMin_Iq=0;//Ensures that the current value will not be higher than what the system is set for.
    }
    Iq_refA_array[0]=IQMin_Iq; //Puts the value in the array.  This is needed for the profiles
    Iq_refA_array[1]=IQMax_Iq; //Puts the value in the array.  This is needed for the profiles
    IQAvg_Iq=_IQ24((Max_Iq+Min_Iq)/2); //IQ math does not work well with floats.  I get wrong solutions in the profiles if I do IQ math with floats.  Therefore I do float math and convert to IQ
    IQSine_Amplitude_Iq=IQMax_Iq-IQAvg_Iq; //Computes the amplitude of the sine wave
    IQ_period=_IQ24(period)+_IQ24(1); //There has to be a one added here because otherwise the halfperiod is 9 seconds and the full period is 19 seconds. If the default timing is 20. I don't know why
    half_period=period/2; //Half period needed for calculations
    IQ_slope_ramp_Iq=_IQ24((Max_Iq-Min_Iq)/half_period);//Positive slope needed for the Ramp profile
    IQ_neg_slope_ramp_Iq=_IQ24((Min_Iq-Max_Iq)/half_period); //Negative slope needed for the Ramp profile
    IQ_2pi_div_period=_IQ24(2*_PI/period);//I need to multiply this number by sinusoid time because y=a*sin(bx)+c.  Where period=2pi/b. Therefore b=2pi/period
    //This reinitializes all the timing if we switch to a new profile.  Edited by Jon Bright 04/02/19.  You don't want to reinitialze count/frequency because then total time will be reset to the whole number that is below its current value as the decimal becomes zero
    clock_count_period=0;
    clock_count_half_period=0;
    if (!gMotorVars.EstState==EST_State_OnLine)//If the motor is turned off that the total time will reset
    {
        time_period=0;
        count_in_one_cycle=0;
    }

    if (Torque_Profile_Description==3) //If Custom Profile is selected then the time and torque are set her
    {
        int a;
        for (a=0; a<100; a++)
        {
            custom_IQ_Iq_refA_array[a]=_IQ24(custom_torque_array[a]*Torque_to_Iq_A_multiplier);
            if (custom_torque_array[a] > max_allowed_torque_Nm)
            {
                custom_IQ_Iq_refA_array[a]=0;
                error_torque_too_high=1;
            }
        }
        Iq_RefA_ptr=&custom_IQ_Iq_refA_array[0];
    }
    else
    {
        Iq_RefA_ptr=&Iq_refA_array[0];
    }
    status_profile_initiated=1; //Makes green LED light up in hte GUI to show that profile has been initialized
    Already_initialize_torque=1; //Prevents this function from constantly running when the motor is off
    free_torque_ctrl_on=0; //If a profile is initiated then free torque control is off.  We don't want both of these on at the same time
    initialize_torque=0; //I want to cut off this initialization after it happens once
}//I created this because I had problem populating the array when I made the size a variable and tried to put variables for the array values

void Torque_Profile_Type() //Jon Bright 03_08_19. A function to implement torque profiles
{
    if (Torque_Profile_Description==0||Torque_Profile_Description==3) //Step Profile or Custom Profile
    {
        Y_long=*Iq_RefA_ptr;
    }
    else if (Torque_Profile_Description==1) //Ramp Profile
    {
        if (Iq_RefA_ptr==&Iq_refA_array[0])
        {
            //Y_float=slope*time+Min_Torque. Slope=(min-max)/time
            Y_long=_IQmpy(IQ_slope_ramp_Iq,_IQ24(check_period_updateGlobalVariables))+IQMin_Iq;//Ramp up
        }
        else if (Iq_RefA_ptr==&Iq_refA_array[1])
        {
            //Y_float=-slope*time+Max_Torque. Slope=(min-max)/time
            Y_long=_IQmpy(IQ_neg_slope_ramp_Iq,_IQ24(check_half_period_updateGlobalVariables))+IQMax_Iq; //Ramp down
        }
    }
    else if (Torque_Profile_Description==2) //Sinusoidal Profile by Jonathan Bright. Last updated 04/02/19
    {
        Y_long=_IQmpy(IQSine_Amplitude_Iq,_IQsin(_IQmpy(IQ_2pi_div_period,_IQ24(check_period_updateGlobalVariables))))+IQAvg_Iq;
    }
    else //The default setting will be 0
    {
        Y_long=0;
    }
}
//****************end of Jon Bright edit

...

This file has been truncated, please download it to see its full contents.