📄 pcm_code.c
字号:
TIMER1_ENABLE ;
TIMER0_DISABLE ;
TIMER0_PERIOD ;
TIMER0_ENABLE;
/****************************************************/
for(j=1;j<100;j++);
VL1=*pADC1;
for(j=1;j<100;j++);
VL1=*pADC1;
for(j=1;j<100;j++);
VL1=*pADC1;
for(j=1;j<100;j++);
VL2=*pADC2;
for(j=1;j<100;j++);
VL2=*pADC2;
for(j=1;j<100;j++);
VL2=*pADC2;
}
/*******************************************************************************/
void Assign_out1(volatile float arg1, volatile float arg2, volatile float arg3, volatile float arg4
, volatile float arg5, volatile float arg6,volatile float out1[])
{
out1[1]=arg1;
out1[2]=arg2;
out1[3]=arg3;
out1[4]=arg4;
out1[5]=arg5;
out1[6]=arg6;
}
/*****************************************************************************/
void Assign_out2(volatile float arg1, volatile float arg2, volatile float arg3, volatile float arg4
, volatile float arg5, volatile float arg6,volatile float out2[])
{
out2[1]=arg1;
out2[2]=arg2;
out2[3]=arg3;
out2[4]=arg4;
out2[5]=arg5;
out2[6]=arg6;
}
/*****************************************************************************/
void Assign_angle(volatile float arg1, volatile float arg2, volatile float arg3, volatile float arg4
, volatile float arg5, volatile float arg6,volatile float arg7, volatile float arg8, volatile float arg9, volatile float arg10
, volatile float arg11 )
{
th2p=arg1;
th2m=arg2;
th2pss=arg3;
th2mss=arg4;
th2=arg5;
sig1=arg6;
sig1ss=arg7;
level=arg8;
step=arg9;
dang1=arg10;
dang2=arg11;
}
/****************************************************************************/
/*
% this function defines a new Case_flag as a function of the
% old Case_flag and SOPin */
short Caseflag(volatile float Uin,volatile float Vin,volatile float Win,volatile float Old_case_flag)
{
Case_flag=2;
if (Vin<0) /* % if SOPin in the back hemisphere, regardless of previous Case */
{
Case_flag=3; /* % new Case = 3*/
}
else
{
if(Win>0)
{
Case_flag=1;
}
}
return (Case_flag);
}
/**************************************************************************/
/*****************************************************************************/
float a2vss(volatile float Angle)
{ volatile float Voltt;
// Voltt=383.5239*pow(Angle,.25)-726.6876*pow(Angle,.5)+732.4840*pow(Angle,.75)-204.2709*Angle;
Voltt=A2VCoff[0]*pow(Angle,.25)+A2VCoff[1]*pow(Angle,.5)+A2VCoff[2]*pow(Angle,.75)+A2VCoff[3]*Angle;
Voltt= Voltt > Vmin? Voltt:0;
Voltt= Voltt < Vmax ? Voltt:Vmax;
return (Voltt);
}
/****************************************************************************/
float v2ass(volatile float Voltage)
{
volatile float Anglee,Arg1;
Arg1=Voltage*Voltage;
// Anglee=-6.194*pow(10,-3)*Voltage+9.6*pow(10,-5)*Arg1-2.4446*pow(10,-7)*Voltage*Arg1+3.3512*pow(10,-10)*Arg1*Arg1;
Anglee=V2ACoff[0]*pow(10,-3)*Voltage+V2ACoff[1]*pow(10,-5)*Arg1+V2ACoff[2]*pow(10,-7)*Voltage*Arg1+V2ACoff[3]*pow(10,-10)*Arg1*Arg1;
return (0 >Anglee ? 0:Anglee);
}
/**************************************************************************/
/*********************************************************************************/
float SOPin(volatile float Theta1, volatile float Theta2,volatile float Sigma1, volatile float *Vin, volatile float *Win)
{
float c1, s1, c2, s2, c3, s3;
s2 = (sin(Theta2));
s3 = (sin(Sigma1));
/* *Uin = s2*s3; will be include in the return return */
s1 = (sin(Theta1));
c2 = (cos(Theta2));
c3 = (cos(Sigma1));
c1 = (cos(Theta1));
*Vin = c1*c3-s1*c2*s3;
*Win = s1*c3+c1*c2*s3;
return (s2*s3);
}
/******************************************************************************/
/* applied voltage as a function of rotation angle th1 / sig1, including dynamics*/
float a2v1n(volatile float Angle_old, volatile float Angle_arg2,volatile float Angless_old,
volatile float temp, volatile float *Angle, volatile float *Angless)
{
volatile float Voltt;
/* should be checked with Geilad */
// Volt=1.0873*(Angle_arg2-0.7373*Angle_old+0.657*Angless_old);
Voltt=a2vss(Angle_arg2);
*Angle=v2ass(Voltt);
return Voltt;
}
/*************************************************************************/
/**********************************************************************************/
void a2v2n(volatile float Th2p_old,volatile float Th2m_old,volatile float Th2_in,
volatile float Th2pss_old,volatile float Th2mss_old,volatile float temp,
volatile float *V2p,volatile float *V2m,volatile float *Th2p,volatile float *Th2m,
volatile float *Th2pss, volatile float *Th2mss, volatile float *Th2)
{
if (Th2_in >=0 )
{
*V2m=0; /* thus the steady state value associated with v2m is also zero*/
// *Th2m=.7373*Th2m_old-.657*Th2mss_old;
*Th2m=0;
*Th2p=Th2_in;
*V2p=a2vss(*Th2p);
*Th2=*Th2p;
}
else /* arriving here if th2<=0*/
{
*V2p=0; /* % thus the steady state value associated with v2m is also zero*/
// *Th2p=.7373*Th2p_old-.657*Th2pss_old;
*Th2p=0;
*Th2m=-Th2_in;
*V2m=a2vss(*Th2m);
*Th2=*Th2p-*Th2m;
}
}
/*******************************************************************************/
/* % this is an "alternative variant" of reset_step.m, used to
% compute the required angles theta_1 and theta_2 when sigma_1=pi/2
% as a function of SOPin=(u_in,v_in,w_in); used in recovery when
% it is found that SOPin lies in the back hemisphere (Case 3)*/
float a_reset_step(volatile float Uin,volatile float Vin,volatile float Win,volatile float *Th2)
{ volatile float tw;
tw=pow((Vin*Vin+Win*Win),.5); /* L2 norm */
*Th2=hpi-atan2(tw,Uin);
tw=hpi-atan2(Win,-Vin);
return (tw);
}
/*******************************************************/
/*% this function computes the angles theta_2 and sigma_1 as
% a function of SOPin=(u_in,v_in,w_in) and of theta_1, during
% continuous reset. */
float reset_step(volatile float Th1,volatile float Uin, volatile float Vin,
volatile float Win, volatile float *Sig1)
{ volatile float c1,s1,tw,ttw,tv,beta1,beta2;
c1=cos(Th1);
s1=sin(Th1);
tw=-Vin*s1+Win*c1;
ttw=pow((Uin*Uin+tw*tw),.5);
tv=Vin*c1+Win*s1;
beta2=atan2(tw,Uin); /* % the matlab function atan2(y,x) finds the phase of */
/* % a point (in the range (-pi,pi]) in the plane with coordinates (x,y);*/
if (beta2<-hpi)
{
beta2=beta2+tpi;
}
beta1=atan2(ttw,-tv);
*Sig1=pi-beta1;
return hpi-beta2;
} /******************************************************************************/
/* ################################################# beginning a control period */
void Recovery(void)
{
volatile float a11,a12,a13,A2sin,A3cos,a21,a22,a23,a31,a32,a33,
determinant_inv,ai11,ai12,ai13,ai21,ai22,ai23,ai31,ai32,ai33;
volatile float arg1,GAngle, GV1,Gv2m,Gv2p,GV3,vout_new;
/* write: V1=0, V2m=0, V2p=0, V3=0 % set all voltages to zero*/
/* % best & fastest way to bring all rotation angles - approximately - to zero
% next compute the predicted angles a the end of the control period */
LED12_on ;
Control_Period =Control_Period_RC ;
V1=V2m=V2p=V3=Vmin;
Write_Output(V1,V2m,V2p,V3);
/* th1=lambda*th1-zeta*th1ss;*/
// Chim Measurments
// th1=zeta*(th1ss-th1ss_old)+lambda*(th1-th1ss_old)+th1ss_old;
// th1=th1>0? th1:0;
// th2p= lambda*th2p-zeta*th2pss;
// Chim Measurments
// th2p=zeta*(th2pss-th2pss_old)+lambda*(th2p-th2pss_old)+th2pss_old;
// th2p=th2p>0 ? th2p:0;
// th2m=lambda*th2m-zeta*th2mss;
// Chim Measurments
// th2m=zeta*(th2mss-th2mss_old)+lambda*(th2m-th2mss_old)+th2mss_old;
// th2m=th2m> 0 ? th2m:0;
// th2=th2p-th2m;
// sig1=lambda*sig1-zeta*sig1ss;
// Chim Measurments
// sig1=zeta*(sig1ss-sig1ss_old)+lambda*(sig1-sig1ss_old)+sig1ss_old;
// sig1=sig1>0 ? sig1:0;
/* % the steady state angles for the zero voltages are all zero */
th1ss=th2mss=th2pss=sig1ss=0.0;
th1=th2m=th2p=sig1=0.0;
a11=sin(sig1)*sin(th2); /* % prepare multipliers*/
a12=cos(sig1)*cos(th1)-sin(sig1)*cos(th2)*sin(th1);
a13=cos(sig1)*sin(th1)+sin(sig1)*cos(th2)*cos(th1);
/* % compute coeffcients of the matrix relating angles to measured vout */
V1=a2v1n(th1,hpi,th1ss,temp,&th1,&th1ss);
a2v2n(th2p,th2m,0,th2pss,th2mss,temp,&V2p,&V2m,&th2p,&th2m,&th2pss,&th2mss,&th2);
V3=a2v1n(sig1,0,sig1ss,temp,&sig1,&sig1ss);
/*
###
Read vout % for 3 zero voltages */
vout= Read_Vout(1);
/* # this command is issued just before the ending of the control period
################################################# beginning a control period
write V1, V2p, V2m, V3 */
measurement_1=vout; /*% approximately ~vin
*/
/*
DAC1_data = V1*DAC_scal;
DAC2_data = V2m*DAC_scal;
DAC3_data = V2p*DAC_scal;
DAC4_data = V3*DAC_scal;
DAC_FIRE ;
*/
Write_Output(V1,V2m,V2p,V3);
a21=sin(sig1)*sin(th2);
a22=cos(sig1)*cos(th1)-sin(sig1)*cos(th2)*sin(th1);
a23=cos(sig1)*sin(th1)+sin(sig1)*cos(th2)*cos(th1);
/* % compute next set of voltages and angles
% bring rotation angles to th1=0; th2=pi/2; sig1=pi/2;*/
V1=a2v1n(th1,0.0,th1ss,temp,&th1,&th1ss);
a2v2n(th2p,th2m,hpi,th2pss,th2mss,temp,&V2p,&V2m,&th2p,&th2m,&th2pss,&th2mss,&th2);
V3=a2v1n(sig1,hpi,sig1ss,temp,&sig1,&sig1ss);
/* ####
Read vout % for th1=pi, th2=0, sig1=0
# this command is issued just before the ending of the control period */
vout= Read_Vout(1);
/* ################################################# beginning a control period
write V1, V2p, V2m, V3 */
measurement_2=vout; /*% measure ~win %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
*/
/*
DAC1_data = V1*DAC_scal;
DAC2_data = V2m*DAC_scal;
DAC3_data = V2p*DAC_scal;
DAC4_data = V3*DAC_scal;
DAC_FIRE ;
*/
Write_Output(V1,V2m,V2p,V3);
a31=sin(sig1)*sin(th2);
a32=cos(sig1)*cos(th1)-sin(sig1)*cos(th2)*sin(th1);
a33=cos(sig1)*sin(th1)+sin(sig1)*cos(th2)*cos(th1);
determinant_inv=1.0/(a11*a22*a33-a11*a23*a32-a21*a12*a33+a21*a13*a32+a31*a12*a23-a31*a13*a22);
ai11=determinant_inv*(a22*a33-a23*a32);
ai12=determinant_inv*(-a12*a33+a13*a32);
ai13=determinant_inv*(a12*a23-a13*a22);
ai21=determinant_inv*(-a21*a33+a23*a31);
ai22=determinant_inv*(a11*a33-a13*a31);
ai23=determinant_inv*(-a11*a23+a13*a21);
ai31=determinant_inv*(a21*a32-a22*a31);
ai32=determinant_inv*(-a11*a32+a12*a31);
ai33=determinant_inv*(a11*a22-a12*a21);
/* % estimate angles at the end of the next control period
% with old voltages left unchanged; the new angles will be "old"
% in forthcoming computations */
/* th1_old=lambda*th1+(1-lambda)*th1ss;
th2p_old=lambda*th2p+(1-lambda)*th2pss;
th2m_old=lambda*th2m+(1-lambda)*th2mss;
th2_old=th2p-th2m;
sig1_old=lambda*sig1+(1-lambda)*sig1ss; */
// Chim Measurments
th1_old=th1;
th2p_old=th2p;
th2m_old=th2;
th2_old=th2p-th2m;
sig1_old=sig1;
/* #####
Read vout % for th1=0, th2=pi/2, sig1=pi/2
# this command is issued just before the ending of the control period */
vout= Read_Vout(1);
/* ################################################# beginning a control period
# no writing new voltages here because only now can find out SOPin*/
measurement_3=vout; /*% measure ~win */
/* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% compute an estimate for SOPin %%%%%%%%%%%% */
uin=ai11*measurement_1+ai12*measurement_2+ai13*measurement_3;
vin=ai21*measurement_1+ai22*measurement_2+ai23*measurement_3;
win=ai31*measurement_1+ai32*measurement_2+ai33*measurement_3;
// S_input=pow((uin*uin+vin*vin+win*win),0.5);
S_measured=pow((measurement_1*measurement_1+measurement_2*measurement_2+measurement_3*measurement_3),0.5);
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
Case_flag=4; /* ndicating a recovery - hence no "history" to the process*/
th1_old=th1;
sig1_old=sig1;
Case_flag=Caseflag(uin,vin,win,Case_flag); /* find out the Case according */
/* to SOPin location without "hysteresis" on Case_flag*/
/* % continue to find out apropriate rotation angles and applied voltages*/
if (Case_flag==1)
{
th1=0.0;
th2=reset_step(0.0,uin,vin,win,&sig1); /*% compute needed th2 and sig1*/
}
else
{
if (Case_flag==2)
{
th1=pi;
th2=reset_step(pi,uin,vin,win,&sig1); /*% compute needed th2 and sig1*/
/* added be Wei to compancate for change in
varation in the plates */
// th2=th2+0;
// sig1=sig1+.2;
/*End Added by Wei */
}
else /*% if Case_flag=3*/
{
sig1=hpi;
th1=a_reset_step(uin,vin,win,&th2); /* % compute needed th1 and th2*/
}
}
/*% compute voltage to be written at the end of this cycle*/
V1=a2v1n(th1_old,th1,th1ss,temp,&th1,&th1ss);
a2v2n(th2p,th2m,th2,th2pss,th2mss,temp,&V2p,&V2m,&th2p,&th2m,&th2pss,&th2mss,&th2);
V3=a2v1n(sig1_old,sig1,sig1ss,temp,&sig1,&sig1ss);
/*% when these voltages are written, the recovery process is complete;
% the system is back in operation with no immediate need for reset
% but will start peak seeking, next round */
Reset_flag=0;
Peak_Seeking_flag=1 ;
/*
% wait till end of control period
################################################# beginning a control period
write V1, V2p, V2m, V3 % at the end of the next step, SOPout~e2
% next, begin computation of next step, as the 1st step of a Peak Search cycle
% call PSstep and use only the "successful" answer*/
/*
DAC1_data = V1*DAC_scal;
DAC2_data = V2m*DAC_scal;
DAC3_data = V2p*DAC_scal;
DAC4_data = V3*DAC_scal;
DAC_FIRE ;
*/
Write_Output(V1,V2m,V2p,V3);
vout=vout_old= Read_Vout(1);
// Rewind();
LED12_off ;
/********************************************************************************************/
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -