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); }
//---------------------------------------------------------------------------- // Main code: //---------------------------------------------------------------------------- int main(void) { unsigned int i; _iq tempX, tempY, tempP, tempM, tempMmax; // char buffer[20]; int *WatchdogWDCR = (void *) 0x7029; // Disable the watchdog: asm(" EALLOW "); *WatchdogWDCR = 0x0068; asm(" EDIS "); Step.Xsize = _IQ(STEP_X_SIZE); Step.Ysize = _IQ(STEP_Y_SIZE); Step.Yoffset = 0; Step.X = 0; Step.Y = Step.Yoffset; // Fill the buffers with some initial value for(i=0; i < DATA_LOG_SIZE; i++) { Dlog.Xwaveform[i] = _IQ(0.0); Dlog.Ywaveform[i] = _IQ(0.0); Dlog.Mag[i] = _IQ(0.0); Dlog.Phase[i] = _IQ(0.0); Dlog.Exp[i] = _IQ(0.0); } Step.GainX = _IQ(X_GAIN); Step.FreqX = _IQ(X_FREQ); Step.GainY = _IQ(Y_GAIN); Step.FreqY = _IQ(Y_FREQ); // Calculate maximum magnitude value: tempMmax = _IQmag(Step.GainX, Step.GainY); for(i=0; i < DATA_LOG_SIZE; i++) { // Calculate waveforms: Step.X = Step.X + _IQmpy(Step.Xsize, Step.FreqX); if( Step.X > _IQ(2*PI) ) Step.X -= _IQ(2*PI); Step.Y = Step.Y + _IQmpy(Step.Ysize, Step.FreqY); if( Step.Y > _IQ(2*PI) ) Step.Y -= _IQ(2*PI); Dlog.Xwaveform[i] = tempX = _IQmpy(_IQsin(Step.X), Step.GainX); Dlog.Ywaveform[i] = tempY = _IQmpy(_IQabs(_IQsin(Step.Y)), Step.GainY); // Calculate normalized magnitude: // // Mag = sqrt(X^2 + Y^2)/sqrt(GainX^2 + GainY^2); tempM = _IQmag(tempX, tempY); Dlog.Mag[i] = _IQdiv(tempM, tempMmax); // Calculate normalized phase: // // Phase = (long) (atan2PU(X,Y) * 360); tempP = _IQatan2PU(tempY,tempX); Dlog.Phase[i] = _IQmpyI32int(tempP, 360); // Use the exp function Dlog.Exp[i] = _IQexp(_IQmpy(_IQ(.075L),_IQ(i))); // Use the asin function Dlog.Asin[i] = _IQasin(Dlog.Xwaveform[i]); } }