akiyoshi oguro
FOC sinewave drive ver2
Diff: main.cpp
- Revision:
- 0:dcfc6150c1d6
- Child:
- 1:e45063ee4be0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Aug 21 01:45:36 2020 +0000
@@ -0,0 +1,626 @@
+#include "mbed.h"
+#include "rtos.h"
+#define TS1 0.2
+#include <math.h>
+int q=0,START=20,i=0,s=0,vector;
+float uii=0,ut=0,ut1=0,ut2=0,usi=0;
+float vii=0,vt=0,vt1=0,vt2=0,vsi=0;
+float wii=0,wt=0,wt1=0,wt2=0,wsi=0;
+unsigned int ui=0,vi=0,wi=0,vect=0;
+float su=0,sv=0,sw=0,suv=0,svv=0,swv=0;
+float uci=0,uc=0;
+float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592;
+float Speed;
+float w,Vqp;
+float dth,phm,W,eth,therr,Em,PLL;
+float uvect,vvect,wvect;
+float sig=0;
+PwmOut mypwmA(PA_8); //PWM_OUT 8
+PwmOut mypwmB(PA_9); //9
+PwmOut mypwmC(PA_10);//10
+
+PwmOut Current_Ref(PB_4);
+
+DigitalOut Vector(PC_4);
+DigitalOut EN1(PC_10);
+DigitalOut EN2(PC_11);
+DigitalOut EN3(PC_12);
+
+DigitalOut vcl(PC_8);
+
+InterruptIn HA(PA_15);
+InterruptIn HB(PB_3);
+InterruptIn HC(PB_10);
+
+//AnalogIn V_adc(PB_1); //volume
+AnalogIn V_adc(PC_2); //Potention
+
+
+AnalogIn Vshuntu(PA_1);
+AnalogIn Vshuntv(PA_0);
+AnalogIn Vshuntw(PB_0);
+
+AnalogIn BEMF1(PC_3);//C7_37
+AnalogIn BEMF2(PB_0);//C7_34
+AnalogIn BEMF3(PA_7);//C10_26
+
+DigitalIn GPIO_BEMF(PC_9);
+DigitalIn CPOUT(PA_12);
+AnalogIn Curr_ui(PA_0);
+AnalogIn Curr_vi(PC_1);
+AnalogIn Curr_wi(PC_0); //PA_1
+Timer uT;
+Timer vT;
+Timer wT;
+AnalogOut SWAVE(PA_4);
+
+Serial pc(USBTX,USBRX);
+DigitalOut myled(LED1);
+
+float r2=sqrt(2.0f),r3=sqrt(3.0f);
+float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3},
+ {0,1.0f/r2,-1.0f/r2}};
+float iuvw[3];
+float iab[2],iabi[2],Vab[2];
+float idq[2],idqi[2];
+float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom;
+float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI);
+float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI);
+float Curr_u,Curr_v,Curr_w;
+float Xin,Xout;
+float Idin,Vdout,Iqin,Vqout,Xsi;
+int vstctle=0,vstctlz=0;
+float Vr_adc=0.0f;
+int adc,vtv=0;
+
+Ticker sinTime;
+
+ void HAH(){
+
+ ut1=uT.read_us();
+ ut=0;
+ ui=0;
+ // vect=0;
+ if(vector==0){
+ EN1=1;
+ EN2=1;
+ EN3=0;
+ }
+ /*if(vector==0){
+ mypwmA.write(su*Vr_adc); //0.5
+ mypwmB.write(0);
+ mypwmC.write(0);
+ }*/
+ }
+ void HAL(){
+
+ ut2=uT.read_us();
+ uT.reset();
+ if(Speed<=2500){
+ // wait_us(100);
+ vect=0; //******
+ }
+
+ if(vector==0){
+ EN1=1;
+ EN2=0;
+ EN3=1;
+ }
+ }
+ void HBH(){
+
+ vt1=vT.read_us();
+ vi=0;
+ if(Speed>2500){
+ vect=0;
+ }
+ if(vector==0){
+ EN1=0;
+ EN2=1;
+ EN3=1;
+ }
+ /* if(vector==0){
+ mypwmA.write(0);
+ mypwmB.write(sv*Vr_adc);
+ mypwmC.write(0);
+ }*/
+ }
+ void HBL(){
+
+ vt2=vT.read_us();
+ // vect=0;
+ vT.reset();
+ if(vector==0){
+ EN1=1;
+ EN2=1;
+ EN3=0;
+ }
+
+ }
+ void HCH(){
+ wt1=wT.read_us();
+ wi=0;
+ //vect=0;
+ if(vector==0){
+ EN1=1;
+ EN2=0;
+ EN3=1;
+ }
+ /* if(vector==0){
+ mypwmA.write(0);
+ mypwmB.write(0);
+ mypwmC.write(sw*Vr_adc);
+ }*/
+ }
+
+ void HCL(){
+ wt2=wT.read_us();
+ // vect=0;
+ wT.reset();
+ if(vector==0){
+ EN1=0;
+ EN2=1;
+ EN3=1;
+ }
+ }
+
+ void CPLT(){
+ pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
+ }
+
+ void timerTS1(void const*argument){
+ CPLT();
+ }
+
+Timer Timer1;
+int main() {
+
+ pc.baud(128000);
+ Timer1.start();
+ EN1=1;
+ EN2=1;
+ EN3=1;
+
+ mypwmA.period_us(20); //20
+
+ mypwmB.period_us(20);
+
+ mypwmC.period_us(20);
+
+ Current_Ref.period_us(15);
+ Current_Ref.write(0.6f);
+ uT.start();
+ vT.start();
+ wT.start();
+ Timer1.start();
+
+ RtosTimer RtosTimerTS1(timerTS1);
+ RtosTimerTS1.start((unsigned int)(TS1*3000));
+ Thread::wait(100);
+
+ while(1) {
+
+ /* if(CPOUT==1){
+ vect=0;
+ }*/
+
+ Vr_adc=V_adc.read();
+
+if((Vr_adc>0.08f)&&(q==0)){
+
+ while(q<10){
+ #if 0
+ mypwmA.write(0.5f);
+ mypwmB.write(0);
+ mypwmC.write(0);
+ wait_ms(START);
+
+ mypwmA.write(0);
+ mypwmB.write(0.5f);
+ mypwmC.write(0.0);
+ wait_ms(START);
+
+ mypwmA.write(0);
+ mypwmB.write(0.0);
+ mypwmC.write(0.5f);
+ wait_ms(START);
+#endif
+ #if 1
+ EN1=1;
+ EN2=1;
+ EN3=0;
+
+ mypwmA.write(0.5f); //0.5
+ mypwmB.write(0);
+ mypwmC.write(0);
+ wait_ms(START);
+ //wait_ms(START);
+
+ EN1=1;
+ EN2=0;
+ EN3=1;
+
+ wait_ms(START);
+
+ EN1=0;
+ EN2=1;
+ EN3=1;
+
+ mypwmA.write(0);
+ mypwmB.write(0.5f);
+ mypwmC.write(0);
+
+ wait_ms(START);
+
+ EN1=1;
+ EN2=1;
+ EN3=0;
+
+ wait_ms(START);
+
+ EN1=1;
+ EN2=0;
+ EN3=1;
+
+ mypwmA.write(0);
+ mypwmB.write(0);
+ mypwmC.write(0.5f);
+
+ wait_ms(START);
+ EN1=0;
+ EN2=1;
+ EN3=1;
+ #endif
+ q++;
+
+ }
+ }
+
+ /* HA.rise(&HAH);
+ HC.fall(&HCL);
+ HB.rise(&HBH);
+ HA.fall(&HAL);
+ HC.rise(&HCH);
+ HB.fall(&HBL);*/
+
+ HA.rise(&HAH);
+ HC.fall(&HCL);
+ HB.rise(&HBH);
+ HA.fall(&HAL);
+ HC.rise(&HCH);
+ HB.fall(&HBL);
+
+ Speed=60*(1/(7.0*2.0*usi*1E-6));
+
+ if(Vr_adc < 0.05f){
+ q=0;
+ Vr_adc=0;
+ vector=0;
+ vst=0;
+ i=0;
+ }
+
+ ui=ui+1;
+ vi=vi+1;
+ wi=wi+1;
+ vect++;
+ /* if(Curr_ui>0.48){ //&&(vtv==0)){
+ vect=0;
+
+ vtv=1;
+ vcl=1;
+ }*/
+ /* if((Curr_ui<=0.4)&&(vtv==0)){
+ // vect=0;
+ vtv=1;
+ vcl=0;
+ }*/
+ /*if(vect==100000){
+ vtv=1;
+ }*/
+
+ if(vector==0){
+ thu=2*PI*(1/(usi*1E-6))*ui*1.54E-4; //1.24E-4
+ thv=2*PI*(1/(vsi*1E-6))*vi*1.54E-4;
+ thw=2*PI*(1/(wsi*1E-6))*wi*1.54E-4;
+
+ thave=(thu+thv+thw)/3;
+ th=thu;
+ s=0;
+ }
+ if((vector==1)){
+ if((Speed > 500)&&(Speed <=1000)){
+
+ Vqp=0.6*Vq;
+ th=W*vect*1.1E-4;
+
+ }
+ if((Speed > 1000)&&(Speed <=1500)){//1000
+
+ Vqp=0.6*Vq; //0.5
+ th=W*vect*1.1E-4;
+
+ }
+ if((Speed > 1500)&&(Speed <=2000)){
+
+ Vqp=0.6*Vq; //0.65
+ th=W*vect*1.1E-4;
+
+ }
+ if((Speed > 2000)&&(Speed <=2500)){
+
+ Vqp=0.5*Vq;
+ th=W*vect*1.15E-4;
+
+ }
+ if((Speed > 2500)&&(Speed <=3000)){
+
+ Vqp=0.6*Vq;
+ th=W*vect*1.2E-4;
+
+ }
+ if((Speed > 3000)&&(Speed <=3500)){
+
+ Vqp=0.7*Vq;
+ th=W*vect*1.25E-4;
+
+ }
+ if((Speed > 3500)&&(Speed <=4000)){
+
+ Vqp=0.8*Vq;
+ th=W*vect*1.3E-4;
+
+ }
+ if((Speed > 4000)&&(Speed <=4500)){
+
+ Vqp=0.9*Vq;
+ th=W*vect*1.35E-4;
+
+ }
+ if((Speed > 4500)&&(Speed <=5000)){
+
+ Vqp=1.0*Vq;
+ th=W*vect*1.4E-4;
+
+ }
+ if((Speed > 5000)&&(Speed <= 6000)){
+
+ Vqp=1.2*Vq;
+ th=W*vect*1.45E-4;
+
+ }
+ if((Speed > 6000)){
+
+ Vqp=1.3*Vq;
+ th=W*vect*1.5E-4;
+
+ }
+
+ // th=(W*((vst*1.0E-6))); //1.0E-4 @HAL
+ th=W*vect*0.75E-4;
+ }
+
+ if(vector==0){
+ su=sin(thu);
+ sv=sin(thv);
+ sw=sin(thw);
+
+ mypwmA.write(su*Vr_adc);
+ mypwmB.write(sv*Vr_adc);
+ mypwmC.write(sw*Vr_adc);
+ }
+ if((vector==1)){
+ EN1=1;
+ EN2=1;
+ EN3=1;
+ mypwmA.write(uvect);
+ mypwmB.write(vvect);
+ mypwmC.write(wvect);
+ }
+
+ myled = !myled;
+
+ Curr_u=(Curr_ui);
+ Curr_v=(Curr_vi);
+ Curr_w=(Curr_wi);
+
+ float Itau=10.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6
+ /****Filter Iu********/
+ float Iu1,Iu2,Iu3,Iu4;//0.01
+ Iu1=Idt*(Curr_u-iuvw[0])/Itau;
+ Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau;
+ Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau;
+ Iu4=Idt*(Curr_u-(iuvw[0]+Iu3))/Itau;
+ iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0;
+ /*************************************/
+ /****Filter Iv********/
+ float Iv1,Iv2,Iv3,Iv4;//0.01
+ Iv1=Idt*(Curr_v-iuvw[1])/Itau;
+ Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau;
+ Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau;
+ Iv4=Idt*(Curr_v-(iuvw[1]+Iv3))/Itau;
+ iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0;
+ /*************************************/
+ /****Filter Iw********/
+ float Iw1,Iw2,Iw3,Iw4;//0.01
+ Iw1=Idt*(Curr_w-iuvw[2])/Itau;
+ Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau;
+ Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau;
+ Iw4=Idt*(Curr_w-(iuvw[2]+Iw3))/Itau;
+ iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
+ /*************************************/
+
+ iabi[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;
+ iabi[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
+ /****Filter Ia********/
+ float Ia1,Ia2,Ia3,Ia4;
+ float Iatau= 3.0E-6,Iadt=1.0E-6; //2.0E-4 1.0E-6
+ Ia1=Iadt*(iabi[0]-iab[0])/Iatau;
+ Ia2=Iadt*(iabi[0]-(iab[0]+Ia1/2.0))/Iatau;
+ Ia3=Iadt*(iabi[0]-(iab[0]+Ia2/2.0))/Iatau;
+ Ia4=Iadt*(iabi[0]-(iab[0]+Ia3))/Iatau;
+ iab[0]=iab[0]+(Ia1+2.0*Ia2+2.0*Ia3+Ia4)/6.0;
+ /*************************************/
+
+ /****Filter Ib********/
+ float Ib1,Ib2,Ib3,Ib4;//0.01
+ float Ibtau= 3.0E-6,Ibdt=1.0E-6; //2.0E-2 1.0E-6
+ Ib1=Ibdt*(iabi[1]-iab[1])/Ibtau;
+ Ib2=Ibdt*(iabi[1]-(iab[1]+Ib1/2.0))/Ibtau;
+ Ib3=Ibdt*(iabi[1]-(iab[1]+Ib2/2.0))/Ibtau;
+ Ib4=Ibdt*(iabi[1]-(iab[1]+Ib3))/Ibtau;
+ iab[1]=iab[1]+(Ib1+2.0*Ib2+2.0*Ib3+Ib4)/6.0;
+ /*************************************/
+
+ idq[0]=cos(th)*iab[0]+sin(th)*iab[1];
+ idq[1]=-sin(th)*iab[0]+cos(th)*iab[1];
+
+ /****Filter Id********/
+ /* float Id1,Id2,Id3,Id4;
+ float Idtau= 2.0E-4,Iddt=1.0E-6;
+ Id1=Iddt*(idqi[0]-idq[0])/Idtau;
+ Id2=Iddt*(idqi[0]-(idq[0]+Id1/2.0))/Idtau;
+ Id3=Iddt*(idqi[0]-(idq[0]+Id2/2.0))/Idtau;
+ Id4=Iddt*(idqi[0]-(idq[0]+Id3))/Idtau;
+ idq[0]=idq[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;*/
+ /*************************************/
+
+ /****Filter Iq********/
+ /* float Iq1,Iq2,Iq3,Iq4;//0.01
+ float Iqtau= 2.0E-4,Iqdt=1.0E-6; //2.0E-2 1.0E-6
+ Iq1=Iqdt*(idqi[1]-idq[1])/Iqtau;
+ Iq2=Iqdt*(idqi[1]-(idq[1]+Iq1/2.0))/Iqtau;
+ Iq3=Iqdt*(idqi[1]-(idq[1]+Iq2/2.0))/Iqtau;
+ Iq4=Iqdt*(idqi[1]-(idq[1]+Iq3))/Iqtau;
+ idq[1]=idq[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;*/
+ /*************************************/
+
+ /*****PID Id *****/
+ Idin=0.1-idq[0]; // -0.2
+ float adi,bdi,cdi,workdi[2];
+ float kpdi=3.0,kidi=1.5,kddi=0.0; //0.3 0.1 0.0
+ float dtdi=10.0E-6;//1E-5
+ adi=Idin;
+ bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi;
+ cdi=(Idin-workdi[0])/dtdi;
+ workdi[0]=Idin;
+ workdi[1]=bdi;
+ Vd=adi*kpdi+bdi*kidi+cdi*kddi;
+ /**********************************/
+
+ /*****PID Iq *****/
+ Iqin=(Vr_adc)-idq[1]; //kaisha 600 ie 500
+ float aqi,bqi,cqi,workqi[2];
+ float kpqi=2.0,kiqi=0.7,kdqi=0.0; // 1.0 0.7
+ float dtqi=10.0E-6;//1E-2
+ aqi=Iqin;
+ bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi;
+ cqi=(Iqin-workqi[0])/dtqi;
+ workqi[0]=Iqin;
+ workdi[1]=bqi;
+ Vq=(aqi*kpqi+bqi*kiqi+cqi*kdqi);
+ /***************************************/
+ usi=ut2-ut1;
+ vsi=vt2-vt1;
+ wsi=wt2-wt1;
+
+if(i<10000){
+ vst=vsi;
+ vstf=vst;
+ }
+ else {
+
+
+
+ vst=vstf;
+ i=10000;
+
+ Wz=(2*PI)/(vst*1E-6); //vst
+ Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3 0.11
+ phm=Ed/(Wz);
+ dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); // Vd 0.018E-3
+ eth=asin(dth);
+
+ /*****PID θ *****/
+ //PLL
+ float as,bs,cs,works[2];
+ float kps=5.0,kis=2.0,kds=0.0;
+ float dts=1.0E-3;//1.0E-6
+ Xsi=0.0-eth;
+ as=Xsi;
+ bs=works[1]+(Xsi+works[0])/2.0*dts;
+ cs=(Xsi-works[0])/dts;
+ works[0]=Xsi;
+ works[1]=bs;
+ therr=as*kps+bs*kis+cs*kds;
+ /*******PLL W ***********/
+ if(therr>0.2){ //0.01
+ W=(2*PI)/((vst*1E-6)+(therr/Wz)); //+
+ }
+ if(therr<-0.01){ //0.01
+ W=(2*PI)/((vst*1E-6)-(therr/Wz)); //-
+ }
+ /*****PID ω *****/
+ Xin=6500*(1.25-Vr_adc)-vst; // 6500 1.15
+ float a,b,c,work[2];
+ float kp=1.1,ki=0.7,kd=0.0; // 1.2 0.7 0.0
+ float dt=10.0E-6;//100E-6
+ a=Xin;
+ b=work[1]+(Xin+work[0])/2.0*dt;
+ c=(Xin-work[0])/dt;
+ work[0]=Xin;
+ work[1]=b;
+ Xout=a*kp+b*ki+c*kd;
+ vstt=Xout;
+ /********************************/
+ if(2000>abs(vst-vstt)){
+ vsti=vstt; //vstt
+ vector=1;
+ }
+ else{
+ vsti=vst;
+ vector=0;
+ }
+ /****Filter********/
+ float dttt=100.0E-6;//100E-6
+ float k11,k22,k33,k44,tau1=0.01;//0.01
+ k11=dttt*(vsti-vstf)/tau1;
+ k22=dttt*(vsti-(vstf+k11/2.0))/tau1;
+ k33=dttt*(vsti-(vstf+k22/2.0))/tau1;
+ k44=dttt*(vsti-(vstf+k33/2.0))/tau1;
+ vstf=vstf+(k11+2.0*k22+2.0*k33+k44)/6.0;
+ /*************************************/
+
+ }//else kokomade
+
+
+ Vab[0]=cos(th)*Vd-sin(th)*Vqp;//Vqp
+ Vab[1]=sin(th)*Vd+cos(th)*Vqp;
+
+
+ uvect=(Vab[0]*zet);
+ vvect=((Vab[0]*cos23+Vab[1]*sin23)*zet);
+ wvect=((Vab[0]*cos43+Vab[1]*sin43)*zet);
+
+ // SWAVE=Vd;
+ // SWAVE=th/4;
+ // SWAVE=Vq;
+ // SWAVE=West/5000;
+ //SWAVE=Wo/5000;
+ //SWAVE=Wz/5000;
+ // SWAVE=sin(svv);
+ // SWAVE=iab[0]+0.5;
+ //SWAVE=Ed;
+ // SWAVE=therr+0.5;
+ //SWAVE=idq[1];
+ // SWAVE=Curr_v;
+ // SWAVE=iuvw[1];
+ // SWAVE=vst/5000;
+ //SWAVE=Vab[1];
+ SWAVE=iuvw[0];
+ //SWAVE=vvect;
+ //SWAVE=BEMF1;
+ Vector=vector;
+ i++;
+ }
+}