akiyoshi oguro
Trapezoid sin Vector
Diff: main.cpp
- Revision:
- 0:fa432f8ea1a6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jun 28 11:52:27 2019 +0000
@@ -0,0 +1,638 @@
+#include "mbed.h"
+#include <math.h>
+
+#include <IO_define.h>
+#include <kukei.h>
+
+void time_UP(){
+ ut1=uT.read_us();
+ //ut=0;
+ ui=0;
+ }
+void time_UN(){
+ ut2=uT.read_us();
+ uT.reset();
+
+ }
+void time_VP(){
+ vt1=vT.read_us();
+ vi=0;
+ }
+void time_VN(){
+ vt2=vT.read_us();
+ vT.reset();
+ }
+void time_WP(){
+ wt1=wT.read_us();
+ wi=0;
+ et=0;
+ }
+void time_WN(){
+ wt2=wT.read_us();
+ wT.reset();
+ }
+
+int main() {
+
+ uT.start();
+ vT.start();
+ wT.start();
+ //uTc.start();
+
+ PWM_IN1_U.period_us(20);//50
+ PWM_IN2_V.period_us(20);
+ PWM_IN3_W.period_us(20);
+
+ // zt.attach_us(&ztr, tau);
+ //ztc.attach_us(&ztrc,tau);
+
+ Vr_adc_i=V_adc.read();
+ wait_ms(100);
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ EN1_U=1;
+ EN2_V=1;
+ EN3_W=1;
+
+ while(1) {
+
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+
+ Vr_adc=V_adc.read();
+ y=Vr_adc- Vr_adc_i;
+ ay=fabs(y);
+ power=ay;
+ if(y>0.1){
+ frd=0;
+ //aout=0.5;
+ }
+ if(y<-0.1){
+ frd=1;
+ //aout=0.9;
+ }
+
+ if(ay<0.1){
+ q=0;
+ aout=0.0;
+ r=0;
+ vst=0;
+ //Speed=0;
+ //SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ //power=0.0;
+ }
+ /************起動********************/
+if(((y<-0.1)||(y>0.1))&&(q==0)){
+//if(q==0){
+ while(q<20){
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+
+/*****Forword 起動******/
+ if((frd==0)){
+ //SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ if(SH==5){
+ start_F5();
+ }
+ if(SH==4){
+ start_F4();
+ }
+ if(SH==6){
+ start_F6();
+ }
+ if(SH==2){
+ start_F2();
+ }
+ if(SH==3){
+ start_F3();
+ }
+ if(SH==1){
+ start_F1();
+ }
+ }
+
+/*******Reversal 起動************/
+ if((frd==1)){
+ //SH=HALL_U<2|HALL_V<<1|HALL_W;
+ if(SH==3){
+ start_R3();
+ }
+ if(SH==2){
+ start_R2();
+ }
+ if(SH==6){
+ start_R6();
+ }
+ if(SH==4){
+ start_R4;
+ }
+ if(SH==5){
+ start_R5();
+ }
+ if(SH==1){
+ start_R1();
+ }
+ }
+ q++;
+ }
+ }
+
+ if(Speed<1000){//2000
+ /**************矩形波駆動始動***************/
+ /*******Forward******************/
+ if(frd==0){
+ switch(SH){
+ case 5: PWM_W();//W
+ break;
+ case 4: EN_W();//W
+ break;
+ case 6: PWM_U();//U
+ break;
+ case 2: EN_U();//U
+ break;
+ case 3: PWM_V();//V
+ break;
+ case 1: EN_V();//V
+ break;
+ default :PWM_W();//W
+ break;
+ }
+ HALL_Ui.rise(time_WP);
+ HALL_Wi.fall(time_VN);
+ HALL_Vi.rise(time_UP);
+ HALL_Ui.fall(time_WN);
+ HALL_Wi.rise(time_VP);
+ HALL_Vi.fall(time_UN);
+ }
+/*******Reversal******************/
+ if(frd==1){
+ switch(SH){
+ case 5: PWM_W();
+ break;
+ case 1: EN_W();
+ break;
+ case 3: PWM_V();
+ break;
+ case 2: EN_V();
+ break;
+ case 6: PWM_U();
+ break;
+ case 4: EN_U();
+ break;
+ default :PWM_W();
+ break;
+ }
+ HALL_Ui.rise(time_UP);
+ HALL_Wi.fall(time_WN);
+ HALL_Vi.rise(time_VP);
+ HALL_Ui.fall(time_UN);
+ HALL_Wi.rise(time_WP);
+ HALL_Vi.fall(time_VN);
+ }
+
+ }//Speed<2000
+
+ /*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*fabs(wsi*1E-6)));
+ usi=1*(ut2-ut1);
+ vsi=1*(vt2-vt1);
+ wsi=1*(wt2-wt1);
+ usic=1*(ut2c-ut1c);
+ //Speed=60*(1/(6.0*fabs(usi)*1E-6));//CQ 6
+ /* if(Vr_adc < 0.08f){
+ q=0;
+ Vr_adc=0;
+ r=0;
+ vst=0;
+ i=0;
+ } */
+
+ #if 1
+ if(Speed>=1000){
+ ui=ui+1;
+ vi=vi+1;
+ wi=wi+1;
+ et=et+1;
+
+ thu=2*PI*(1/(2*usi*1E-6))*ui*3.6E-4; //usi 2.3E-4 vst
+ thv=2*PI*(1/(2*vsi*1E-6))*vi*3.6E-4;
+ thw=2*PI*(1/(2*wsi*1E-6))*wi*3.6E-4;
+ //th=(thu+thv+thw)/3;
+ if(r==0){
+ thave=(thu+thv+thw)/3;
+ th=thu;
+ //th=thave;
+ s=0;
+ }
+ if(r==1){
+ if((Speed > 1000)&&(Speed <=1500)){
+ iso=-0.3;
+ Vqp=0.3*Vq; //0.2
+ Vdlink=0.4;
+ Edw=3500;
+ z=0.7;
+ }
+ if((Speed > 1500)&&(Speed <=2000)){
+ iso=0.7;
+ Vqp=0.3*Vq; //0.2
+ Vdlink=0.45;
+ Edw=3600;
+ z=0.7;
+ }
+ if((Speed > 2000)&&(Speed <=3000)){
+ iso=0.8;
+ Vqp=0.3*Vq; //0.2
+ Vdlink=0.5;
+ Edw=3700;
+ z=0.7;
+ }
+ if((Speed > 3000)&&(Speed <=4000)){
+ iso=1.2; //1.2
+ Vqp=0.3*Vq; //0.3
+ Vdlink=0.5; //0.5
+ Edw=3700; //3700
+ z=0.7;
+ }
+ if((Speed > 4000)&&(Speed <=5000)){
+ iso=1.8; //1.8
+ Vqp=0.3*Vq; //0.3
+ Vdlink=0.6; //0.6
+ Edw=3800; //3800
+ z=0.7;
+ }
+ if((Speed > 5000)&&(Speed <=5500)){
+ iso=2.2; //2.2
+ Vqp=0.3*Vq; //0.3
+ Vdlink=0.5; //0.7
+ Edw=3900; //3900
+ z=0.7; //0.66
+ }
+ if((Speed > 5500)&&(Speed <=6000)){
+ iso=2.8; //2.2
+ Vqp=1.0*Vq; //0.35
+ Vdlink=0.5; //0.8
+ Edw=6000; //6000
+ z=0.8; //0.7
+ }
+ if((Speed > 6000)){
+ iso=2.2; //2.2
+ Vqp=1.0*Vq; // 0.4
+ Vdlink=0.5; //0.9
+ Edw=7000;//7000
+ z=0.8; //0.8
+ }
+ // th=(Wz*et*1.9E-4)+iso; //1.99E-4
+ th=(W*et*1.9E-4)+iso; //1.9E-4
+ // th=Wo*wi*1.85E-4;
+ }
+
+ if(r==0){
+ su=sin(thu);//-0.2f; // p51 0.5
+ sv=sin(thv);//-0.05f;
+ sw=sin(thw);//-0.05f;//-0.7
+
+ PWM_IN1_U.write((su+0.5)*power); //kaisha sv ier su
+ PWM_IN2_V.write((sv+0.5)*power); //kaisha sw ie sv
+ PWM_IN3_W.write((sw+0.5)*power); //kaisha su ie sw
+ }
+ if(r==1){
+ PWM_IN1_U.write(du); // dw ie no white and Black
+ PWM_IN2_V.write(dv); // du
+ PWM_IN3_W.write(dw); // dv
+ }
+
+
+ // myled = !myled;
+ // }
+// }
+
+ Curr_u=(Curr_ui);//0.33f; kaisha v w u narabikae
+ Curr_v=(Curr_vi);///0.33f;
+ Curr_w=(Curr_wi);///0.33f;
+ /* iuvw[0]=(Curr_ui);
+ iuvw[1]=(Curr_vi);
+ iuvw[2]=(Curr_wi);*/
+ float Itau=1.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/2.0))/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/2.0))/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/2.0))/Itau;
+ iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
+ /*************************************/
+
+ iab[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;
+ iab[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
+
+
+ idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; //thave
+ idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; //th
+
+
+ /****Filter Id********/
+ idqo[0]=idq[0];
+ float Id1,Id2,Id3,Id4;//0.01
+ float Idtau= 1.0E-6,Iddt=1.0E-6; //1E-1
+ Id1=Iddt*(idqi[0]-idqo[0])/Idtau;
+ Id2=Iddt*(idqi[0]-(idqo[0]+Id1/2.0))/Idtau;
+ Id3=Iddt*(idqi[0]-(idqo[0]+Id2/2.0))/Idtau;
+ Id4=Iddt*(idqi[0]-(idqo[0]+Id3))/Idtau;
+ idq[0]=idqo[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;
+ /*************************************/
+
+ /****Filter Iq********/
+ idqo[1]=idq[1];
+ float Iq1,Iq2,Iq3,Iq4;//0.01
+ float Iqtau= 1.0E-6,Iqdt=1.0E-6;
+ Iq1=Iqdt*(idqi[1]-idqo[1])/Iqtau;
+ Iq2=Iqdt*(idqi[1]-(idqo[1]+Iq1/2.0))/Iqtau;
+ Iq3=Iqdt*(idqi[1]-(idqo[1]+Iq2/2.0))/Iqtau;
+ Iq4=Iqdt*(idqi[1]-(idqo[1]+Iq3/2.0))/Iqtau;
+ idq[1]=idqo[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;
+ /*************************************/
+
+ // Wcom=(2*PI)/(vstf*1E-6);
+
+
+ // Vd=(idq[0]);//tmp vdq
+ // Vq=(idq[1]);//tmp vdq
+
+ /*****PID Id *****/
+ Idin=(-0.25)-idq[0]; // -0.25
+ float adi,bdi,cdi,workdi[2];
+ float kpdi=1.0,kidi=0.5,kddi=0.0; //p51 0.5 1.2 0.0
+ float dtdi=1.0E-6;//1E-6
+ 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;
+ /**********************************/
+ //Icom=0.5;
+ /*****PID Iq *****/
+ Iqin=Vr_adc-idq[1]; //kaisha 600 ie 500
+ float aqi,bqi,cqi,workqi[2];
+ float kpqi=2.0,kiqi=1.2,kdqi=0.0; // 1.5 0.8
+ float dtqi=1.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;
+ /**********************************/
+ /****Filter Vq********/
+ /*float Vq1,Vq2,Vq3,Vq4;//0.01
+ float Vqtau= 1.0E-6,Vqdt=1.0E-6;
+ Vq1=Vqdt*(Vqi-Vq)/Vqtau;
+ Vq2=Vqdt*(Vqi-(Vq+Vq1/2.0))/Vqtau;
+ Vq3=Vqdt*(Vqi-(Vq+Vq2/2.0))/Vqtau;
+ Vq4=Vqdt*(Vqi-(Vq+Vq3/2.0))/Vqtau;
+ Vq=Vq+(Vq1+2.0*Vq2+2.0*Vq3+Vq4)/6.0;*/
+ /*************************************/
+ //Vq=Vr_adc;
+ //if(i<10000){
+//if(r==0){
+ /* usi=ut2-ut1;
+ vsi=vt2-vt1;
+ wsi=wt2-wt1;*/
+
+if(i<10000){
+ vst=vsi;
+ vstf=vst;
+ }
+ //}
+ else { //kokokara else
+ //if(r==1){
+ // usi=vstf;vsi=vstf;wsi=vstf;
+ vst=vstf;
+
+ i=10000;
+ // }
+ // r=i%100;
+ // i++;
+
+ // Vd=(idq[0]);//tmp vdq
+ // Vq=(idq[1]);//tmp vdq
+
+
+ /* Ed= (Vd)-0.11f*idq[0]+Wz*0.018E-3*idq[1];//0.018
+
+ if(Ed>=0){
+ Wz=(2*PI)/((vst+Edw*Ed*0.2)*1E-6);// 0.2
+ }
+ else{
+ Wz=(2*PI)/((vst-Edw*Ed*0.2)*1E-6); //0.45
+ } */
+
+ Wz=(2*PI)/(vst*1E-6); //vst
+ Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3
+ phm=Ed/(Wz);
+ dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); //0.018E-3
+ eth=asin(dth);
+
+ /*****PID θ *****/
+ // PLL=(Speed/100); //7 21 28
+ //PLL=Speed*2*PI/(60*28);
+ // PLL=W/Wz;
+ PLL=1.0; //30
+ //float WPLL=150.0; // 170
+ float as,bs,cs,works[2];
+ float kps=PLL,kis=PLL*PLL/5.0,kds=0.0;
+ float dts=1.0E-6;
+ Xsi=5.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.01){
+ W=(2*PI)/((vst*1E-6)+(therr/Wz));
+ }
+ if(therr<-0.01){
+ W=(2*PI)/((vst*1E-6)-(therr/Wz));
+ }
+
+ /*****PID ω *****/
+ Xin=5000*(1.05-Vr_adc)-vst; // 5500
+ float a,b,c,work[2];
+ float kp=2.0,ki=0.5,kd=0.0; // 2.0 0.5
+ float dt=10.0E-6;//10E-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;
+ /**********************************/
+ // float dtt=1000.0E-6;
+
+ /*float k1,k2,k3,k4,tau=0.001;//0.01
+ k1=dtt*(Xout-vstt)/tau;
+ k2=dtt*(Xout-(vstt+k1/2.0))/tau;
+ k3=dtt*(Xout-(vstt+k2/2.0))/tau;
+ k4=dtt*(Xout-(vstt+k3/2.0))/tau;
+ vstt=vstt+(k1+2.0*k2+2.0*k3+k4)/6.0;*/
+ vstt=Xout;
+ /********************************/
+if(frd==1){
+ if(300>fabs(vstt-vst)){ // ie 2000
+ vsti=vstt;
+ // vsti=vst;
+ r=1;
+ }
+ }
+if(frd==0){
+ if(300>fabs(vstt-vst)){ // ie 2000
+ vsti=vstt;
+ // vsti=vst;
+ r=1;
+ }
+ }
+ /****Filter********/
+ float vstfo=vstf;
+ float dttt=10.0E-6;//100E-6
+ float k11,k22,k33,k44,tau1=0.1;//0.01
+ k11=dttt*(vsti-vstfo)/tau1;
+ k22=dttt*(vsti-(vstfo+k11/2.0))/tau1;
+ k33=dttt*(vsti-(vstfo+k22/2.0))/tau1;
+ k44=dttt*(vsti-(vstfo+k33))/tau1;
+ vstf=vstfo+(k11+2.0*k22+2.0*k33+k44)/6.0;
+ //vstf=vsti;
+ /*************************************/
+
+
+
+ }//else kokomade
+ /* if((Vr_adc > 0.9f)&&(Vr_adc <= 0.95f)){
+ vstf =800.0f;
+ }
+ if((Vr_adc > 0.95f)&&(Vr_adc < 0.975f)){
+ vstf=700.0f;
+ }
+ if(Vr_adc >= 0.975f){
+ vstf=600.0f;
+ } */
+
+
+
+ Va=cos(th)*Vd-sin(th)*Vqp; //Vqp 負荷小 Vq*0.8～1.0負荷大
+ Vb=sin(th)*Vd+cos(th)*Vqp; //Vqp
+
+ aVa=abs(Va);
+ a3Vb=abs(sq3*Vb);
+
+ if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0
+
+ d1=sq32*(Va-sq3*Vb)*Vdlink;
+ d2=sq32*(sq23*Vb)*Vdlink;
+ d07=z-(d1+d2);
+ du=d1+d2+d07;
+ dv=d2+d07;
+ dw=d07;
+ }
+ if((aVa<=sq3*Vb)){ //sect 1
+
+ d3=sq32*(-Va+sq3*Vb)*Vdlink;
+ d2=sq32*(Va+sq3*Vb)*Vdlink;
+ d07=z-(d2+d3);
+ du=d2+d07;
+ dv=d2+d3+d07;
+ dw=d07;
+ }
+
+ if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2
+
+ d3=sq32*sq23*Vb*Vdlink;
+ d4=sq32*(-Va-sq3*Vb)*Vdlink;
+ d07=z-(d3+d4);
+ du=d07;
+ dv=d3+d2+d07;
+ dw=d4+d07;
+ }
+
+ if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3
+
+ d5=-sq32*sq23*Vb*Vdlink;
+ d4=sq32*(-Va+sq3*Vb)*Vdlink;
+ d07=z-(d4+d5);
+ du=d07;
+ dv=d4+d07;
+ dw=d4+d5+d07;
+ }
+
+ if((aVa<=-sq3*Vb)){ //sect 4
+
+ d5=sq32*(-Va-sq3*Vb)*Vdlink;;
+ d6=sq32*(Va-sq3*Vb)*Vdlink;
+ d07=z-(d5+d6);
+ du=d6+d07;
+ dv=d07;
+ dw=d5+d6+d07;
+ }
+
+ if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5
+
+ d1=sq32*(Va+sq3*Vb)*Vdlink;;
+ d6=-sq32*sq23*Vb*Vdlink;;
+ d07=z-(d1+d6);
+ du=d1+d6+d07;
+ dv=d07;
+ dw=d6+d07;
+ }
+
+ /* mypwmA.write(du);
+ mypwmB.write(dv);
+ mypwmC.write(dw);*/
+
+
+ /* suv=Vab[0]*zet;
+ svv=(Vab[0]*cos23+Vab[1]*sin23)*zet;
+ swv=(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(suv);
+ // SWAVE=iab[0]+0.5;
+ // SWAVE=Ed/10;
+ // SWAVE=idq[1];
+ // SWAVE=sin(th);
+ // SWAVE=vst/5000;
+ //SWAVE=Vb;
+ // SWAVE=iuvw[0];
+ //aout=(su+0.2)*power;
+ aout=du;
+
+ Vector=r;
+ i++;
+ }
+ #endif
+ }
+
+}
\ No newline at end of file