CCS/LAUNCHXL-F28069M: MTTP Dynamometer: Some few queries

/* --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   modules/enc/src/32b/enc.c
//! \brief  Portable C fixed point code.  These functions define the
//! \brief  encoder routines.
//!
//! (C) Copyright 2011, Texas Instruments, Inc.


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

#include "sw/modules/enc/src/32b/enc.h"

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


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

uint32_t enc_mechAngle,enc_mechAngle_1;

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

ENC_Handle ENC_init(void *pMemory,const size_t numBytes)
{
  ENC_Handle encHandle;
  
  if(numBytes < sizeof(ENC_Obj))
    return((ENC_Handle)NULL);

  // assign the handle
  encHandle = (ENC_Handle)pMemory;
  
  return(encHandle);
} // end of ENC_init() function

void ENC_setup(ENC_Handle encHandle, const int16_t sample_period, const uint16_t num_pole_pairs, const uint16_t num_enc_slots, const uint32_t enc_zero_offset, const float_t full_scale_freq, const float_t speed_update_freq, const float_t speed_cutoff)
{
  ENC_Obj *enc;
  float_t temp;
  float_t speed_cutoff_radians;
  int16_t i;
 
  // create an object pointer for manipulation
  enc = (ENC_Obj *)encHandle;
  
  // setup the encoder sample rate
  enc->sample_count = 0;
  enc->sample_period = sample_period;
  
  // copy the parameters into the data structure
  enc->num_enc_slots = num_enc_slots;
  enc->num_pole_pairs = num_pole_pairs;
  enc->enc_zero_offset = enc_zero_offset;
  enc->full_scale_freq = full_scale_freq;
  enc->delta_enc = 0;
  enc->prev_enc = 0;
  
  // initialize the electrical angle
  enc->enc_elec_angle = 0;
    
  // compute the gain which translates the mech into the elec angle
  enc->mech_angle_gain = (_iq)((((uint32_t)1)<<24)/(4*num_enc_slots));

  // compute the speed gain
  temp = ((float_t)num_pole_pairs*speed_update_freq*ENC_SPEED_SCALING_FACTOR) / (4.0*(float_t)num_enc_slots*full_scale_freq*(float_t)sample_period);
  enc->speed_gain = (int32_t) temp;
  
  // compute the rpm gain
  temp = (float_t)((full_scale_freq*60.0)/num_pole_pairs);
  enc->rpm_gain = (int32_t) temp;
  
  // compute the speed coefficients and initialize the low-pass filtered output
  enc->speed_cutoff = speed_cutoff;
  speed_cutoff_radians = ENC_2PI*speed_cutoff;
  temp = speed_cutoff_radians/(speed_update_freq+speed_cutoff_radians);
  enc->speed_lpf_cx = (int32_t) (ENC_SPEED_COEFF_SCALING*temp);
  enc->speed_lpf_cy = ENC_SPEED_COEFF_SCALING - enc->speed_lpf_cx;
  enc->speed_lpf_out = 0;
  
  // setup the encoder log
  enc->log_state        = ENC_LOG_STATE_IDLE;
  enc->run_flag         = 0;
  enc->trigger_idx      = 0;
  enc->trigger_delta    = 0;  
  enc->log_idx          = 0;
  enc->post_trigger_len = ENC_LOG_LEN>>1;
  enc->post_trigger_cnt = enc->post_trigger_len;
  for (i=0; i<ENC_LOG_LEN; i++)
  {
  	enc->log[i] = 0;
  }
  
  return;
} // end of ENC setup function


void ENC_calcElecAngle(ENC_Handle encHandle, uint32_t posnCounts)
{
	ENC_Obj *enc;
	uint32_t temp;
	//_iq24 multiplier;//edit by Jon Bright
  // create an object pointer for manipulation
  enc = (ENC_Obj *) encHandle;

  // compute the mechanical angle
  temp = posnCounts*enc->mech_angle_gain;
  enc_mechAngle=temp;//*_IQ16(360);//Edit by Jon Bright
  enc_mechAngle_1=temp;//*_IQ16(360);//Edit by Jon Bright
  //End of edit by Jon Bright
  // add in calibrated offset
  temp += enc->enc_zero_offset;
  // convert to electrical angle
  temp = temp * enc->num_pole_pairs;
  // wrap around 1.0 (Q24)
  temp &= ((uint32_t) 0x00ffffff);
  // store encoder electrical angle
  enc->enc_elec_angle = (_iq)temp;
  // update the slip angle
  enc->enc_slip_angle = enc->enc_slip_angle + enc->incremental_slip;
  // wrap around 1.0 (Q24)
  enc->enc_slip_angle &= ((uint32_t) 0x00ffffff);
  // add in compensation for slip
  temp = temp + enc->enc_slip_angle;
  // wrap around 1.0 (Q24)
  temp &= ((uint32_t) 0x00ffffff);
  // store encoder magnetic angle
  enc->enc_magnetic_angle = (_iq)temp;
  
  return;
} // end of ENC_calc_elec_angle() function



void ENC_run(ENC_Handle encHandle, uint32_t posnCounts, uint16_t indextFlag, uint16_t dirFlag, int16_t log_flag)
{
	uint16_t dir;
	uint16_t index_event_before;
	uint16_t index_event_after;
	ENC_Obj *enc;
	int32_t enc_val;
	int32_t delta_enc;
	int32_t temp;
	int16_t sample_count;
 
  // create an object pointer for manipulation
  enc = (ENC_Obj *) encHandle;
  
  // update the encoder counter
  sample_count = enc->sample_count;
  sample_count++;
  
  // if it reaches the sample period, read and process encoder data
  if (sample_count == enc->sample_period)
  {
    enc->sample_count = 0;
    
    // check for index event before the encoder reading
    index_event_before = indextFlag;
  	
    // read the encoder
    enc_val = posnCounts;
	
    // compute the mechanical angle
    // compute the mechanical angle
    temp = (enc_val)*enc->mech_angle_gain;
    // add in calibrated offset
    temp += enc->enc_zero_offset;
    // convert to electrical angle
    temp = temp * enc->num_pole_pairs;
    // wrap around 1.0
    temp &= ((uint32_t) 0x00ffffff);

    enc->enc_elec_angle = (_iq)temp;
  
    /*********************/
    /* compute the SPEED */
    /*********************/
  
    // read QEP direction from quadrature direction latch flag
    dir = dirFlag;
  
    // check for index event after the encoder reading
    index_event_after = indextFlag;
  
    // handle a rollover event
    if (index_event_after)
    {
  	   if (dir)
  	   {
  	     delta_enc = enc_val + (4*enc->num_enc_slots-1) - enc->prev_enc;
  	   }
  	   else
  	   {
         delta_enc = enc->prev_enc + (4*enc->num_enc_slots-1) - enc_val;
  	   }
    }
    else
    {
       delta_enc = enc_val - enc->prev_enc;
    }
    
    // save off the delta encoder value in the data structure only if the index event before and after are the same
    if ((index_event_after == index_event_before) && (abs(delta_enc) < enc->num_enc_slots))
    	enc->delta_enc = delta_enc;
    
    // log the startup data
    switch(enc->log_state)
    {
    	// wait for run flag to be set
    	case ENC_LOG_STATE_IDLE:
    	{
	    	if (enc->run_flag)
	    	{
	    		enc->run_flag  = 0;
	    		enc->log_idx   = 0;
	    		enc->log_state = ENC_LOG_STATE_FREERUN;
	    	}
    	}
    	break;
    	
    	// collect data round robin until there's a trigger event
    	case ENC_LOG_STATE_FREERUN:
    	{
			enc->log[enc->log_idx] = (int16_t)enc_val;
			enc->log_idx = enc->log_idx + 1;
			if (enc->log_idx > ENC_LOG_LEN)
				enc->log_idx = 0;
			if (abs(delta_enc)>ENC_LOG_DELTA_TRIGGER_THRES)
			{
				enc->trigger_delta = delta_enc;
				enc->trigger_idx = enc->log_idx;
				enc->post_trigger_cnt = enc->post_trigger_len;
				enc->log_state = ENC_LOG_STATE_ACQUIRE;
			}
    	}    		
    	break;
    	
    	// when trigger occurs collect 1/2 a buffer of post-trigger information
    	case ENC_LOG_STATE_ACQUIRE:
    	{
			enc->log[enc->log_idx] = (int16_t)enc_val;
			enc->log_idx = enc->log_idx + 1;
			if (enc->log_idx > ENC_LOG_LEN)
				enc->log_idx = 0;
			enc->post_trigger_cnt = enc->post_trigger_cnt - 1;
			if (enc->post_trigger_cnt == 0)
				enc->log_state = ENC_LOG_STATE_IDLE;
    	}      	
    	break;
    }

    // shift it to Q24
    temp = enc->delta_enc*enc->speed_gain;
  
    // LPF the encoder
    temp = (enc->speed_lpf_cy)*enc->speed_lpf_out + (enc->speed_lpf_cx)*temp;
    temp >>= ENC_SPEED_COEFF_Q;
    enc->speed_lpf_out = temp;

    // copy the current into the previous value  
    enc->prev_enc = enc_val;
  }
  else
  {
  	enc->sample_count = sample_count;
  }
  
} // end of ENC_run() function


int16_t ENC_getSpeedRPM(ENC_Handle encHandle)
{
	ENC_Obj *enc;
	_iq temp;
 
  // create an object pointer for manipulation
  enc = (ENC_Obj *) encHandle;
  
  temp = (enc->speed_lpf_out >> ENC_RPM_Q1);
  temp = (temp * enc->rpm_gain) >> ENC_RPM_Q2;
  return (int16_t) temp;
} // end of ENC_getSpeedRPM() 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_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


void updateIqRef(CTRL_Handle handle) // new version, edited by Jon Bright
{
    //*********03/04/19 Jon Bright:  Code implemented to change the IqRef_A variable from CCS Code to create torque profiles
    if (gMotorVars.EstState==EST_State_OnLine)
    {
        if (free_torque_ctrl_on==1) //If statement for free/constant control 04/10/19.
        {
            status_profile_initiated=0;//This is the LED in the GUI.
            Y_long=_IQmpy(_IQ24(free_torque_ctrl_Nm),_IQ24(Torque_to_Iq_A_multiplier));//Computes torque to Iq_RefA for the torque input into the
            if (initialize_torque==1) //Changes from constant/free torque control, to a torque control profile
            {
                status_profile_initiated=0;
                Torque_Profile_Populate();
            }
            if (Y_long > IQmax_allowed_torque_Iq_A) //If the input torque is higher than the allowed torque than the current is set to zero instead
            {
                free_torque_ctrl_Nm=0;
                Y_long=0;
                error_torque_too_high=1;
            }
            else
            {
                error_torque_too_high=0;
            }
        }//Free torque control completed on 04/11/19.
        else if (initialize_torque==1) //Jon Bright 04/11/19.  Put in order to change the profile while the profile is active
        {
            status_profile_initiated=0;
            Torque_Profile_Populate();
        }
        else if (free_torque_ctrl_on==0 && status_profile_initiated==0)//Needed in case someone cuts off free torque control and no profile is initialized
        {
            Y_long=0; //Turn off torque if free control is off and no torque profile is initiated.
        }
        else
        {
            if (Torque_Profile_Description<3) //If any profile except for Custom profile is selected than the following is active
            {
                if (half_period >= check_period_updateGlobalVariables)
                {
                    Torque_Profile_Type(); //May be able to delete this because the value is already in Y_Long.  May have to call once. Not in a loop
                }
                else
                {
                    if (Iq_RefA_ptr==&Iq_refA_array[0]) //If we are in the first half of a period, then after half a period we go to the next value
                    {
                        Iq_RefA_ptr++;
                        clock_count_half_period=0;
                    }
                    if (period >= check_period_updateGlobalVariables)
                    {
                        Torque_Profile_Type(); //May be able to delete this because the value is already in Y_Long. May have to call once. Not in a loop
                    }                           //Or I can keep this and make it my Step and Ramp timing
                    else
                    {
                        Iq_RefA_ptr=&Iq_refA_array[0];
                        time_period=0;
                        clock_count_period=0;
                    }
                }
            }
            //End of Step, Ramp, Sinusoid function timing

            else //If Custom Profile is selected
            {
                if (custom_time_array_ptr==&custom_time_array[99]) //Special Case: If we are at the end of the profile we are at spot [99].  We want the Torque to match here. Then we will reset to the beginning of the profile.
                {
                    Torque_Profile_Type();
                    Iq_RefA_ptr=&custom_IQ_Iq_refA_array[0]; //Resets Custom Torque array to the zeroth time
                    custom_time_array_ptr=&custom_time_array[0]; //Resets time to zero
                    clock_count_period=0;
                    count_in_one_cycle=0;
                    check_period_updateGlobalVariables=0;
                }
                else if (*(custom_time_array_ptr+1)<=0)//This is in case they don't enter 100 time slots.  In this case we would reset when the next time value is a zero.
                {
                    Iq_RefA_ptr=&custom_IQ_Iq_refA_array[0]; //Resets Custom Torque array to the zeroth time
                    custom_time_array_ptr=&custom_time_array[0]; //Resets time to zero
                    clock_count_period=0;
                    count_in_one_cycle=0;
                    check_period_updateGlobalVariables=0;
                    Torque_Profile_Type();
                }
                else
                {
                    if (*(custom_time_array_ptr+1) > check_period_updateGlobalVariables)//If the current time is less than the next time entered then our torque will stay the same
                    { //TOTAL_TIME_updateGlobalVariables=time_total+count/frequency.  We will enter total time for the tracking of custom profile
                        Torque_Profile_Type();
                    }
                    else //If the current time is equal or greater than the next time, then we move to the next torque value
                    {
                        custom_time_array_ptr++;
                        Iq_RefA_ptr++;
                        Torque_Profile_Type();
                    }
                }
            }//Temporary end of Custom array timing.  There is one more part at the bottom to reset once the period is over.
        }
        //Timing
        if (count_in_one_cycle+1>=estimated_frequency)//When our clock counter equals frequency
        {
            clock_count_half_period++;
            clock_count_period++;
            time_period++;
            time_total++;
            count_in_one_cycle=0;
        }
        else //We don't want to increase count over frequency if count is supposed to equal zero.  I discovered this while testing.
        {
            count_in_one_cycle++;//We want to increase cycle count for every time we update IQrefA.
        }

        gMotorVars.IqRef_A=Y_long;

    }//End of Jon Bright edit
    _iq iq_ref = _IQmpy(gMotorVars.IqRef_A,_IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)); //This is from the original code

  // set the speed reference so that the forced angle rotates in the correct direction for startup
  if(_IQabs(gMotorVars.Speed_krpm) < _IQ(0.01))
    {
      if(iq_ref < _IQ(0.0))
        {
          CTRL_setSpd_ref_krpm(handle,_IQ(-0.01));
        }
      else if(iq_ref > _IQ(0.0))
        {
          CTRL_setSpd_ref_krpm(handle,_IQ(0.01));
        }
    }

  // Set the Iq reference that use to come out of the PI speed control
  CTRL_setIq_ref_pu(handle, iq_ref);

  return;
} // end of updateIqRef() function