Exemple #1
0
void CAL_setParams(CAL_Handle handle,const USER_Params *pUserParams)
{
  CAL_Obj *obj = (CAL_Obj *)handle;
  _iq beta_lp_pu = _IQ(pUserParams->offsetPole_rps/(float_t)pUserParams->ctrlFreq_Hz);
  uint_least8_t cnt;


  for(cnt=0;cnt<USER_NUM_CURRENT_SENSORS;cnt++)
    {
      OFFSET_setBeta(obj->offsetHandle_I[cnt],beta_lp_pu);
      OFFSET_setInitCond(obj->offsetHandle_I[cnt],_IQ(0.0));
      OFFSET_setOffset(obj->offsetHandle_I[cnt],_IQ(0.0));
    }


  for(cnt=0;cnt<USER_NUM_VOLTAGE_SENSORS;cnt++)
    {
      OFFSET_setBeta(obj->offsetHandle_V[cnt],beta_lp_pu);
      OFFSET_setInitCond(obj->offsetHandle_V[cnt],_IQ(0.0));
      OFFSET_setOffset(obj->offsetHandle_V[cnt],_IQ(0.0));
    }


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

  CAL_setCount_state(handle,0);

  CAL_setFlag_enable(handle,false);
  CAL_setFlag_enableAdcOffset(handle,true);

  CAL_setState(handle,CAL_State_Idle);

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


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


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


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


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


  if(FW_getFlag_enableFw(fwHandle) == true)
    {
      FW_incCounter(fwHandle);

      if(FW_getCounter(fwHandle) > FW_getNumIsrTicksPerFwTick(fwHandle))
        {
    	  _iq refValue;
    	  _iq fbackValue;
    	  _iq output;

    	  FW_clearCounter(fwHandle);

    	  refValue = gMotorVars.VsRef;

    	  fbackValue = gMotorVars.Vs;

    	  FW_run(fwHandle, refValue, fbackValue, &output);

    	  CTRL_setId_ref_pu(ctrlHandle, output);

    	  gMotorVars.IdRef_A = _IQmpy(CTRL_getId_ref_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
        }
    }
  else
    {
      CTRL_setId_ref_pu(ctrlHandle, _IQmpy(gMotorVars.IdRef_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
    }

  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function
void ST_setupVelPlan(ST_Handle handle) {

  _iq accMax, jrkMax;
  ST_Obj *stObj = (ST_Obj *)handle;

  // Pass the configuration array pointer into SpinTAC Velocity Plan
  STVELPLAN_setCfgArray(stObj->velPlanHandle, &stVelPlanCfgArray[0], sizeof(stVelPlanCfgArray), 0, 0, 0, NUM_PLAN_TRANS, NUM_PLAN_STATES);

  // Establish the Acceleration, and Jerk Maximums
  accMax = _IQ24(10.0);
  jrkMax = _IQ20(62.5);

  // Configure SpinTAC Velocity Plan: Sample Time, LoopENB
  STVELPLAN_setCfg(stObj->velPlanHandle, _IQ(ST_SAMPLE_TIME), false);
  // Configure halt state: VelEnd, AccMax, JrkMax, Timer
  STVELPLAN_setCfgHaltState(stObj->velPlanHandle, 0, accMax, jrkMax, 1000L);

  //Example: STVELPLAN_addCfgState(handle,    VelSetpoint[pups],                 StateTimer[ticks]);
  STVELPLAN_addCfgState(stObj->velPlanHandle, 0,                                 200L); // StateA
  STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(0.25 * ST_SPEED_PU_PER_KRPM),  2000L); // StateB
  STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(-0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateC

  //Example: STVELPLAN_addCfgTran(handle,    FromState, ToState, CondOption, CondIdx1, CondiIdx2, AccLim[pups2], JrkLim[pups3]);
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_A,   STATE_B, ST_COND_NC, 0,        0,         _IQ(0.1),      _IQ20(1));	// From StateA to StateB
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_B,   STATE_C, ST_COND_NC, 0,        0,         _IQ(0.1),      _IQ20(1));	// From StateB to StateC
  STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_C,   STATE_A, ST_COND_NC, 0,        0,         _IQ(1),        _IQ20(1));	// From StateC to StateA
  // Note: set CondIdx1 to 0 if CondOption is ST_COND_NC; set CondIdx2 to 0 if CondOption is ST_COND_NC  or ST_COND_FC
}
void ST_runVelMove(ST_Handle handle, CTRL_Handle ctrlHandle)
{

    ST_Obj *stObj = (ST_Obj *)handle;
    CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;

	// Run SpinTAC Move
	// If we are not in reset, and the SpeedRef_krpm has been modified
	if((EST_getState(ctrlObj->estHandle) == EST_State_OnLine) && (_IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STVELMOVE_getVelocityEnd(stObj->velMoveHandle))) {
		// Get the configuration for SpinTAC Move
		STVELMOVE_setCurveType(stObj->velMoveHandle, gMotorVars.SpinTAC.VelMoveCurveType);
		STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
		STVELMOVE_setAccelerationLimit(stObj->velMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
		STVELMOVE_setJerkLimit(stObj->velMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
		// Enable SpinTAC Move
		STVELMOVE_setEnable(stObj->velMoveHandle, true);
		// If starting from zero speed, enable ForceAngle, otherwise disable ForceAngle
		if(_IQabs(STVELMOVE_getVelocityStart(stObj->velMoveHandle)) < _IQ(ST_MIN_ID_SPEED_PU)) {
			EST_setFlag_enableForceAngle(ctrlObj->estHandle, true);
			gMotorVars.Flag_enableForceAngle = true;
		}
		else {
			EST_setFlag_enableForceAngle(ctrlObj->estHandle, false);
			gMotorVars.Flag_enableForceAngle = false;
		}
	}
	STVELMOVE_run(stObj->velMoveHandle);
}
void recalcKpKi(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  EST_State_e EstState = EST_getState(obj->estHandle);

  if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
    {
      float_t Lhf = CTRL_getLhf(handle);
      float_t Rhf = CTRL_getRhf(handle);
      float_t RhfoverLhf = Rhf/Lhf;
      _iq Kp = _IQ(0.25*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
      _iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);

      // set Rhf/Lhf
      CTRL_setRoverL(handle,RhfoverLhf);

      // set the controller proportional gains
      CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
      CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);

      // set the Id controller gains
      CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
      PID_setKi(obj->pidHandle_Id,Ki);

      // set the Iq controller gains
      CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
      PID_setKi(obj->pidHandle_Iq,Ki);
    }

  return;
} // end of recalcKpKi() function
Exemple #6
0
void runRsOnLine(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  // execute Rs OnLine code
  if(gMotorVars.Flag_Run_Identify == true)
    {
      if(EST_getState(obj->estHandle) == EST_State_OnLine)
        {
    	  float_t RsError_Ohm = gMotorVars.RsOnLine_Ohm - gMotorVars.Rs_Ohm;

          EST_setFlag_enableRsOnLine(obj->estHandle,true);
          EST_setRsOnLineId_mag_pu(obj->estHandle,_IQmpy(gMotorVars.RsOnLineCurrent_A,_IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));

          if(abs(RsError_Ohm) < (gMotorVars.Rs_Ohm * 0.05))
            {
              EST_setFlag_updateRs(obj->estHandle,true);
            }
        }
      else
        {
          EST_setRsOnLineId_mag_pu(obj->estHandle,_IQ(0.0));
          EST_setRsOnLineId_pu(obj->estHandle,_IQ(0.0));
          EST_setRsOnLine_pu(obj->estHandle,_IQ(0.0));
          EST_setFlag_enableRsOnLine(obj->estHandle,false);
          EST_setFlag_updateRs(obj->estHandle,false);
          EST_setRsOnLine_qFmt(obj->estHandle,EST_getRs_qFmt(obj->estHandle));
        }
    }

  return;
} // end of runRsOnLine() function
void USER_calcPIgains(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A;
  float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V;
  float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
  float_t Ls_d;
  float_t Ls_q;
  float_t Rs;
  float_t RoverLs_d;
  float_t RoverLs_q;
  _iq Kp_Id;
  _iq Ki_Id;
  _iq Kp_Iq;
  _iq Ki_Iq;
  _iq Kd;

#ifdef __TMS320C28XX_FPU32__
  int32_t tmp;

  // when calling EST_ functions that return a float, and fpu32 is enabled, an integer is needed as a return
  // so that the compiler reads the returned value from the accumulator instead of fpu32 registers
  tmp = EST_getLs_d_H(obj->estHandle);
  Ls_d = *((float_t *)&tmp);

  tmp = EST_getLs_q_H(obj->estHandle);
  Ls_q = *((float_t *)&tmp);

  tmp = EST_getRs_Ohm(obj->estHandle);
  Rs = *((float_t *)&tmp);
#else
  Ls_d = EST_getLs_d_H(obj->estHandle);

  Ls_q = EST_getLs_q_H(obj->estHandle);

  Rs = EST_getRs_Ohm(obj->estHandle);
#endif

  RoverLs_d = Rs/Ls_d;
  Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);

  RoverLs_q = Rs/Ls_q;
  Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
  Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);

  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
Exemple #8
0
/******************************************************************************
* Записывает параметры по умолчанию
*/
void prmInitDef(void){
    uint8_t i;
    
    ps.error.bit.noCalibration = 1;
    
    sysset.kKorHighvoltage  = 1125;
    sysset.kLval            = 10000;
    sysset.hiTemp           = 50;
    sysset.lowTemp          = 30;
    sysset.freqBeep         = 4000;
    sysset.lcdContrast      = 100;
    sysset.spsUse   = 0;            //Используется sps
    sysset.stopsFan = 1;            //Не будет останавливатся куллер
    
    /*******************************
    * U
    */
    sysset.pU[0].qu     = _IQ(0.039);
    sysset.pU[0].adc    = 60;
    sysset.pU[0].dac    = 31;

    sysset.pU[1].qu     = _IQ(0.999);
    sysset.pU[1].adc    = 384;
    sysset.pU[1].dac    = 193;
    
    /*******************************
    * I
    */
    sysset.pI[0].qi     = _IQ(0.002);
    sysset.pI[0].adc    = 50;
    sysset.pI[0].dac    = 25;

    sysset.pI[1].qi     = _IQ(0.061);
    sysset.pI[1].adc    = 110;
    sysset.pI[1].dac    = 55;

    /**************************************
    * BS
    */
    for(i = 0; i < NPRESET; i++){
        bs.set[i].i    = I_BIG_STEP;
        bs.set[i].u    = U_BIG_STEP;
        bs.set[i].Mode = baseImax;
    }

    /**************************************
    * CH
    */  
    ch.Idac = I_BIG_STEP;
    ch.Mode = 0;
    ch.Time = 60;
    ch.Udac = U_BIG_STEP;
}
Exemple #9
0
/*
 * Function 	: vInitDataForTask6
 * Arguments 	: None
 * Returns 		: None
 * Description 	: Initializes data points that will be used in Task 6 of the CLA
 */
void vInitDataForTask6()
{
	sCPUtoCLAMsg.x1.iq24 = _IQ(1.2346);		sCPUtoCLAMsg.x2.iq24 = _IQ(0.1233);
	sCPUtoCLAMsg.x3.iq24 = _IQ(-3.1244);	sCPUtoCLAMsg.x4.iq24 = _IQ(0.0199);
	sCPUtoCLAMsg.x5.iq24 = _IQ(3.2162);		sCPUtoCLAMsg.x6.iq24 = _IQ(3.2163);
	sCPUtoCLAMsg.x7.iq24 = _IQ(0.0000);		sCPUtoCLAMsg.x8.iq24 = _IQ(-1.5000);
}
Exemple #10
0
extern void ANGLE_COMP_setParams(ANGLE_COMP_Handle handle,
                            float_t iqFullScaleFreq_Hz,
                            float_t pwmPeriod_usec,
                            uint_least16_t numPwmTicksPerIsrTick)
{
  ANGLE_COMP_Obj *obj = (ANGLE_COMP_Obj *)handle;
  float_t pwmPeriod_s = 1.0e-6 * pwmPeriod_usec;
  
  
  obj->angleDeltaFactor = _IQ(iqFullScaleFreq_Hz * pwmPeriod_s);
  obj->angleCompFactor = _IQ(1.0 + (float_t)numPwmTicksPerIsrTick * 0.5);

  return;
} // end of ANGLE_COMP_setParams() function
void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{

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

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

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

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

	if(gMotorVars.SpinTAC.VelCtlEnb == true) {
		// Set the Iq reference that came out of SpinTAC Velocity Control
		CTRL_setIq_ref_pu(ctrlHandle, iqReference);
		// Update internal PI integrator, if running SpinTAC
		gSpdError = TRAJ_getIntValue(ctrlObj->trajHandle_spd) - speedFeedback;
		gUp = _IQmpy(gSpdError, CTRL_getKp(ctrlHandle, CTRL_Type_PID_spd));
		gUi = _IQmpy(gSpdError, CTRL_getKi(ctrlHandle, CTRL_Type_PID_spd));
		CTRL_setUi(ctrlHandle, CTRL_Type_PID_spd, iqReference - gUp - gUi);
	}
}
Exemple #12
0
//! \brief     Sets up the throttle module parameters initially
//! \param[in] handle  The throttle handle
extern void THROTTLE_setParams(THROTTLE_Handle handle,  \
                                const bool invert,
                                const _iq max_adc,      \
                                const _iq min_adc,      \
                                const _iq max_out,      \
                                const _iq min_out)
{
  THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;

  obj->flagSw1 = false;
  obj->flagSw2 = false;
  obj->max_adc = max_adc;
  obj->min_adc = min_adc;

  obj->max_out = max_out;
  obj->min_out = min_out;

  obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
  obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);

  obj->state = THROTTLE_Run;
  obj->value = _IQ(0.0);

  return;
}
//! \brief     Computes the scale factor needed to convert from torque created by Ld, Lq, Id and Iq, from per unit to Nm
//!
_iq USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf(void)
{
  float_t FullScaleInductance = (USER_IQ_FULL_SCALE_VOLTAGE_V/(USER_IQ_FULL_SCALE_CURRENT_A*USER_VOLTAGE_FILTER_POLE_rps));
  float_t FullScaleCurrent = (USER_IQ_FULL_SCALE_CURRENT_A);
  float_t lShift = ceil(log(USER_MOTOR_Ls_d/(0.7*FullScaleInductance))/log(2.0));

  return(_IQ(FullScaleInductance*FullScaleCurrent*FullScaleCurrent*USER_MOTOR_NUM_POLE_PAIRS*1.5*pow(2.0,lShift)));
} // end of USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf() function
//! \brief     Computes the scale factor needed to convert from per unit to V/Hz
//!
_iq USER_computeFlux_pu_to_VpHz_sf(void)
{
  float_t FullScaleFlux = (USER_IQ_FULL_SCALE_VOLTAGE_V/(float_t)USER_EST_FREQ_Hz);
  float_t maxFlux = (USER_MOTOR_RATED_FLUX*((USER_MOTOR_TYPE==MOTOR_Type_Induction)?0.05:0.7));
  float_t lShift = -ceil(log(FullScaleFlux/maxFlux)/log(2.0));

  return(_IQ(FullScaleFlux*pow(2.0,lShift)));
} // end of USER_computeFlux_pu_to_VpHz_sf() function
//! \brief     Computes the scale factor needed to convert from torque created by flux and Iq, from per unit to Nm
//!
_iq USER_computeTorque_Flux_Iq_pu_to_Nm_sf(void)
{
  float_t FullScaleFlux = (USER_IQ_FULL_SCALE_VOLTAGE_V/(float_t)USER_EST_FREQ_Hz);
  float_t FullScaleCurrent = (USER_IQ_FULL_SCALE_CURRENT_A);
  float_t maxFlux = (USER_MOTOR_RATED_FLUX*((USER_MOTOR_TYPE==MOTOR_Type_Induction)?0.05:0.7));
  float_t lShift = -ceil(log(FullScaleFlux/maxFlux)/log(2.0));

  return(_IQ(FullScaleFlux/(2.0*MATH_PI)*FullScaleCurrent*USER_MOTOR_NUM_POLE_PAIRS*1.5*pow(2.0,lShift)));
} // end of USER_computeTorque_Flux_Iq_pu_to_Nm_sf() function
Exemple #16
0
PID_Handle PID_init(void *pMemory,const size_t numBytes)
{
  PID_Handle handle;


  if(numBytes < sizeof(PID_Obj))
    return((PID_Handle)NULL);

  // assign the handle
  handle = (PID_Handle)pMemory;

  // set some defaults
  PID_setUi(handle,_IQ(0.0));
  PID_setRefValue(handle,_IQ(0.0));
  PID_setFbackValue(handle,_IQ(0.0));

  return(handle);
} // end of PID_init() function
Exemple #17
0
__interrupt void ecapISR(void)
{
    // Clear capture (CAP) interrupt flags
    CAP_clearInt(halHandle->capHandle, CAP_Int_Type_All);

    // Compute the PWM high period (rising edge to falling edge)
    uint32_t PwmDuty = (uint32_t) CAP_getCap2(halHandle->capHandle) - (uint32_t) CAP_getCap1(halHandle->capHandle);

    // Assign the appropriate speed command, combine 0-5% and 95-100%
    // 0-100% speed is proportional to 1-2ms high period
    // 60MHz * 2ms = 120000

    // 0-1%
    if (PwmDuty <= 61000)
    {
        gSpeedRef_Ok = 0;
        gSpeedRef_duty = _IQ(0);
        gMotorVars.Flag_Run_Identify = 0;
    }
    // 1-99%
    if ((PwmDuty > 61000) && (PwmDuty < 119000))
    {
        gSpeedRef_Ok = 0;
        gSpeedRef_duty = _IQdiv(PwmDuty - 60000, 60000);
        gMotorVars.Flag_Run_Identify = 1;
    }
    // 99-100%
    else if (PwmDuty >= 119000)
    {
        gSpeedRef_Ok = 0;
        gSpeedRef_duty = _IQ(1.0);
        gMotorVars.Flag_Run_Identify = 1;
    }
    // Catch all
    else
    {
        gSpeedRef_duty = _IQ(0);
        gMotorVars.Flag_Run_Identify = 0;
    }

    // Clears an interrupt defined by group number
    PIE_clearInt(halHandle->pieHandle, PIE_GroupNumber_4);
}  // end of ecapISR() function
void updateGlobalVariables_motor(CTRL_Handle handle)
{
	CTRL_Obj *obj = (CTRL_Obj *) handle;


#ifdef __TMS320C28XX_FPU32__
  int32_t tmp;
#endif



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

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

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

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

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

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

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

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

	// get the flux in V/Hz in floating point
   fpu_assign(gMotorVars.Flux_VpHz , EST_getFlux_VpHz( obj->estHandle ));



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

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

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

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

	return;
} // end of updateGlobalVariables_motor() function
void updateGlobalVariables_motor(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  int32_t tmp;

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

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

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

  // when calling EST_ functions that return a float, and fpu32 is enabled, an integer is needed as a return
  // so that the compiler reads the returned value from the accumulator instead of fpu32 registers
  // get the magnetizing current
  tmp = EST_getIdRated(obj->estHandle);
  gMotorVars.MagnCurr_A = *((float_t *)&tmp);

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

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

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

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

  // get the flux in V/Hz in floating point
  tmp = EST_getFlux_VpHz(obj->estHandle);
  gMotorVars.Flux_VpHz = *((float_t *)&tmp);

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

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

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

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

  return;
} // end of updateGlobalVariables_motor() function
void OFFSET_setBeta(OFFSET_Handle handle,const _iq beta)
{
  OFFSET_Obj *obj = (OFFSET_Obj *)handle;
  _iq a1 = (beta - _IQ(1.0));
  _iq b0 = beta;
  _iq b1 = 0;

  FILTER_FO_setDenCoeffs(obj->filterHandle,a1);
  FILTER_FO_setNumCoeffs(obj->filterHandle,b0,b1);

  return;
} // end of OFFSET_setBeta() function
Exemple #21
0
void setupClarke_I(CLARKE_Handle handle,const uint_least8_t numCurrentSensors)
{
  _iq alpha_sf,beta_sf;


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

  // set the parameters
  CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
  CLARKE_setNumSensors(handle,numCurrentSensors);

  return;
} // end of setupClarke_I() function
Exemple #22
0
void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  _iq alpha_sf,beta_sf;
  

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

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

  return;
} // end of CTRL_setupClarke_I() function
void recalcKpKiPmsm(CTRL_Handle handle)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;
  EST_State_e EstState = EST_getState(obj->estHandle);

  if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
    {
      float_t Lhf = CTRL_getLhf(handle);
      float_t Rhf = CTRL_getRhf(handle);
      float_t RhfoverLhf = Rhf/Lhf;
      _iq Kp = _IQ(0.05*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
      _iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);
      _iq Kp_spd = _IQ(0.005*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);

      // set Rhf/Lhf
      CTRL_setRoverL(handle,RhfoverLhf);

      // set the controller proportional gains
      CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
      CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);

      // set the Id controller gains
      CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
      PID_setKi(obj->pidHandle_Id,Ki);

      // set the Iq controller gains
      CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
      PID_setKi(obj->pidHandle_Iq,Ki);

      // set speed gains
      PID_setKp(obj->pidHandle_spd,Kp_spd);
    }
  else if(EstState == EST_State_RatedFlux)
    {
      _iq Ki_spd = _IQ(0.5*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);

      // Set Ki on closing the feedback loop
      PID_setKi(obj->pidHandle_spd,Ki_spd);
    }
  else if(EstState == EST_State_RampDown)
    {
      _iq Kp_spd = _IQ(0.02*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
      _iq Ki_spd = _IQ(2.0*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);

      // Set Kp and Ki of the speed controller
      PID_setKp(obj->pidHandle_spd,Kp_spd);
      PID_setKi(obj->pidHandle_spd,Ki_spd);

      TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(USER_MOTOR_RES_EST_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A));
    }

  return;
} // end of recalcKpKiPmsm() function
//! \brief     Computes Torque in Nm
//!
_iq USER_computeTorque_lbin(CTRL_Handle handle, const _iq torque_Flux_sf, const _iq torque_Ls_sf)
{
  CTRL_Obj *obj = (CTRL_Obj *)handle;

  _iq Flux_pu = EST_getFlux_pu(obj->estHandle);
  _iq Id_pu = PID_getFbackValue(obj->pidHandle_Id);
  _iq Iq_pu = PID_getFbackValue(obj->pidHandle_Iq);
  _iq Ld_minus_Lq_pu = _IQ30toIQ(EST_getLs_d_pu(obj->estHandle)-EST_getLs_q_pu(obj->estHandle));
  _iq Torque_Flux_Iq_Nm = _IQmpy(_IQmpy(Flux_pu,Iq_pu),torque_Flux_sf);
  _iq Torque_Ls_Id_Iq_Nm = _IQmpy(_IQmpy(_IQmpy(Ld_minus_Lq_pu,Id_pu),Iq_pu),torque_Ls_sf);
  _iq Torque_Nm = Torque_Flux_Iq_Nm + Torque_Ls_Id_Iq_Nm;

  return(_IQmpy(Torque_Nm, _IQ(MATH_Nm_TO_lbin_SF)));
} // end of USER_computeTorque_lbin() function
extern void VS_FREQ_setParams(VS_FREQ_Handle handle,
        					float_t iqFullScaleFreq_Hz,
        					float_t iqFullScaleVoltage_V,
							float_t maxVsMag_pu)
{
  VS_FREQ_Obj *obj = (VS_FREQ_Obj *)handle;

  obj->iqFullScaleFreq_Hz = iqFullScaleFreq_Hz;
  obj->iqFullScaleVoltage_V = iqFullScaleVoltage_V;
  obj->maxVsMag_pu = maxVsMag_pu;
  obj->MaxFreq = _IQ(USER_MOTOR_FREQ_MAX/iqFullScaleFreq_Hz);

  return;
} // end of VS_FREQ_setParams() function
Exemple #26
0
extern void THROTTLE_runState(THROTTLE_Handle handle)
{
  THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;


  if(obj->flagSw1)
  {
    obj->result = _IQ(0.0);
    obj->state = THROTTLE_CalMaxMin;
    obj->max_adc = obj->min_out;
    obj->min_adc = obj->max_out;
  }
  else
  {
    switch (obj->state)
    {
    case THROTTLE_CalMaxMin:

      if (obj->value > obj->max_adc)
        obj->max_adc = obj->value;
      else if (obj->value < obj->min_adc)
        obj->min_adc = obj->value;
      else if (obj->flagSw2)
        obj->state = THROTTLE_CalCalc;

    break;

    case THROTTLE_CalCalc:
    {
      obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
      obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);
      
      obj->state = THROTTLE_Run;
    }
    break;

    case THROTTLE_Run:
    {
      _iq result = _IQmpy(obj->value,obj->slope) + obj->offset;
      obj->result = _IQsat(result,obj->max_out,obj->min_out);
    }
    break;
    }
  }


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

  // Check if speed reference signal is active
  // If more than 2000 service routine cycles pass without signal, disable motor
  if (gSpeedRef_Ok++ > 2000)
  {
      gSpeedRef_duty = _IQ(0);
      gMotorVars.Flag_Run_Identify = 0;
      gSpeedRef_Ok = 0;
  }


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


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


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


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


  // setup the controller
  CTRL_setup(ctrlHandle);


  return;
} // end of mainISR() function
void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{

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

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

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

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

	// Set the Iq reference that came out of SpinTAC Velocity Control
	CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
Exemple #29
0
void smopos_calc(SMOPOS *v)
{	
    _iq E0;
    
    E0 = _IQ(0.5);
    
// Sliding mode current observer
    v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha-v->Ealpha-v->Zalpha));
    v->EstIbeta = _IQmpy(v->Fsmopos,v->EstIbeta) + _IQmpy(v->Gsmopos,(v->Vbeta-v->Ebeta-v->Zbeta));

// Current errors
    v->IalphaError = v->EstIalpha - v->Ialpha;
    v->IbetaError = v->EstIbeta - v->Ibeta;

// Sliding control calculator
    if (_IQabs(v->IalphaError) < E0)
       v->Zalpha = _IQmpy(v->Kslide,_IQdiv(v->IalphaError,E0));  
    else if (v->IalphaError >= E0) 
       v->Zalpha = v->Kslide;
    else if (v->IalphaError <= -E0) 
       v->Zalpha = -v->Kslide;

    if (_IQabs(v->IbetaError) < E0)
       v->Zbeta = _IQmpy(v->Kslide,_IQdiv(v->IbetaError,E0));  
    else if (v->IbetaError >= E0) 
       v->Zbeta = v->Kslide;
    else if (v->IbetaError <= -E0) 
       v->Zbeta = -v->Kslide;

// Sliding control filter -> back EMF calculator
    v->Ealpha = v->Ealpha + _IQmpy(v->Kslf,(v->Zalpha-v->Ealpha));
    v->Ebeta = v->Ebeta + _IQmpy(v->Kslf,(v->Zbeta-v->Ebeta));

// Rotor angle calculator -> Theta = atan(-Ealpha,Ebeta)
    v->Theta = _IQatan2PU(-v->Ealpha,v->Ebeta); 
}
Exemple #30
-1
void speed_frq_calc(SPEED_MEAS_QEP *v)
{

   _iq tmp1;

/* Differentiator  */
/*
   if (v->dir_QEP==0)      // T2 is counting down
   {
    if (v->theta_elec>v->theta_old)
      tmp1 = _IQ(1) - v->theta_elec + v->theta_old;
    else
      tmp1 = v->theta_elec - v->theta_old;
   }
   else if (v->dir_QEP==1)      // T2 is counting up
   {
    if (v->theta_elec<v->theta_old)
      tmp1 = _IQ(1) + v->theta_elec - v->theta_old;
    else 
      tmp1 = v->theta_elec - v->theta_old;
   }
   tmp1 = _IQmpy(v->K1,tmp1);     // Q21 = Q21*GLOBAL_Q
*/


// Differentiator  
  // Synchronous speed computation   
   if ((v->theta_elec < _IQ(0.9))&(v->theta_elec > _IQ(0.1)))
		// Q21 = Q21*(GLOBAL_Q-GLOBAL_Q)
   		tmp1 = _IQmpy(v->K1,(v->theta_elec - v->theta_old));
   else tmp1 = _IQtoIQ21(v->speed_frq);


// Low-pass filter 
   // Q21 = GLOBAL_Q*Q21 + GLOBAL_Q*Q21
   tmp1 = _IQmpy(v->K2,_IQtoIQ21(v->speed_frq))+_IQmpy(v->K3,tmp1);  

   if (tmp1>_IQ21(1))
     v->speed_frq = _IQ(1);
   else if (tmp1<_IQ21(-1))
     v->speed_frq = _IQ(-1);      
   else
     v->speed_frq = _IQ21toIQ(tmp1);

/* Update the electrical angle */
    v->theta_old = v->theta_elec;

/* Change motor speed from pu value to rpm value (GLOBAL_Q -> Q0) */
/* Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q */
   v->speed_rpm = _IQmpy(v->rpm_max,v->speed_frq); 

}