/* ==============================================================================
System Name: Micro stepping bipolar drive for two-phase stepping motor

File Name:	ecs_microstep.c

Description:	Primary system file

Originator:		ECS Development

Target:  TMS320F2808

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 06-15-2006	Release	Rev 1.0 : Original release
=================================================================================  */

#include "ECS_MICROSTEP.h"
#include "ECS_MAIN.h"

extern float T;

void MICROSTEP_Init(MICROSTEP *v)
{

// ***************** LEVEL1 *****************
#if (BUILDLEVEL==LEVEL1)

//  Configure reference valuses
    v->Ref.Mode = 0;
    v->Ref.Dir = 0;
    v->Ref.Spd = _IQ(0.1);
    v->Ref.T = _IQ(0.5);
    v->Ref.Pos = _IQ(0.5);
    v->Ref.Id = _IQ(0.0);
    v->Ref.Iq = _IQ(0.1);
    v->Ref.Ipeak = _IQ(0.2);

//  Configure RAMP module
    v->Ramp.DelayRate = 1;

//  Configure ANGGEN module
    v->AngGen.StepAngleMax = _IQ(BASE_FREQ*T);

//  Configure position PI module
    v->PI_Pos.Kp = _IQ(1.0);
    v->PI_Pos.Ki = _IQ(T/0.1);
    v->PI_Pos.Kc = _IQ(0.5);
    v->PI_Pos.OutMax = _IQ(0.9995);
    v->PI_Pos.OutMin = _IQ(0);

//  Configure speed PI module
    v->PI_Spd.Kp = _IQ(1.0);
    v->PI_Spd.Ki = _IQ(T/0.1);
    v->PI_Spd.Kc = _IQ(0.5);
    v->PI_Spd.OutMax = _IQ(0.9995);
    v->PI_Spd.OutMin = _IQ(-0.9995);

//  Configure d-axis current PI module
    v->PI_Id.Kp = _IQ(1.0);
    v->PI_Id.Ki = _IQ(T/0.01);
    v->PI_Id.Kc = _IQ(0.5);
    v->PI_Id.OutMax = _IQ(0.6);
    v->PI_Id.OutMin = _IQ(-0.6);

//  Configure q-axis current PI module
    v->PI_Iq.Kp = _IQ(1.0);
    v->PI_Iq.Ki = _IQ(T/0.01);
    v->PI_Iq.Kc = _IQ(0.5);
    v->PI_Iq.OutMax = _IQ(0.6);
    v->PI_Iq.OutMin = _IQ(-0.6);

//  Configure peak current PI module
    v->PI_Ipeak.Kp = _IQ(1.0);
    v->PI_Ipeak.Ki = _IQ(T/0.003);
    v->PI_Ipeak.Kc = _IQ(0.5);
    v->PI_Ipeak.OutMax = _IQ(0.99);
    v->PI_Ipeak.OutMin = _IQ(0);


#endif // (BUILDLEVEL==LEVEL1)
}



void MICROSTEP_Update(MICROSTEP *v)
{  

// ***************** LEVEL1 *****************
#if (BUILDLEVEL==LEVEL1)

// Ramp for speed reference
   v->Ramp.TargetIn = v->Ref.Spd;
   v->Ramp.calc(&v->Ramp);

// Angle generation
   v->AngGen.Freq = v->Ramp.TargetOut;
   v->AngGen.calc(&v->AngGen);

// ZOH calculation
    v->Zoh_Ang.SignalIn = v->AngGen.AngleOut;
	v->Zoh_Ang.maxTimer = _IQ16div(_IQ16(ISR_FREQUENCY*1000/BASE_FREQ),_IQtoIQ16(v->Ramp.TargetOut));
	v->Zoh_Ang.calc(&v->Zoh_Ang);

// Low-pass filter
    v->Lpf_Ipeak.In = _IQsqrt(_IQmpy(v->Fdb.Ia,v->Fdb.Ia)+_IQmpy(v->Fdb.Ib,v->Fdb.Ib));
    v->Lpf_Ipeak.Out = _IQmpy(v->Lpf_Ipeak.Out,v->Lpf_Ipeak.Gain) + _IQmpy(v->Lpf_Ipeak.In,(_IQ(1.0)-v->Lpf_Ipeak.Gain));

// Peak current PI controller
    v->PI_Ipeak.Ref = v->Ref.Ipeak;
	v->PI_Ipeak.Fdb = v->Lpf_Ipeak.Out;
    v->PI_Ipeak.calc(&v->PI_Ipeak);

// Vcontrol computation
	v->Vref.SinDisc = _IQmpy(v->PI_Ipeak.Out,_IQsinPU(v->Zoh_Ang.SignalOut));
	v->Vref.CosDisc = _IQmpy(v->PI_Ipeak.Out,_IQcosPU(v->Zoh_Ang.SignalOut));

#endif // (BUILDLEVEL==LEVEL1)
}