CNC Router: Synchronization between X- and Y-axis

It sounds like you have everything setup correctly.  Couple of questions.

1. Do your two 8301 boards share a common ground?

2. Can you try pressing start on the Y-axis board than pressing start on the X-axis board?  In the code, the X-axis is setup as the master and the Y-axis is setup as a slave.  So this means that the Y-axis will always wait on the X-axis.  I'm thinking that maybe your two axes are getting out of synchronization.

Sorry for the late response...

1. There're only TWO grounds - GND and IGND, right?? I checked the GND and two boards are shorted well.

2. Actually, if I press x- START only and leave y-axis STOP, x-axis goes thru. ALL non-zero STATEs over, w/o. any stop. Same thing happens on y-axis too. In short, they're working independently at all.

In the settings of CONDITION and TRANSITION, x-axis cannot jump to next STATE until receiving Y-done signal, isn't it?? 

I'm now wondering if my signal connection is correct...

From the codes, my understanding is GPIO 40 and GPIO 41 should be connected respectively bet. two boards. They are located at J5: pin14 and 16. Are these accurate understanding??

J10 is the hall sensor signal header and I leave them open all the time. (Why they're using GPIO40 & 41 though??)

I did tried START Y first and followed by START X, and they(two axes) just go thru. all non-zero STATES set in "ST_addSTATE" simultaneously and independently, w/o. any cooperation! That's why I wondered if I wired GPIOs correct.

It seems to me that two GPIO signals are kept at HIGH all the time, so that no axis needs to wait another axis to be done to jump to next STATE. In other words, the CONDITION setting is invalid.

J10 also connects to GPIO40 & GPIO41 due to the limitations of the DRV8301 board.  There is limited typical GPIO available.  

It is possible that the GPIO isn't changing state when the microcontroller is telling it to.

To me this seems like an electrical problem with the setup.  Maybe there is something in the system that is holding the signal HIGH when it should be going LOW.

Just got time to come back on this project...

I thought the same thing as you do but I really cannot see any mistake in electrical setup. Also, I tried another thing here:

I disconnected the gpio 40 & 41. And press START for both boards. The X-axis just go through ALL planned STATEs w/o. any stop, while the Y-axis cannot even move.

To me, Y-axis behavior makes sense since it's not receiving any to-go signal from X-axis. But, X-axis behavior indicates it does not even need any signal from Y to go (to transition bet. different STATE). This makes me suspect that the codes might have flaw... I'm pasting the codes(X.c and Y.c) I have and hoping you don't mind to check for me.

I really appreciate your patience and time!

X-AXIS:


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

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

#include "shape_square.h"
#include "shape_triangle.h"
#include "shape_circle_small.h"
#include "shape_circle_large.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

// State Machine Options for this Project
typedef enum
{
    SQUARE=0,    // Square Pattern
    TRIANGLE,    // Triangle Pattern
    SM_CIRCLE,   // Small Circle Pattern
    LG_CIRCLE,    // Large Circle Pattern
    MAX_SHAPE
} PLAN_SELECT_e;

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

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

DRV_Handle drvHandle;

USER_Params gUserParams;

DRV_PwmData_t gPwmData;

DRV_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

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

ENC_Handle encHandle;
ENC_Obj enc;

ST_Obj st_obj;
ST_Handle stHandle;

_iq square_steps[SQUARE_STATE_LENGTH] = SQUARE_X_STEPS;
_iq square_speed = SQUARE_X_SPEED;

_iq triangle_isteps[TRIANGLE_STATE_LENGTH] = TRIANGLE_X_ISTEPS;
_iq triangle_fsteps[TRIANGLE_STATE_LENGTH] = TRIANGLE_X_FSTEPS;
_iq triangle_speeds[TRIANGLE_STATE_LENGTH] = TRIANGLE_X_SPEEDS;

_iq circle_sm_steps[CIRCLE_SM_STATE_LENGTH] = CIRCLE_SM_X_STEPS;
_iq circle_sm_speeds[CIRCLE_SM_STATE_LENGTH] = CIRCLE_SM_X_SPEEDS;

_iq circle_lg_isteps[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_X_ISTEPS;
_iq circle_lg_fsteps[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_X_FSTEPS;
_iq circle_lg_speeds[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_X_SPEEDS;

// only
#define SHAPE_PLAN_ACTIONS    (CIRCLE_LG_STATE_LENGTH)
#define SHAPE_PLAN_CONDITIONS 1
#define SHAPE_PLAN_VARS       2

// Used to select a state machine
PLAN_SELECT_e gSelectedPlan = SQUARE;

// Used to hold the current configured state machine
PLAN_SELECT_e gConfiguredPlan = SQUARE;

// interfacing variable
uint32_t X_Go = 0;
uint32_t X_GoOneShot = 0;
uint16_t OneShotLatch = 0;
uint32_t Y_MoveDone = 0;
uint32_t Y_MoveDoneOld = 0;
uint32_t Y_MoveDoneLatch = 0;
uint16_t debounceCount_Y_MoveDone = 0;

uint32_t shapeSelectMode = 1;
uint32_t shapeSelectModeSwitch = 1;
uint16_t debounceCount_shapeSelectMode = 0;
uint32_t shapeSelectAdv = 1;
uint32_t shapeSelectAdvLatch = 0;
uint16_t debounceCount_shapeSelectAdv = 0;

// Used to handle controlling SpinTAC Plan
ST_PlanButton_e gPosPlanRunFlag = ST_PLAN_STOP;

// Calculates the amount of memory required for the SpinTAC Position Plan configuration
// This is based on the above enumerations
#define ST_POSPLAN_CFG_ARRAY_DWORDS (   (ST_POS_PLAN_TRAN_DWORDS  * CIRCLE_LG_STATE_LENGTH)   + \
                                        (ST_POS_PLAN_STATE_DWORDS * CIRCLE_LG_STATE_LENGTH)   + \
                                        (ST_POS_PLAN_ACT_DWORDS   * SHAPE_PLAN_ACTIONS)    + \
                                        (ST_POS_PLAN_COND_DWORDS  * SHAPE_PLAN_CONDITIONS) + \
                                        (ST_POS_PLAN_VAR_DWORDS   * SHAPE_PLAN_VARS))

// Used to store the configuration of SpinTAC Position Plan
uint32_t stPosPlanCfgArray[ST_POSPLAN_CFG_ARRAY_DWORDS];

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;

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


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

// **************************************************************************
// the functions
// plans
void ST_setupSquarePlan(ST_Handle stHandle);
void ST_setupTrianglePlan(ST_Handle stHandle);
void ST_setupCircleSmPlan(ST_Handle stHandle);
void ST_setupCircleLgPlan(ST_Handle stHandle);

void main(void)
{

  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

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

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


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


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


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


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


  // setup faults
  DRV_setupFaults(drvHandle);


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


  // enable the ADC interrupts
  DRV_enableAdcInts(drvHandle);


  // enable global interrupts
  DRV_enableGlobalInts(drvHandle);


  // enable debug interrupts
  DRV_enableDebugInt(drvHandle);


  // disable the PWM
  DRV_disablePwm(drvHandle);


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


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

  // setup the ENC module
  DRV_Obj *drv_obj = (DRV_Obj *)drvHandle;
  ENC_setup(encHandle, drv_obj->qepHandle[0], 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 SpinTAC Components
  stHandle = ST_init(&st_obj, sizeof(st_obj));


  // setup the SpinTAC Components
  ST_setupPosConv(stHandle);
  ST_setupPosCtl(stHandle);
  ST_setupPosMove(stHandle);
  ST_setupPosPlan(stHandle);

  // configure the actual plan
  switch(gSelectedPlan) {
      case SQUARE:
          ST_setupSquarePlan(stHandle);
      break;
      case TRIANGLE:
          ST_setupTrianglePlan(stHandle);
      break;
      case SM_CIRCLE:
          ST_setupCircleSmPlan(stHandle);
      break;
      case LG_CIRCLE:
          ST_setupCircleLgPlan(stHandle);
      break;
  }
  // report selected state machine
    gConfiguredPlan = gSelectedPlan;


#ifdef FLASH
  gMotorVars.Flag_enableSys = 1;
  gMotorVars.Flag_Run_Identify = 1;
  gMotorVars.SpinTAC.PosMoveCurveType = ST_MOVE_CUR_TRAP;
#endif
#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  DRV_enable8301(drvHandle);
  // initialize the DRV8301 interface
  DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars);
#endif

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


    // select if we should identify the motor
    CTRL_setFlag_enableUserMotorParams(ctrlHandle, gMotorVars.Flag_enableUserParams);

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

      // increment counters
      gCounter_updateGlobals++;

      // change the current state
      if(shapeSelectMode == 0) {
          // indicate we are in this mode
          DRV_turnLedOn(drvHandle,(GPIO_Number_e)DRV_Gpio_LED3);
          // display current selected plan
          switch(gSelectedPlan) {
            case SQUARE:
                GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
                GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
            break;
            case TRIANGLE:
                GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
                GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
            break;
            case SM_CIRCLE:
                GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
                GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
            break;
            case LG_CIRCLE:
                GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
                GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
            break;
          }
          // if the switch is pressed, advance plan
          if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
              shapeSelectAdvLatch = 1;
              gSelectedPlan++;
              if(gSelectedPlan == MAX_SHAPE) {
                  gSelectedPlan = SQUARE;
              }
          }
          else if(shapeSelectAdv == 1) {
              shapeSelectAdvLatch = 0;
          }
      }
      else {
          // indicate we are not in shape select mode
          DRV_turnLedOff(drvHandle,(GPIO_Number_e)DRV_Gpio_LED3);

          if(STPOSPLAN_getStatus(stObj->posPlanHandle) == ST_PLAN_BUSY) {
              GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_22);
              // if we press, the START button, stop PLAN
              if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
                  shapeSelectAdvLatch = 1;
                  gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_STOP;
              }
              else if(shapeSelectAdv == 1) {
                  shapeSelectAdvLatch = 0;
              }
          }
          else {
              GPIO_setLow(drvObj->gpioHandle, GPIO_Number_22);
              if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
                  shapeSelectAdvLatch = 1;
                  gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_START;
              }
              else if(shapeSelectAdv == 1) {
                  shapeSelectAdvLatch = 0;
              }
          }
      }

      // configure a different Plan if selected
      if((gSelectedPlan != gConfiguredPlan) && (STPOSPLAN_getStatus(stObj->posPlanHandle) == ST_PLAN_IDLE))
      {
        // setup the selected Plan
        switch(gSelectedPlan) {
          case SQUARE:
            ST_setupSquarePlan(stHandle);
          break;
          case TRIANGLE:
            ST_setupTrianglePlan(stHandle);
          break;
          case SM_CIRCLE:
            ST_setupCircleSmPlan(stHandle);
          break;
          case LG_CIRCLE:
            ST_setupCircleLgPlan(stHandle);
          break;
        }

        // report selected state machine
        gConfiguredPlan = gSelectedPlan;
      }

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

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

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

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

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

              if(ctrlState == CTRL_State_OffLine) {
                  // enable the PWM
                  DRV_enablePwm(drvHandle);
              }
              else if(ctrlState == CTRL_State_OnLine) {
                  // update the ADC bias values
                  DRV_updateAdcBias(drvHandle);

                  // enable the PWM
                  DRV_enablePwm(drvHandle);
                  // initialize the watch window Bw value with the default value
                  gMotorVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidthScale(stObj->posCtlHandle);
              }
              else if(ctrlState == CTRL_State_Idle) {
                  // disable the PWM
                  DRV_disablePwm(drvHandle);
                  gMotorVars.Flag_Run_Identify = false;
              }
          }

          if(EST_getState(obj->estHandle) == EST_State_OnLine) {
              // remove ST_POS_CTL from reset
              STPOSCTL_setReset(stObj->posCtlHandle, false);
              STPOSCTL_setEnable(stObj->posCtlHandle, true);
          }
          else
          {
              // place SpinTAC Position Control into reset
              STPOSCTL_setReset(stObj->posCtlHandle, true);
              // If motor is not running, feed the position feedback into SpinTAC Position Move
              STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));
          }
      }


      if(EST_isMotorIdentified(obj->estHandle)) {
          // set the speed reference acceleration
          EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
          gMotorVars.Flag_MotorIdentified = true;
          EST_setFlag_enableRsRecalc(obj->estHandle, gMotorVars.Flag_enableRsRecalc);
          CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
          if(Flag_Latch_softwareUpdate)
          {
              Flag_Latch_softwareUpdate = false;

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

              calcPIgains(ctrlHandle);
          }
      }
      else {
          Flag_Latch_softwareUpdate = true;

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


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

          updateGlobalVariables_motor(ctrlHandle, stHandle);
      }

  } // end of while(gFlag_enableSys) loop


  // disable the PWM
  DRV_disablePwm(drvHandle);

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

    // setup the SpinTAC Components
    ST_setupPosConv(stHandle);
    ST_setupPosCtl(stHandle);
    ST_setupPosMove(stHandle);
    ST_setupPosPlan(stHandle);

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{
  static uint16_t stCnt = 0;
  CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
  DRV_Obj *drvObj = (DRV_Obj *)drvHandle;
  

  // compute the electrical angle
  ENC_calcElecAngle(encHandle);

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


  // do a debounce on the Y_MoveDone signal
  if(Y_MoveDone != GPIO_read(drvObj->gpioHandle, GPIO_Number_41)) {
    debounceCount_Y_MoveDone++;
    if(debounceCount_Y_MoveDone > 5) {
        Y_MoveDone = GPIO_read(drvObj->gpioHandle, GPIO_Number_41);
    }
  }
  else {
    debounceCount_Y_MoveDone = 0;
  }

  // do a debounce on the select switch
  if(shapeSelectModeSwitch != GPIO_read(drvObj->gpioHandle, GPIO_Number_7)) {
    debounceCount_shapeSelectMode++;
    if(debounceCount_shapeSelectMode > 500) {
      shapeSelectModeSwitch = GPIO_read(drvObj->gpioHandle, GPIO_Number_7);
      if(shapeSelectModeSwitch == 0) {
          shapeSelectMode = !shapeSelectMode;
      }
    }
  }
  else {
    debounceCount_shapeSelectMode = 0;
  }

  // do a debounce on the advance switch
  if(shapeSelectAdv != GPIO_read(drvObj->gpioHandle, GPIO_Number_9)) {
    debounceCount_shapeSelectAdv++;
    if(debounceCount_shapeSelectAdv > 500) {
        shapeSelectAdv = GPIO_read(drvObj->gpioHandle, GPIO_Number_9);
    }
  }
  else {
      debounceCount_shapeSelectAdv = 0;
  }

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


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


  // Run the SpinTAC Speed controller
  if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
      ST_runPosConv(stHandle);
      ST_runPosPlanTick(stHandle);
      ST_runPosPlan(stHandle);
      ST_runPosMove(stHandle);
      ST_runPosCtl(stHandle);
      stCnt = 1;
  }


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


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


  // setup the controller
  CTRL_setup(ctrlHandle);

  // 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)
  {
      ENC_setZeroOffset(encHandle, (uint32_t)(ENC_getPositionMax(encHandle) - ENC_getRawEncoderCounts(encHandle)));
  }

  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle stHandle)
{
  uint16_t stPosPlanCfgErrIdx, stPosPlanCfgErrCode;
  uint32_t ProTime_tick, ProTime_mtick;
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  ST_Obj *stObj = (ST_Obj *)stHandle;
  // get the speed estimate
  gMotorVars.Speed_krpm = _IQmpy(STPOSCONV_getVelocityFiltered(stObj->posConvHandle), _IQ(ST_SPEED_KRPM_PER_PU));

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

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

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

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

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

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

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

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

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

  // gets the Position Controller status
  gMotorVars.SpinTAC.PosCtlStatus = STPOSCTL_getStatus(stObj->posCtlHandle);
    
  // get the inertia setting
  gMotorVars.SpinTAC.Inertia_Aperkrpm = _IQmpy(STPOSCTL_getInertia(stObj->posCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

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

  // set the SpinTAC (ST) bandwidth scale
  STPOSCTL_setBandwidthScale(stObj->posCtlHandle, gMotorVars.SpinTAC.PosCtlBwScale);

  // get the SpinTAC (ST) bandwidth
  gMotorVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidth_radps(stObj->posCtlHandle);

  // get the Position Controller error
  gMotorVars.SpinTAC.PosCtlErrorID = STPOSCTL_getErrorID(stObj->posCtlHandle);

  // get the Position Move status
  gMotorVars.SpinTAC.PosMoveStatus = STPOSMOVE_getStatus(stObj->posMoveHandle);

  // get the Position Move profile time
  STPOSMOVE_getProfileTime_tick(stObj->posMoveHandle, &ProTime_tick, &ProTime_mtick);
  gMotorVars.SpinTAC.PosMoveTime_ticks = ProTime_tick;
  gMotorVars.SpinTAC.PosMoveTime_mticks = ProTime_mtick;

  // get the Position Move done signal
  gMotorVars.SpinTAC.PosMoveDone = STPOSMOVE_getDone(stObj->posMoveHandle);

  // get the Position Move error
  gMotorVars.SpinTAC.PosMoveErrorID = STPOSMOVE_getErrorID(stObj->posMoveHandle);
  
  // get the Position Plan status
  gMotorVars.SpinTAC.PosPlanStatus = STPOSPLAN_getStatus(stObj->posPlanHandle);

  // get the Position Plan Done signal
  gMotorVars.SpinTAC.PosPlanDone = STPOSPLAN_getDone(stObj->posPlanHandle);

  // get the Position Plan error
  gMotorVars.SpinTAC.PosPlanErrorID = STPOSPLAN_getCfgError(stObj->posPlanHandle, &stPosPlanCfgErrIdx, &stPosPlanCfgErrCode);
  gMotorVars.SpinTAC.PosPlanCfgErrorIdx = stPosPlanCfgErrIdx;
  gMotorVars.SpinTAC.PosPlanCfgErrorCode = stPosPlanCfgErrCode;

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

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

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

  return;
} // end of updateGlobalVariables_motor() function

#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{

  CTRL_Obj *obj = (CTRL_Obj *)handle;

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


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

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

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

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

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

  return;
} // end of calcPIgains() function

void ST_setupPosPlan(ST_Handle sthandle) {

  ST_Obj *stObj = (ST_Obj *)sthandle;

  // Pass the configuration array pointer into SpinTAC Position Plan
  STPOSPLAN_setCfgArray(stObj->posPlanHandle, &stPosPlanCfgArray[0], sizeof(stPosPlanCfgArray), SHAPE_PLAN_ACTIONS, SHAPE_PLAN_CONDITIONS, SHAPE_PLAN_VARS, CIRCLE_LG_STATE_LENGTH, CIRCLE_LG_STATE_LENGTH);
}

void ST_setupSquarePlan(ST_Handle stHandle) {
    _iq velMax, accMax, decMax, jrkMax;
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // Reset the Plan configuration
    STPOSPLAN_reset(stObj->posPlanHandle);
    // Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
    velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
    accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
    decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
    jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

    // Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
    STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
    // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
    STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);    // VarIdx0: Go
    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_IN,    0);    // VarIdx1: Y Axis Done

    STPOSPLAN_addCfgCond(stObj->posPlanHandle, 1,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[0], 0, 0L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[1], 0, 0); // StateIdx1: 0
    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[2], 0, 0L); // StateIdx2: -20
    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[3], 0, 0L); // StateIdx3: 0
    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[4], 0, 0L); // StateIdx4: 20
    STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[5], 0, 0L); // StateIdx4: 0

    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 0, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 1, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 2, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 3, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 4, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 5, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state

    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_NC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 0, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));

    if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
        // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
        STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
        // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
        STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
      }
}

void ST_setupTrianglePlan(ST_Handle stHandle) {
    _iq velMax, accMax, decMax, jrkMax;
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // Reset the Plan configuration
    STPOSPLAN_reset(stObj->posPlanHandle);

    // Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
    velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
    accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
    decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
    jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

    // Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
    STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
    // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
    STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);    // VarIdx0: Go
    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_IN,    0);    // VarIdx1: Y Axis Done

    STPOSPLAN_addCfgCond(stObj->posPlanHandle, 1,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

    // put your plan here fool
    STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_isteps[0], triangle_fsteps[0], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_isteps[1], triangle_fsteps[1], 1000); // StateIdx1: 0
    STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_isteps[2], triangle_fsteps[2], 1); // StateIdx2: -17.3205
    STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_isteps[3], triangle_fsteps[3], 1); // StateIdx3: 17.3205
    STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_isteps[4], triangle_fsteps[4], 1000); // StateIdx4: 0

    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 0, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 1, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 2, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 3, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 4, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state

    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_NC, 0, 0, _IQmpy(triangle_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 0, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);

    if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
      // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
      STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
      // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
      STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
    }
}

void ST_setupCircleSmPlan(ST_Handle stHandle) {
    _iq velMax, accMax, decMax, jrkMax;
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // Reset the Plan configuration
    STPOSPLAN_reset(stObj->posPlanHandle);

    // Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
    velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
    accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
    decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
    jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

    // Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
    STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
    // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
    STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);    // VarIdx0: Go
    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_IN,    0);    // VarIdx1: Y Axis Done

    STPOSPLAN_addCfgCond(stObj->posPlanHandle, 1,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

    // put your plan here fool
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[0], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[1], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[2], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[3], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[4], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[5], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[6], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[7], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[8], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[9], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[10], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[11], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[12], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[13], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[14], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[15], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[16], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[17], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[18], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[19], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[20], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[21], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[22], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[23], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[24], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[25], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[26], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[27], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[28], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[29], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[30], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[31], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[32], 1L); // StateIdx0: start

    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 0, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 1, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 2, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 3, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 4, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 5, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 6, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 7, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 8, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 9, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 10, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 11, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 12, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 13, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 14, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 15, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 16, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 17, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 18, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 19, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 20, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 21, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 22, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 23, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 24, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 25, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 26, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 27, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 28, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 29, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 30, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 31, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 32, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state

    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_NC, 0, 0, _IQmpy(circle_sm_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 6, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[5], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 6, 7, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[6], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 7, 8, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[7], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 8, 9, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[8], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 9, 10, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[9], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 10, 11, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[10], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 11, 12, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[11], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 12, 13, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[12], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 13, 14, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[13], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 14, 15, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[14], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 15, 16, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[15], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 16, 17, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[16], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 17, 18, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[17], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 18, 19, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[18], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 19, 20, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[19], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 20, 21, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[20], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 21, 22, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[21], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 22, 23, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[22], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 23, 24, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[23], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 24, 25, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[24], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 25, 26, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[25], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 26, 27, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[26], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 27, 28, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[27], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 28, 29, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[28], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 29, 30, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[29], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 30, 31, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[30], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 31, 32, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[31], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 32, 0, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[32], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);

    if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
      // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
      STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
      // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
      STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
    }
}

void ST_setupCircleLgPlan(ST_Handle stHandle) {
    _iq velMax, accMax, decMax, jrkMax;
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // Reset the Plan configuration
    STPOSPLAN_reset(stObj->posPlanHandle);

    // Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
    velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
    accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
    decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
    jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

    // Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
    STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
    // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
    STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);    // VarIdx0: Go
    STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_IN,    0);    // VarIdx1: Y Axis Done

    STPOSPLAN_addCfgCond(stObj->posPlanHandle, 1,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

    // put your plan here fool
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[0], circle_lg_fsteps[0], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[1], circle_lg_fsteps[1], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[2], circle_lg_fsteps[2], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[3], circle_lg_fsteps[3], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[4], circle_lg_fsteps[4], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[5], circle_lg_fsteps[5], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[6], circle_lg_fsteps[6], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[7], circle_lg_fsteps[7], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[8], circle_lg_fsteps[8], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[9], circle_lg_fsteps[9], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[10], circle_lg_fsteps[10], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[11], circle_lg_fsteps[11], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[12], circle_lg_fsteps[12], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[13], circle_lg_fsteps[13], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[14], circle_lg_fsteps[14], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[15], circle_lg_fsteps[15], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[16], circle_lg_fsteps[16], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[17], circle_lg_fsteps[17], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[18], circle_lg_fsteps[18], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[19], circle_lg_fsteps[19], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[20], circle_lg_fsteps[20], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[21], circle_lg_fsteps[21], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[22], circle_lg_fsteps[22], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[23], circle_lg_fsteps[23], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[24], circle_lg_fsteps[24], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[25], circle_lg_fsteps[25], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[26], circle_lg_fsteps[26], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[27], circle_lg_fsteps[27], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[28], circle_lg_fsteps[28], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[29], circle_lg_fsteps[29], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[30], circle_lg_fsteps[30], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[31], circle_lg_fsteps[31], 1L); // StateIdx0: start
    STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[32], circle_lg_fsteps[32], 1L); // StateIdx0: start

    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 0, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 1, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 2, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 3, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 4, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 5, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 6, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 7, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 8, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 9, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 10, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 11, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 12, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 13, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 14, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 15, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 16, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 17, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 18, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 19, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 20, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 21, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 22, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 23, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 24, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 25, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 26, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 27, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 28, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 29, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 30, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 31, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state
    STPOSPLAN_addCfgAct(stObj->posPlanHandle, 32, 0, ST_ACT_EQ, 1, ST_ACT_EXIT);    // Set the X Axis done equal to 1 when we exit a state

    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_NC, 0, 0, _IQmpy(circle_lg_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 6, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[5], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 6, 7, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[6], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 7, 8, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[7], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 8, 9, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[8], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 9, 10, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[9], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 10, 11, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[10], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 11, 12, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[11], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 12, 13, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[12], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 13, 14, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[13], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 14, 15, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[14], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 15, 16, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[15], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 16, 17, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[16], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 17, 18, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[17], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 18, 19, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[18], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 19, 20, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[19], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 20, 21, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[20], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 21, 22, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[21], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 22, 23, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[22], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 23, 24, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[23], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 24, 25, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[24], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 25, 26, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[25], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 26, 27, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[26], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 27, 28, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[27], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 28, 29, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[28], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 29, 30, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[29], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 30, 31, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[30], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 31, 32, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[31], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
    STPOSPLAN_addCfgTran(stObj->posPlanHandle, 32, 0, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[32], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);

    if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
      // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
      STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
      // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
      STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
    }
}

void ST_runPosConv(ST_Handle stHandle)
{
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // get the electrical angle from the ENC module
    STPOSCONV_setElecAngle_erev(stObj->posConvHandle, ENC_getElecAngle(encHandle));
    // run the SpinTAC Position Converter
    STPOSCONV_run(stObj->posConvHandle);
}

void ST_runPosCtl(ST_Handle stHandle)
{
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // provide the updated references to the SpinTAC Position Control
    STPOSCTL_setPositionReference_mrev(stObj->posCtlHandle, STPOSMOVE_getPositionReference_mrev(stObj->posMoveHandle));
    STPOSCTL_setVelocityReference(stObj->posCtlHandle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle));
    STPOSCTL_setAccelerationReference(stObj->posCtlHandle, STPOSMOVE_getAccelerationReference(stObj->posMoveHandle));
    // provide the feedback to the SpinTAC Position Control
    STPOSCTL_setPositionFeedback_mrev(stObj->posCtlHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));

    // Run SpinTAC Position Control
    STPOSCTL_run(stObj->posCtlHandle);
    
    // Provide SpinTAC Position Control Torque Output to the FOC
    CTRL_setIq_ref_pu(ctrlHandle, STPOSCTL_getTorqueReference(stObj->posCtlHandle));
}

void ST_runPosMove(ST_Handle stHandle)
{
    ST_Obj *stObj = (ST_Obj *)stHandle;

    // Run SpinTAC Position Profile Generator
    // If we are not running a profile, and command indicates we should has been modified
    if((STPOSMOVE_getStatus(stObj->posMoveHandle) == ST_MOVE_IDLE) && (gMotorVars.RunPositionProfile == true)) {
        // Get the configuration for SpinTAC Position Move
        STPOSMOVE_setCurveType(stObj->posMoveHandle, gMotorVars.SpinTAC.PosMoveCurveType);
        STPOSMOVE_setPositionStep_mrev(stObj->posMoveHandle, gMotorVars.PosStepInt_MRev,  gMotorVars.PosStepFrac_MRev);
        STPOSMOVE_setVelocityLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
        STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
        // Enable the SpinTAC Position Profile Generator
        STPOSMOVE_setEnable(stObj->posMoveHandle, true);
        // clear the position step command
        gMotorVars.PosStepInt_MRev = 0;
        gMotorVars.PosStepFrac_MRev = 0;
        gMotorVars.RunPositionProfile = false;
    }

    STPOSMOVE_run(stObj->posMoveHandle);
}

void ST_runPosPlan(ST_Handle sthandle)
{
    ST_Obj *stObj = (ST_Obj *)sthandle;
    DRV_Obj *drvObj = (DRV_Obj *)drvHandle;
    CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;

    // SpinTAC Position Plan
    if(gPosPlanRunFlag == ST_PLAN_STOP && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
        if((STPOSMOVE_getDone(stObj->posMoveHandle) == true) && (EST_getState(obj->estHandle) == EST_State_OnLine)) {
            if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
                STPOSPLAN_setEnable(stObj->posPlanHandle, false);
                STPOSPLAN_setReset(stObj->posPlanHandle, true);
                gMotorVars.SpinTAC.PosPlanRun = gPosPlanRunFlag;
            }
            else {
                STPOSPLAN_setEnable(stObj->posPlanHandle, true);
                STPOSPLAN_setReset(stObj->posPlanHandle, false);
                gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
            }
        }
    }
    if(gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_STOP) {
        STPOSPLAN_setReset(stObj->posPlanHandle, true);
        STPOSMOVE_setEnable(stObj->posMoveHandle, false);
        gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
    }
    if(gPosPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_PAUSE) {
        STPOSPLAN_setEnable(stObj->posPlanHandle, false);
        STPOSMOVE_setEnable(stObj->posMoveHandle, false);
        gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
    }
    if(gPosPlanRunFlag == ST_PLAN_PAUSE && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
        STPOSPLAN_setEnable(stObj->posPlanHandle, true);
        gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
    }

    // read value for Y Done from GPIO41
    //Y_MoveDone = GPIO_read(drvObj->gpioHandle, GPIO_Number_41);
    // we want to force this to got between 0 and 1 before sending a 1 again
    if(Y_MoveDone == 1) {
        Y_MoveDoneLatch = 1;
    }
    else {
        Y_MoveDoneLatch = 0;
    }

    // set vars
    if(shapeSelectMode == 1) {
        STPOSPLAN_setVar(stObj->posPlanHandle, 1, Y_MoveDoneLatch);
    }

    // Run SpinTAC Position Plan
    STPOSPLAN_run(stObj->posPlanHandle);

    // get vars
    if(shapeSelectMode == 1) {
        STPOSPLAN_getVar(stObj->posPlanHandle, 0, (_iq24 *)&X_Go);
    }

    // making a little one shot...
    if((X_Go == 1) && (OneShotLatch < 50)) {
        X_GoOneShot = 1;
        OneShotLatch++;
        if(OneShotLatch > 49) {
            X_GoOneShot = 0;
            STPOSPLAN_setVar(stObj->posPlanHandle, 0, 0);
        }
    }
    else if(X_Go == 0) {
        // clear latch
        OneShotLatch = 0;
        // clear one shot
        X_GoOneShot = 0;
    }
    // write the value for the Go bit out to GPIO40
    if(X_GoOneShot) {
        GPIO_setHigh(drvObj->gpioHandle,GPIO_Number_40);
    }
    else {
        GPIO_setLow(drvObj->gpioHandle,GPIO_Number_40);
    }

    // if we are not selecting a state machine, display the state
    if(shapeSelectMode == 1) {
        switch(STPOSPLAN_getCurrentState(stObj->posPlanHandle)) {
        case 0:
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
        break;
        case 1:
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
            GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
        break;
        case 2:
            GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
        break;
        case 3:
            GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
            GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
        break;
        case 4:
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
            GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
        break;
        }
    }

    if(STPOSPLAN_getStatus(stObj->posPlanHandle) != ST_PLAN_IDLE) {
        // if we are running the plan, don't let it switch profiles
        shapeSelectMode = 1;
        // Send the profile configuration to SpinTAC Position Move
        STPOSPLAN_getPositionStep_mrev(stObj->posPlanHandle, (_iq24 *)&gMotorVars.PosStepInt_MRev, (_iq24 *)&gMotorVars.PosStepFrac_MRev);
        gMotorVars.RunPositionProfile = true;
        gMotorVars.MaxVel_krpm = _IQmpy(STPOSPLAN_getVelocityLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
        gMotorVars.MaxAccel_krpmps = _IQmpy(STPOSPLAN_getAccelerationLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
        gMotorVars.MaxDecel_krpmps = _IQmpy(STPOSPLAN_getDecelerationLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
        gMotorVars.MaxJrk_krpmps2 = _IQ20mpy(STPOSPLAN_getJerkLimit(stObj->posPlanHandle), _IQ20(ST_SPEED_KRPM_PER_PU));
    }
    else
    {
        if(gPosPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
            gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_STOP;
            gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
        }
    }
}

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

Y-AXIS:


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

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

#include "shape_square.h"
#include "shape_triangle.h"
#include "shape_circle_small.h"
#include "shape_circle_large.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

// State Machine Options for this Project
typedef enum
{
	SQUARE=0,    // Square Pattern
	TRIANGLE,    // Triangle Pattern
	SM_CIRCLE,   // Small Circle Pattern
	LG_CIRCLE,    // Large Circle Pattern
	MAX_SHAPE
} PLAN_SELECT_e;

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

uint_least16_t gCounter_updateGlobals = 0;

bool Flag_Latch_softwareUpdate = true;

CTRL_Handle ctrlHandle;

DRV_Handle drvHandle;

USER_Params gUserParams;

DRV_PwmData_t gPwmData;

DRV_AdcData_t gAdcData;

_iq gMaxCurrentSlope = _IQ(0.0);

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

ENC_Handle encHandle;
ENC_Obj enc;

ST_Obj st_obj;
ST_Handle stHandle;

_iq square_steps[SQUARE_STATE_LENGTH] = SQUARE_Y_STEPS;
_iq square_speed = SQUARE_Y_SPEED;

_iq triangle_steps[TRIANGLE_STATE_LENGTH] = TRIANGLE_Y_STEPS;
_iq triangle_speeds[TRIANGLE_STATE_LENGTH] = TRIANGLE_Y_SPEEDS;

_iq circle_sm_steps[CIRCLE_SM_STATE_LENGTH] = CIRCLE_SM_Y_STEPS;
_iq circle_sm_speeds[CIRCLE_SM_STATE_LENGTH] = CIRCLE_SM_Y_SPEEDS;

_iq circle_lg_isteps[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_Y_ISTEPS;
_iq circle_lg_fsteps[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_Y_FSTEPS;
_iq circle_lg_speeds[CIRCLE_LG_STATE_LENGTH] = CIRCLE_LG_Y_SPEEDS;

// only
#define SHAPE_PLAN_ACTIONS    (CIRCLE_LG_STATE_LENGTH)
#define SHAPE_PLAN_CONDITIONS 1
#define SHAPE_PLAN_VARS       2

// Used to select a state machine
PLAN_SELECT_e gSelectedPlan = SQUARE;

// Used to hold the current configured state machine
PLAN_SELECT_e gConfiguredPlan = SQUARE;

// interfacing variable
uint32_t X_Go = 0;
uint32_t X_GoOld = 0;
uint32_t X_GoLatch = 0;
uint16_t debounceCount_X_Go = 0;
uint32_t Y_Done = 0;

uint32_t shapeSelectMode = 1;
uint32_t shapeSelectModeSwitch = 1;
uint16_t debounceCount_shapeSelectMode = 0;
uint32_t shapeSelectAdv = 1;
uint32_t shapeSelectAdvLatch = 0;
uint16_t debounceCount_shapeSelectAdv = 0;

// Used to handle controlling SpinTAC Plan
ST_PlanButton_e gPosPlanRunFlag = ST_PLAN_STOP;

// Calculates the amount of memory required for the SpinTAC Position Plan configuration
// This is based on the above enumerations
#define ST_POSPLAN_CFG_ARRAY_DWORDS (   (ST_POS_PLAN_TRAN_DWORDS  * CIRCLE_LG_STATE_LENGTH)   + \
                                        (ST_POS_PLAN_STATE_DWORDS * CIRCLE_LG_STATE_LENGTH)   + \
                                        (ST_POS_PLAN_ACT_DWORDS   * SHAPE_PLAN_ACTIONS)    + \
                                        (ST_POS_PLAN_COND_DWORDS  * SHAPE_PLAN_CONDITIONS) + \
                                        (ST_POS_PLAN_VAR_DWORDS   * SHAPE_PLAN_VARS))

// Used to store the configuration of SpinTAC Position Plan
uint32_t stPosPlanCfgArray[ST_POSPLAN_CFG_ARRAY_DWORDS];

uint16_t gLEDcnt = 0;

volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT;

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


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

// **************************************************************************
// the functions
// plans
void ST_setupSquarePlan(ST_Handle stHandle);
void ST_setupTrianglePlan(ST_Handle stHandle);
void ST_setupCircleSmPlan(ST_Handle stHandle);
void ST_setupCircleLgPlan(ST_Handle stHandle);

void main(void)
{

  uint_least8_t estNumber = 0;

#ifdef FAST_ROM_V1p6
  uint_least8_t ctrlNumber = 0;
#endif

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

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


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


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


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


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


  // setup faults
  DRV_setupFaults(drvHandle);


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


  // enable the ADC interrupts
  DRV_enableAdcInts(drvHandle);


  // enable global interrupts
  DRV_enableGlobalInts(drvHandle);


  // enable debug interrupts
  DRV_enableDebugInt(drvHandle);


  // disable the PWM
  DRV_disablePwm(drvHandle);


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


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

  // setup the ENC module
  DRV_Obj *drv_obj = (DRV_Obj *)drvHandle;
  ENC_setup(encHandle, drv_obj->qepHandle[0], 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 SpinTAC Components
  stHandle = ST_init(&st_obj, sizeof(st_obj));


  // setup the SpinTAC Components
  ST_setupPosConv(stHandle);
  ST_setupPosCtl(stHandle);
  ST_setupPosMove(stHandle);
  ST_setupPosPlan(stHandle);

  // configure the actual plan
  switch(gSelectedPlan) {
      case SQUARE:
    	  ST_setupSquarePlan(stHandle);
	  break;
      case TRIANGLE:
    	  ST_setupTrianglePlan(stHandle);
	  break;
      case SM_CIRCLE:
    	  ST_setupCircleSmPlan(stHandle);
	  break;
      case LG_CIRCLE:
          ST_setupCircleLgPlan(stHandle);
      break;
  }
  // report selected state machine
    gConfiguredPlan = gSelectedPlan;


#ifdef FLASH
  gMotorVars.Flag_enableSys = 1;
  gMotorVars.Flag_Run_Identify = 1;
  gMotorVars.SpinTAC.PosMoveCurveType = ST_MOVE_CUR_TRAP;
#endif
#ifdef DRV8301_SPI
  // turn on the DRV8301 if present
  DRV_enable8301(drvHandle);
  // initialize the DRV8301 interface
  DRV_8301_SpiInterface_init(drvHandle,&gDrvSpi8301Vars);
#endif

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


    // select if we should identify the motor
    CTRL_setFlag_enableUserMotorParams(ctrlHandle, gMotorVars.Flag_enableUserParams);

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

      // increment counters
      gCounter_updateGlobals++;

      // change the current state
      if(shapeSelectMode == 0) {
    	  // indicate we are in this mode
    	  DRV_turnLedOn(drvHandle,(GPIO_Number_e)DRV_Gpio_LED3);
    	  // display current selected plan
    	  switch(gSelectedPlan) {
		    case SQUARE:
		    	GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
		    	GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
		    break;
		    case TRIANGLE:
		    	GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
		    	GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
		    break;
		    case SM_CIRCLE:
		    	GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
		    	GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
		    break;
		    case LG_CIRCLE:
		    	GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
		    	GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
		    break;
		  }
    	  // if the switch is pressed, advance plan
    	  if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
    		  shapeSelectAdvLatch = 1;
    		  gSelectedPlan++;
    		  if(gSelectedPlan == MAX_SHAPE) {
    			  gSelectedPlan = SQUARE;
    		  }
    	  }
    	  else if(shapeSelectAdv == 1) {
    		  shapeSelectAdvLatch = 0;
    	  }
      }
      else {
    	  // indicate we are not in shape select mode
    	  DRV_turnLedOff(drvHandle,(GPIO_Number_e)DRV_Gpio_LED3);

    	  if(STPOSPLAN_getStatus(stObj->posPlanHandle) == ST_PLAN_BUSY) {
			  GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_22);
			  // if we press, the START button, stop PLAN
			  if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
				  shapeSelectAdvLatch = 1;
				  gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_STOP;
			  }
			  else if(shapeSelectAdv == 1) {
	    		  shapeSelectAdvLatch = 0;
	    	  }
		  }
		  else {
			  GPIO_setLow(drvObj->gpioHandle, GPIO_Number_22);
			  if((shapeSelectAdv == 0) && (shapeSelectAdvLatch == 0)) {
				  shapeSelectAdvLatch = 1;
				  gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_START;
			  }
			  else if(shapeSelectAdv == 1) {
				  shapeSelectAdvLatch = 0;
			  }
		  }
      }

      // configure a different Plan if selected
	  if((gSelectedPlan != gConfiguredPlan) && (STPOSPLAN_getStatus(stObj->posPlanHandle) == ST_PLAN_IDLE))
	  {
		// setup the selected Plan
		switch(gSelectedPlan) {
		  case SQUARE:
			ST_setupSquarePlan(stHandle);
		  break;
		  case TRIANGLE:
		    ST_setupTrianglePlan(stHandle);
		  break;
		  case SM_CIRCLE:
		    ST_setupCircleSmPlan(stHandle);
		  break;
		  case LG_CIRCLE:
			ST_setupCircleLgPlan(stHandle);
		  break;
		}

		// report selected state machine
		gConfiguredPlan = gSelectedPlan;
	  }

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

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

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

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

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

              if(ctrlState == CTRL_State_OffLine) {
                  // enable the PWM
                  DRV_enablePwm(drvHandle);
              }
              else if(ctrlState == CTRL_State_OnLine) {
                  // update the ADC bias values
                  DRV_updateAdcBias(drvHandle);

                  // enable the PWM
                  DRV_enablePwm(drvHandle);
                  // initialize the watch window Bw value with the default value
                  gMotorVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidthScale(stObj->posCtlHandle);
              }
              else if(ctrlState == CTRL_State_Idle) {
                  // disable the PWM
                  DRV_disablePwm(drvHandle);
                  gMotorVars.Flag_Run_Identify = false;
              }
          }

          if(EST_getState(obj->estHandle) == EST_State_OnLine) {
        	  // remove ST_POS_CTL from reset
        	  STPOSCTL_setReset(stObj->posCtlHandle, false);
        	  STPOSCTL_setEnable(stObj->posCtlHandle, true);
          }
          else
          {
        	  // place SpinTAC Position Control into reset
        	  STPOSCTL_setReset(stObj->posCtlHandle, true);
			  // If motor is not running, feed the position feedback into SpinTAC Position Move
              STPOSMOVE_setPositionStart_mrev(stObj->posMoveHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));
          }
      }


      if(EST_isMotorIdentified(obj->estHandle)) {
          // set the speed reference acceleration
          EST_setMaxCurrentSlope_pu(obj->estHandle,gMaxCurrentSlope);
          gMotorVars.Flag_MotorIdentified = true;
		  EST_setFlag_enableRsRecalc(obj->estHandle, gMotorVars.Flag_enableRsRecalc);
		  CTRL_setFlag_enableSpeedCtrl(ctrlHandle, false);
		  if(Flag_Latch_softwareUpdate)
          {
              Flag_Latch_softwareUpdate = false;

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

              calcPIgains(ctrlHandle);
          }
      }
      else {
          Flag_Latch_softwareUpdate = true;

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


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

          updateGlobalVariables_motor(ctrlHandle, stHandle);
      }

  } // end of while(gFlag_enableSys) loop


  // disable the PWM
  DRV_disablePwm(drvHandle);

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

    // setup the SpinTAC Components
    ST_setupPosConv(stHandle);
    ST_setupPosCtl(stHandle);
    ST_setupPosMove(stHandle);
	ST_setupPosPlan(stHandle);

  } // end of for(;;) loop

} // end of main() function


interrupt void mainISR(void)
{
  static uint16_t stCnt = 0;
  CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;
  DRV_Obj * drvObj = (DRV_Obj *)drvHandle;
  

  // compute the electrical angle
  ENC_calcElecAngle(encHandle);

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


  // do a debounce on the X_Go signal
  if(X_Go != GPIO_read(drvObj->gpioHandle, GPIO_Number_40)) {
	  debounceCount_X_Go++;
	  if(debounceCount_X_Go > 5) {
		  X_Go = GPIO_read(drvObj->gpioHandle, GPIO_Number_40);
	  }
  }
  else {
	  debounceCount_X_Go = 0;
  }

  // do a debounce on the select switch
  if(shapeSelectModeSwitch != GPIO_read(drvObj->gpioHandle, GPIO_Number_7)) {
    debounceCount_shapeSelectMode++;
    if(debounceCount_shapeSelectMode > 500) {
      shapeSelectModeSwitch = GPIO_read(drvObj->gpioHandle, GPIO_Number_7);
      if(shapeSelectModeSwitch == 0) {
    	  shapeSelectMode = !shapeSelectMode;
      }
    }
  }
  else {
    debounceCount_shapeSelectMode = 0;
  }

  // do a debounce on the advance switch
  if(shapeSelectAdv != GPIO_read(drvObj->gpioHandle, GPIO_Number_9)) {
    debounceCount_shapeSelectAdv++;
    if(debounceCount_shapeSelectAdv > 500) {
    	shapeSelectAdv = GPIO_read(drvObj->gpioHandle, GPIO_Number_9);
    }
  }
  else {
	  debounceCount_shapeSelectAdv = 0;
  }

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


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


  // Run the SpinTAC Speed controller
  if(stCnt++ >= ISR_TICKS_PER_SPINTAC_TICK) {
	  ST_runPosConv(stHandle);
	  ST_runPosPlanTick(stHandle);
	  ST_runPosPlan(stHandle);
	  ST_runPosMove(stHandle);
	  ST_runPosCtl(stHandle);
	  stCnt = 1;
  }


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


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


  // setup the controller
  CTRL_setup(ctrlHandle);

  // 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)
  {
	  ENC_setZeroOffset(encHandle, (uint32_t)(ENC_getPositionMax(encHandle) - ENC_getRawEncoderCounts(encHandle)));
  }

  return;
} // end of mainISR() function


void updateGlobalVariables_motor(CTRL_Handle handle, ST_Handle stHandle)
{
  uint16_t stPosPlanCfgErrIdx, stPosPlanCfgErrCode;
  uint32_t ProTime_tick, ProTime_mtick;
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  ST_Obj *stObj = (ST_Obj *)stHandle;
  // get the speed estimate
  gMotorVars.Speed_krpm = _IQmpy(STPOSCONV_getVelocityFiltered(stObj->posConvHandle), _IQ(ST_SPEED_KRPM_PER_PU));

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

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

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

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

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

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

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

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

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

  // gets the Position Controller status
  gMotorVars.SpinTAC.PosCtlStatus = STPOSCTL_getStatus(stObj->posCtlHandle);
	
  // get the inertia setting
  gMotorVars.SpinTAC.Inertia_Aperkrpm = _IQmpy(STPOSCTL_getInertia(stObj->posCtlHandle), _IQ(ST_SPEED_PU_PER_KRPM * USER_IQ_FULL_SCALE_CURRENT_A));

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

  // set the SpinTAC (ST) bandwidth scale
  STPOSCTL_setBandwidthScale(stObj->posCtlHandle, gMotorVars.SpinTAC.PosCtlBwScale);

  // get the SpinTAC (ST) bandwidth
  gMotorVars.SpinTAC.PosCtlBw_radps = STPOSCTL_getBandwidth_radps(stObj->posCtlHandle);

  // get the Position Controller error
  gMotorVars.SpinTAC.PosCtlErrorID = STPOSCTL_getErrorID(stObj->posCtlHandle);

  // get the Position Move status
  gMotorVars.SpinTAC.PosMoveStatus = STPOSMOVE_getStatus(stObj->posMoveHandle);

  // get the Position Move profile time
  STPOSMOVE_getProfileTime_tick(stObj->posMoveHandle, &ProTime_tick, &ProTime_mtick);
  gMotorVars.SpinTAC.PosMoveTime_ticks = ProTime_tick;
  gMotorVars.SpinTAC.PosMoveTime_mticks = ProTime_mtick;

  // get the Position Move done signal
  gMotorVars.SpinTAC.PosMoveDone = STPOSMOVE_getDone(stObj->posMoveHandle);

  // get the Position Move error
  gMotorVars.SpinTAC.PosMoveErrorID = STPOSMOVE_getErrorID(stObj->posMoveHandle);
  
  // get the Position Plan status
  gMotorVars.SpinTAC.PosPlanStatus = STPOSPLAN_getStatus(stObj->posPlanHandle);

  // get the Position Plan Done signal
  gMotorVars.SpinTAC.PosPlanDone = STPOSPLAN_getDone(stObj->posPlanHandle);

  // get the Position Plan error
  gMotorVars.SpinTAC.PosPlanErrorID = STPOSPLAN_getCfgError(stObj->posPlanHandle, &stPosPlanCfgErrIdx, &stPosPlanCfgErrCode);
  gMotorVars.SpinTAC.PosPlanCfgErrorIdx = stPosPlanCfgErrIdx;
  gMotorVars.SpinTAC.PosPlanCfgErrorCode = stPosPlanCfgErrCode;

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

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

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

  return;
} // end of updateGlobalVariables_motor() function

#ifdef FAST_ROM_V1p6
void softwareUpdate1p6(CTRL_Handle handle)
{

  CTRL_Obj *obj = (CTRL_Obj *)handle;

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


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

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

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

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

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

  return;
} // end of calcPIgains() function

void ST_setupPosPlan(ST_Handle sthandle) {

  ST_Obj *stObj = (ST_Obj *)sthandle;

  // Pass the configuration array pointer into SpinTAC Position Plan
  STPOSPLAN_setCfgArray(stObj->posPlanHandle, &stPosPlanCfgArray[0], sizeof(stPosPlanCfgArray), SHAPE_PLAN_ACTIONS, SHAPE_PLAN_CONDITIONS, SHAPE_PLAN_VARS, CIRCLE_LG_STATE_LENGTH, CIRCLE_LG_STATE_LENGTH);
}

void ST_setupSquarePlan(ST_Handle stHandle) {
	_iq velMax, accMax, decMax, jrkMax;
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// Reset the Plan configuration
	STPOSPLAN_reset(stObj->posPlanHandle);
	// Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
	velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
	accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
	decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
	jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

	// Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
	STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	// Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

	STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);	// VarIdx0: Go

	STPOSPLAN_addCfgCond(stObj->posPlanHandle, 0,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done
	// put your plan here fool
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[0], 0, 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[1], 0, 0L); // StateIdx1: 10
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[2], 0, 0L); // StateIdx2: 0
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[3], 0, 0L); // StateIdx3: -20
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[4], 0, 0L); // StateIdx4: 0
	STPOSPLAN_addCfgState(stObj->posPlanHandle, square_steps[5], 0, 0L); // StateIdx4: 10

	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 0, ST_COND_FC, 0, 0, _IQmpy(square_speed, _IQ(ST_SPEED_PU_PER_KRPM)), _IQ(0.1), _IQ(0.1), _IQ20(1));

	if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
		// Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
		STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
		// Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
		STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
	  }
}

void ST_setupTrianglePlan(ST_Handle stHandle) {
	_iq velMax, accMax, decMax, jrkMax;
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// Reset the Plan configuration
	STPOSPLAN_reset(stObj->posPlanHandle);

	// Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
	velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
	accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
	decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
	jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

	// Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
	STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	// Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

	STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);	// VarIdx0: Go

	STPOSPLAN_addCfgCond(stObj->posPlanHandle, 0,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done
	// put your plan here fool
	STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_steps[0], 0, 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_steps[1], 0, 0L); // StateIdx1: 10
	STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_steps[2], 0, 0L); // StateIdx2: -10
	STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_steps[3], 0, 0L); // StateIdx3: -10
	STPOSPLAN_addCfgState(stObj->posPlanHandle, triangle_steps[4], 0, 0L); // StateIdx3: 10

	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 0, ST_COND_FC, 0, 0, _IQmpy(triangle_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);


	if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
	  // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
	  STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	  // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	  STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
	}
}

void ST_setupCircleSmPlan(ST_Handle stHandle) {
	_iq velMax, accMax, decMax, jrkMax;
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// Reset the Plan configuration
	STPOSPLAN_reset(stObj->posPlanHandle);

	// Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
	velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
	accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
	decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
	jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

	// Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
	STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	// Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

	STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);	// VarIdx0: Go

	STPOSPLAN_addCfgCond(stObj->posPlanHandle, 0,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

	// put your plan here fool
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[0], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[1], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[2], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[3], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[4], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[5], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[6], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[7], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[8], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[9], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[10], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[11], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[12], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[13], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[14], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[15], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[16], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[17], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[18], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[19], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[20], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[21], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[22], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[23], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[24], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[25], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[26], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[27], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[28], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[29], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[30], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[31], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, 0, circle_sm_steps[32], 0L); // StateIdx0: start

	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 6, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[5], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 6, 7, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[6], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 7, 8, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[7], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 8, 9, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[8], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 9, 10, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[9], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 10, 11, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[10], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 11, 12, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[11], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 12, 13, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[12], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 13, 14, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[13], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 14, 15, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[14], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 15, 16, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[15], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 16, 17, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[16], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 17, 18, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[17], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 18, 19, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[18], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 19, 20, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[19], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 20, 21, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[20], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 21, 22, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[21], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 22, 23, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[22], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 23, 24, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[23], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 24, 25, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[24], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 25, 26, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[25], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 26, 27, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[26], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 27, 28, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[27], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 28, 29, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[28], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 29, 30, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[29], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 30, 31, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[30], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 31, 32, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[31], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 32, 0, ST_COND_FC, 0, 0, _IQmpy(circle_sm_speeds[32], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);

	if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
	  // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
	  STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	  // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	  STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
	}
}

void ST_setupCircleLgPlan(ST_Handle stHandle) {
	_iq velMax, accMax, decMax, jrkMax;
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// Reset the Plan configuration
	STPOSPLAN_reset(stObj->posPlanHandle);

	// Establish the Velocity, Acceleration, Deceleration, and Jerk Maximums
	velMax = STPOSMOVE_getVelocityMaximum(stObj->posMoveHandle);
	accMax = STPOSMOVE_getAccelerationMaximum(stObj->posMoveHandle);
	decMax = STPOSMOVE_getDecelerationMaximum(stObj->posMoveHandle);
	jrkMax = STPOSMOVE_getJerkMaximum(stObj->posMoveHandle);

	// Configure SpinTAC Position Plan: Sample Time, VelMax, AccMax, DecMax, JrkMax, LoopENB
	STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	// Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);

	STPOSPLAN_addCfgVar(stObj->posPlanHandle, ST_VAR_INOUT, 0);	// VarIdx0: Go

	STPOSPLAN_addCfgCond(stObj->posPlanHandle, 0,      ST_COMP_EQ,  1,      0); // CondIdx0: Y Axis reports done

	// put your plan here fool
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[0], circle_lg_fsteps[0], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[1], circle_lg_fsteps[1], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[2], circle_lg_fsteps[2], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[3], circle_lg_fsteps[3], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[4], circle_lg_fsteps[4], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[5], circle_lg_fsteps[5], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[6], circle_lg_fsteps[6], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[7], circle_lg_fsteps[7], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[8], circle_lg_fsteps[8], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[9], circle_lg_fsteps[9], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[10], circle_lg_fsteps[10], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[11], circle_lg_fsteps[11], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[12], circle_lg_fsteps[12], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[13], circle_lg_fsteps[13], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[14], circle_lg_fsteps[14], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[15], circle_lg_fsteps[15], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[16], circle_lg_fsteps[16], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[17], circle_lg_fsteps[17], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[18], circle_lg_fsteps[18], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[19], circle_lg_fsteps[19], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[20], circle_lg_fsteps[20], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[21], circle_lg_fsteps[21], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[22], circle_lg_fsteps[22], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[23], circle_lg_fsteps[23], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[24], circle_lg_fsteps[24], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[25], circle_lg_fsteps[25], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[26], circle_lg_fsteps[26], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[27], circle_lg_fsteps[27], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[28], circle_lg_fsteps[28], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[29], circle_lg_fsteps[29], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[30], circle_lg_fsteps[30], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[31], circle_lg_fsteps[31], 0L); // StateIdx0: start
	STPOSPLAN_addCfgState(stObj->posPlanHandle, circle_lg_isteps[32], circle_lg_fsteps[32], 0L); // StateIdx0: start

	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 0, 1, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[0], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 1, 2, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[1], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 2, 3, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[2], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 3, 4, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[3], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 4, 5, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[4], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 5, 6, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[5], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 6, 7, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[6], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 7, 8, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[7], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 8, 9, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[8], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 9, 10, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[9], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 10, 11, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[10], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 11, 12, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[11], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 12, 13, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[12], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 13, 14, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[13], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 14, 15, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[14], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 15, 16, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[15], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 16, 17, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[16], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 17, 18, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[17], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 18, 19, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[18], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 19, 20, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[19], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 20, 21, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[20], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 21, 22, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[21], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 22, 23, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[22], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 23, 24, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[23], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 24, 25, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[24], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 25, 26, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[25], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 26, 27, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[26], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 27, 28, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[27], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 28, 29, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[28], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 29, 30, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[29], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 30, 31, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[30], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 31, 32, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[31], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);
	STPOSPLAN_addCfgTran(stObj->posPlanHandle, 32, 0, ST_COND_FC, 0, 0, _IQmpy(circle_lg_speeds[32], _IQ(ST_SPEED_PU_PER_KRPM)), accMax, decMax, jrkMax);

	if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
	  // Configure FSM: Ts, VelMax, AccMax, DecMax, JrkMax, LoopENB
	  STPOSPLAN_setCfg(stObj->posPlanHandle, _IQ(ST_SAMPLE_TIME), velMax, accMax, decMax, jrkMax, false);
	  // Configure halt state: PosStep[MRev], PosStepFrac[MRev], VelEnd, AccMax, JrkMax, Timer
	  STPOSPLAN_setCfgHaltState(stObj->posPlanHandle, 0, 0, velMax, accMax, jrkMax, 1000L);
	}
}

void ST_runPosConv(ST_Handle stHandle)
{
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// get the electrical angle from the ENC module
    STPOSCONV_setElecAngle_erev(stObj->posConvHandle, ENC_getElecAngle(encHandle));
	// run the SpinTAC Position Converter
	STPOSCONV_run(stObj->posConvHandle);
}

void ST_runPosCtl(ST_Handle stHandle)
{
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// provide the updated references to the SpinTAC Position Control
	STPOSCTL_setPositionReference_mrev(stObj->posCtlHandle, STPOSMOVE_getPositionReference_mrev(stObj->posMoveHandle));
	STPOSCTL_setVelocityReference(stObj->posCtlHandle, STPOSMOVE_getVelocityReference(stObj->posMoveHandle));
	STPOSCTL_setAccelerationReference(stObj->posCtlHandle, STPOSMOVE_getAccelerationReference(stObj->posMoveHandle));
	// provide the feedback to the SpinTAC Position Control
	STPOSCTL_setPositionFeedback_mrev(stObj->posCtlHandle, STPOSCONV_getPosition_mrev(stObj->posConvHandle));

	// Run SpinTAC Position Control
	STPOSCTL_run(stObj->posCtlHandle);
	
	// Provide SpinTAC Position Control Torque Output to the FOC
	CTRL_setIq_ref_pu(ctrlHandle, STPOSCTL_getTorqueReference(stObj->posCtlHandle));
}

void ST_runPosMove(ST_Handle stHandle)
{
	ST_Obj *stObj = (ST_Obj *)stHandle;

	if(gMotorVars.PosStepInt_MRev > 25) {
		// can't more more than 25 MRevs...
		gMotorVars.RunPositionProfile = false;

	}

	// Run SpinTAC Position Profile Generator
	// If we are not running a profile, and command indicates we should has been modified
	if((STPOSMOVE_getStatus(stObj->posMoveHandle) == ST_MOVE_IDLE) && (gMotorVars.RunPositionProfile == true)) {
		// Get the configuration for SpinTAC Position Move
		STPOSMOVE_setCurveType(stObj->posMoveHandle, gMotorVars.SpinTAC.PosMoveCurveType);
		STPOSMOVE_setPositionStep_mrev(stObj->posMoveHandle, gMotorVars.PosStepInt_MRev,  gMotorVars.PosStepFrac_MRev);
		STPOSMOVE_setVelocityLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
		// Enable the SpinTAC Position Profile Generator
		STPOSMOVE_setEnable(stObj->posMoveHandle, true);
		// clear the position step command
		gMotorVars.PosStepInt_MRev = 0;
		gMotorVars.PosStepFrac_MRev = 0;
		gMotorVars.RunPositionProfile = false;
	}

	STPOSMOVE_run(stObj->posMoveHandle);
}

void ST_runPosPlan(ST_Handle sthandle)
{
    ST_Obj *stObj = (ST_Obj *)sthandle;
    DRV_Obj *drvObj = (DRV_Obj *)drvHandle;
    CTRL_Obj *obj = (CTRL_Obj *)ctrlHandle;

	// SpinTAC Position Plan
	if(gPosPlanRunFlag == ST_PLAN_STOP && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
		if((STPOSMOVE_getDone(stObj->posMoveHandle) == true) && (EST_getState(obj->estHandle) == EST_State_OnLine)) {
			if(STPOSPLAN_getErrorID(stObj->posPlanHandle) != false) {
				STPOSPLAN_setEnable(stObj->posPlanHandle, false);
				STPOSPLAN_setReset(stObj->posPlanHandle, true);
				gMotorVars.SpinTAC.PosPlanRun = gPosPlanRunFlag;
			}
			else {
				STPOSPLAN_setEnable(stObj->posPlanHandle, true);
				STPOSPLAN_setReset(stObj->posPlanHandle, false);
				gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
			}
		}
	}
	if(gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_STOP) {
		STPOSPLAN_setReset(stObj->posPlanHandle, true);
		STPOSMOVE_setEnable(stObj->posMoveHandle, false);
		gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
	}
	if(gPosPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_PAUSE) {
		STPOSPLAN_setEnable(stObj->posPlanHandle, false);
		STPOSMOVE_setEnable(stObj->posMoveHandle, false);
		gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
	}
	if(gPosPlanRunFlag == ST_PLAN_PAUSE && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
		STPOSPLAN_setEnable(stObj->posPlanHandle, true);
		gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
	}
	// set vars
	// read the GO bit from GPIO40
	//X_Go = GPIO_read(drvObj->gpioHandle, GPIO_Number_40);

	if((X_Go == 1) && (X_GoOld == 0)) {
		// rising edge
		X_GoLatch = 1;
	}
	else {
		X_GoLatch = 0;
	}

	X_GoOld = X_Go;
	if(shapeSelectMode == 1) {
		STPOSPLAN_setVar(stObj->posPlanHandle, 0, X_GoLatch);
	}
	// Run SpinTAC Position Plan
	STPOSPLAN_run(stObj->posPlanHandle);

	Y_Done = STPOSMOVE_getDone(stObj->posMoveHandle);
	if(STPOSPLAN_getFsmState(stObj->posPlanHandle) != ST_FSM_STATE_STAY) {
		if(Y_Done) { // write this out to GPIO41
			GPIO_setHigh(drvObj->gpioHandle,GPIO_Number_41);
		}
		else {
			GPIO_setLow(drvObj->gpioHandle,GPIO_Number_41);
		}
	}

	// if we are not selecting a state machine, display the state
	if(shapeSelectMode == 1) {
		switch(STPOSPLAN_getCurrentState(stObj->posPlanHandle)) {
		case 0:
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
		break;
		case 1:
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
			GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
		break;
		case 2:
			GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
		break;
		case 3:
			GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_12);
			GPIO_setHigh(drvObj->gpioHandle, GPIO_Number_15);
		break;
		case 4:
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_12);
			GPIO_setLow(drvObj->gpioHandle, GPIO_Number_15);
		break;
		}
	}

	if(STPOSPLAN_getStatus(stObj->posPlanHandle) != ST_PLAN_IDLE) {
		// if we are running the plan, don't let it switch profiles
		shapeSelectMode = 1;
		// Send the profile configuration to SpinTAC Position Move
		STPOSPLAN_getPositionStep_mrev(stObj->posPlanHandle, (_iq24 *)&gMotorVars.PosStepInt_MRev, (_iq24 *)&gMotorVars.PosStepFrac_MRev);
		gMotorVars.RunPositionProfile = true;
		gMotorVars.MaxVel_krpm = _IQmpy(STPOSPLAN_getVelocityLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
		gMotorVars.MaxAccel_krpmps = _IQmpy(STPOSPLAN_getAccelerationLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
		gMotorVars.MaxDecel_krpmps = _IQmpy(STPOSPLAN_getDecelerationLimit(stObj->posPlanHandle), _IQ(ST_SPEED_KRPM_PER_PU));
		gMotorVars.MaxJrk_krpmps2 = _IQ20mpy(STPOSPLAN_getJerkLimit(stObj->posPlanHandle), _IQ20(ST_SPEED_KRPM_PER_PU));
	}
	else
	{
		if(gPosPlanRunFlag == ST_PLAN_START && gMotorVars.SpinTAC.PosPlanRun == ST_PLAN_START) {
			gMotorVars.SpinTAC.PosPlanRun = ST_PLAN_STOP;
			gPosPlanRunFlag = gMotorVars.SpinTAC.PosPlanRun;
		}
	}
}

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

Thank you for the explanation on the X-axis STATE advance. I got that part now. (didn't notice the pullup...)

Following your suggestion, I used a scope to measure gpio 41 and cannot capture any drop down moment at all (my time division is set at 20ms.). It is kept at about 3V.

Also, I'm attaching my setup pictures here. I know it's hard to tell from a picture, but does it look like there's anything wrong on it??

Your setup looks good.  I really like your CNC router.  

From the pictures I don't see anything wrong.  It looks like you have connected everything in the way I would have expected.  

From your explanation, I'm guessing that the Y_DONE signal never going to 0 is the problem.

What I would recommend as a good test is start the two plans, disconnect the two boards and watch the Y_DONE signal from the y-axis.  This should tell us if the y-axis is having trouble driving the signal or if the issue is that the x-axis board is holding the signal as high.

Adam,

It finally works!! It turns out that I didn't copy all of the files from example code folder correctly... It's quite a minor thing but your previous reply made me learn sth. I really appreciate that! 

I found that the Y-axis has a limitation of 25 rotation maximum. Is that right? Where (which file) actually this limit was set by?

Thanks Adam but actually what I meant is:  Where to set the max. limit in the code? That is, my hardware can support y- move between +50 to -50 revolutions. Where, in the code, did you set this 50 limits?

Because if I try to run your y-axis by +40 revolutions, the y-axis does not move at all. So, I think you set a limit in the code, like: if (y- revolution >= 30) {stop y- plan }; else {...}

Adam,

I just cannot find this part of code in ST_runPosMove. Is it in the proj_x-axis.c file? 

I'm attaching the ST_runPosMove in given proj_x-axis.c here:

void ST_runPosMove(ST_Handle stHandle)
{
	ST_Obj *stObj = (ST_Obj *)stHandle;

	// Run SpinTAC Position Profile Generator
	// If we are not running a profile, and command indicates we should has been modified
	if((STPOSMOVE_getStatus(stObj->posMoveHandle) == ST_MOVE_IDLE) && (gMotorVars.RunPositionProfile == true)) {
		// Get the configuration for SpinTAC Position Move
		STPOSMOVE_setCurveType(stObj->posMoveHandle, gMotorVars.SpinTAC.PosMoveCurveType);
		STPOSMOVE_setPositionStep_mrev(stObj->posMoveHandle, gMotorVars.PosStepInt_MRev,  gMotorVars.PosStepFrac_MRev);
		STPOSMOVE_setVelocityLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxVel_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setAccelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setDecelerationLimit(stObj->posMoveHandle, _IQmpy(gMotorVars.MaxDecel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
		STPOSMOVE_setJerkLimit(stObj->posMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
		// Enable the SpinTAC Position Profile Generator
		STPOSMOVE_setEnable(stObj->posMoveHandle, true);
		// clear the position step command
		gMotorVars.PosStepInt_MRev = 0;
		gMotorVars.PosStepFrac_MRev = 0;
		gMotorVars.RunPositionProfile = false;
	}

	STPOSMOVE_run(stObj->posMoveHandle);
}

Probably, sorry and that should be a easy fix... will look into it.

Besides, another question here: I'm trying to have more shapes(or letters) to plot. I found that the code can only support up to 6 shapes state machine. So, I'm wondering where, in the code, is setting this limitation?

typedef enum
{
	SQUARE=0,    //  1st one, Square Pattern
	TRIANGLE,    // 2nd one, Triangle Pattern
	SM_CIRCLE,   // 3rd one, Small Circle Pattern
	LG_CIRCLE,    // 4th one, Large Circle Pattern
	MAX_SHAPE, // 5th one
	Shape1,    //  6th one, works fine
	Shape2     // 7th one, doesn't work at all
} PLAN_SELECT_e;

Here's the scnshot of the error reported. I think it means that the code is exceeding the physical memory capacity. What you think??

If so, what's the best move to solve this then?

Just wanna make sure I'm doing the right thing:

Temporarily, I choose to modified the f28069M_RAM_Ink.cmd file as: Because of line 112, I increase the "length" in line 81 from 0x008000 to 0x009000. 

Is this correct? Anything I missed or did wrong? (The code "building" went thru. w/o. previous error anymore; but haven't get a chance to try it on my hardware yet...)

/*
// TI File $Revision: /main/3 $
// Checkin $Date: March 3, 2011   13:45:43 $
//###########################################################################
//
// FILE:    28069_RAM_lnk.cmd
//
// TITLE:   Linker Command File For F28069 examples that run out of RAM
//
//          This ONLY includes all SARAM blocks on the F28069 device.
//          This does not include flash or OTP.
//
//          Keep in mind that L0,L1,L2,L3 and L4 are protected by the code
//          security module.
//
//          What this means is in most cases you will want to move to
//          another memory map file which has more memory defined.
//
//###########################################################################
// $TI Release: 2806x C/C++ Header Files V1.10 $ 
// $Release Date: April 7, 2011 $ 
//###########################################################################
*/

/* ======================================================
// For Code Composer Studio V2.2 and later
// ---------------------------------------
// In addition to this memory linker command file,
// add the header linker command file directly to the project.
// The header linker command file is required to link the
// peripheral structures to the proper locations within
// the memory map.
//
// The header linker files are found in <base>\F2806x_headers\cmd
//
// For BIOS applications add:      F2806x_Headers_BIOS.cmd
// For nonBIOS applications add:   F2806x_Headers_nonBIOS.cmd
========================================================= */

/* ======================================================
// For Code Composer Studio prior to V2.2
// --------------------------------------
// 1) Use one of the following -l statements to include the
// header linker command file in the project. The header linker
// file is required to link the peripheral structures to the proper
// locations within the memory map                                    */

/* Uncomment this line to include file only for non-BIOS applications */
/* -l F2806x_Headers_nonBIOS.cmd */

/* Uncomment this line to include file only for BIOS applications */
/* -l F2806x_Headers_BIOS.cmd */

/* 2) In your project add the path to <base>\F2806x_headers\cmd to the
   library search path under project->build options, linker tab,
   library search path (-i).
/*========================================================= */

/* Define the memory block start/length for the F2806x
   PAGE 0 will be used to organize program sections
   PAGE 1 will be used to organize data sections

   Notes:
         Memory blocks on F28069 are uniform (ie same
         physical memory) in both PAGE 0 and PAGE 1.
         That is the same memory region should not be
         defined for both PAGE 0 and PAGE 1.
         Doing so will result in corruption of program
         and/or data.

         Contiguous SARAM memory blocks can be combined
         if required to create a larger memory block.
*/

MEMORY
{
PAGE 0 :

   BEGIN       : origin = 0x000000, length = 0x000002    /* BEGIN is used for the "boot to SARAM" bootloader mode   */
   RAMM0       : origin = 0x000050, length = 0x0003B0
   RAML0_L6    : origin = 0x008000, length = 0x009000	 /* on-chip RAM block RAML0-L6 */

   RESET       : origin = 0x3FFFC0, length = 0x000002
   FPUTABLES   : origin = 0x3FD590, length = 0x0006A0	 /* FPU Tables in Boot ROM */
   IQTABLES    : origin = 0x3FDC30, length = 0x000B50    /* IQ Math Tables in Boot ROM */
   IQTABLES2   : origin = 0x3FE780, length = 0x00008C    /* IQ Math Tables in Boot ROM */
   IQTABLES3   : origin = 0x3FE80C, length = 0x0000AA	 /* IQ Math Tables in Boot ROM */

   BOOTROM    : origin = 0x3FF3B0, length = 0x000C10


PAGE 1 :

   BOOT_RSVD   : origin = 0x000002, length = 0x00004E    /* Part of M0, BOOT rom will use this for stack */
   RAMM1       : origin = 0x000400, length = 0x000400    /* on-chip RAM block M1 */
   USB_RAM     : origin = 0x040000, length = 0x000800    /* USB RAM */
}


SECTIONS
{
   /* Setup for "boot to SARAM" mode:
      The codestart section (found in DSP28_CodeStartBranch.asm)
      re-directs execution to the start of user code.  */
   codestart        : > BEGIN,      PAGE = 0
   ramfuncs         : > RAMM0,
                        LOAD_START(_RamfuncsLoadStart),
                        LOAD_END(_RamfuncsLoadEnd),
                        RUN_START(_RamfuncsRunStart),
                        LOAD_SIZE(_RamfuncsLoadSize),
                                    PAGE = 0
   .text            : > RAML0_L6,   PAGE = 0
   .cinit           : > RAMM0,      PAGE = 0
   .pinit           : > RAMM0,      PAGE = 0
   .switch          : > RAMM0,      PAGE = 0
   .reset           : > RESET,      PAGE = 0, TYPE = DSECT /* not used, */

   .stack           : > RAMM1,      PAGE = 1
   .ebss            : > RAML0_L6,   PAGE = 0
   .econst          : > RAML0_L6,   PAGE = 0
   .esysmem         : > RAML0_L6,   PAGE = 0

   IQmath           : > RAML0_L6,   PAGE = 0
   IQmathTables     : > IQTABLES,   PAGE = 0, TYPE = NOLOAD
   
   /* Allocate FPU math areas: */
   FPUmathTables    : > FPUTABLES,  PAGE = 0, TYPE = NOLOAD
   
   /* DMARAML4_6	        : > RAML4_6,      PAGE = 1  /* */


  /* Uncomment the section below if calling the IQNexp() or IQexp()
      functions from the IQMath.lib library in order to utilize the
      relevant IQ Math table in Boot ROM (This saves space and Boot ROM
      is 1 wait-state). If this section is not uncommented, IQmathTables2
      will be loaded into other memory (SARAM, Flash, etc.) and will take
      up space, but 0 wait-state is possible.
   */
   /*
   IQmathTables2    : > IQTABLES2, PAGE = 0, TYPE = NOLOAD
   {

              IQmath.lib<IQNexpTable.obj> (IQmathTablesRam)

   }
   */
   /* Uncomment the section below if calling the IQNasin() or IQasin()
      functions from the IQMath.lib library in order to utilize the
      relevant IQ Math table in Boot ROM (This saves space and Boot ROM
      is 1 wait-state). If this section is not uncommented, IQmathTables2
      will be loaded into other memory (SARAM, Flash, etc.) and will take
      up space, but 0 wait-state is possible.
   */
   /*
   IQmathTables3    : > IQTABLES3, PAGE = 0, TYPE = NOLOAD
   {

              IQmath.lib<IQNasinTable.obj> (IQmathTablesRam)

   }
   */

}

/*
//===========================================================================
// End of file.
//===========================================================================
*/

Adam,

My router works now and I'm attaching a video here to share... Really appreciate your patient help!

And I do have two more questions:

1. According to the memory map, I calculate the max. capacity (length) bet. RAM L0 - L6 as: 0x010000 - 0x008000 = 0x008000. So, how to get your 0xB800 this value??

2. What is the very last argument of STPOSPLAN_addCfgState - min. time stay in the state for?? It seems to me it's almost not contributing to anything since we already set the # of revolution and speed.

I'm glad to hear that everything is now working.  I really liked the video.

Yu Zou said:

1. According to the memory map, I calculate the max. capacity (length) bet. RAM L0 - L6 as: 0x010000 - 0x008000 = 0x008000. So, how to get your 0xB800 this value??

The max capacity of RAM L0-L6 would be 0x008000, but the continuous RAM block on the 28069M device allows you to use L7 & most of L8 (part of L8 is reserved for InstaSPIN variables).  So by expanding L0-L6 into L0-L8 you can increase the length to 0x00B800

Yu Zou said:

2. What is the very last argument of STPOSPLAN_addCfgState - min. time stay in the state for?? It seems to me it's almost not contributing to anything since we already set the # of revolution and speed.

You are right for the plans that you are running, but there could be other situations where someone wants to hold at a set speed for a specific amount of time.  A good example of this is the high speed spin in a washing machine.

Thank you, Adam!

Q1 makes perfect sense now to me. So, to have length of 0xB800, I should modify the code as: (the updated part is in bold).

... ...
RAML0_L8    : origin = 0x008000, length = 0x0B8000	 /* on-chip RAM block RAML0-L8 */
... ...

   .text            : > RAML0_L8,   PAGE = 0

... ...

Is it right??



As to Q2, in your washing machine case, what to do on # of revolution argument then? Should we calculate the # of revolution? since we already have time length and speed.

For the washing machine I was speaking more about a speed control application's use of the min state time.  For a position control example, maybe something needs to hold a fixed position for a fixed amount of time, like in an assembly-line type situation.