akiyoshi oguro / Mbed 2 deprecated Vector_SVPWM_drive_F302R8

Vector SVPWM Drive SPMSM

Dependencies:   mbed mbed-rtos

main.cpp@3:cb3ad5a26986, 2020-11-18 (annotated)

Committer:
oguro
Date:
Wed Nov 18 06:44:05 2020 +0000
Revision:
3:cb3ad5a26986
Parent:
2:0058dae8606d 
Refine Vector SVPWM Drive

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 1:74d7a989b741 1 #include "mbed.h"
oguro 0:866aafac0128 2 #include "rtos.h"
oguro 0:866aafac0128 3 #define TS1 0.2
oguro 0:866aafac0128 4 #include <math.h>
oguro 1:74d7a989b741 5 int q=0,START=15,i=0,r=0,s=0;
oguro 0:866aafac0128 6 float uii=0,ut=0,ut1=0,ut2=0,usi=0;
oguro 0:866aafac0128 7 float vii=0,vt=0,vt1=0,vt2=0,vsi=0;
oguro 0:866aafac0128 8 float wii=0,wt=0,wt1=0,wt2=0,wsi=0;
oguro 0:866aafac0128 9
oguro 0:866aafac0128 10 unsigned int ui=0,vi=0,wi=0,et=0;
oguro 0:866aafac0128 11 float su=0,sv=0,sw=0,suv=0,svv=0,swv=0;
oguro 0:866aafac0128 12 float uci=0,uc=0;
oguro 0:866aafac0128 13 float vst=0,vstt=0,vsti=0,vstf=0,Edw=0,PI=3.141592;
oguro 0:866aafac0128 14 float Speed;
oguro 0:866aafac0128 15 float Wnon,Ednon,W,PLL;
oguro 0:866aafac0128 16 PwmOut mypwmA(PA_8); //PWM_OUT 8
oguro 0:866aafac0128 17 PwmOut mypwmB(PA_9); //9
oguro 0:866aafac0128 18 PwmOut mypwmC(PA_10);//10
oguro 0:866aafac0128 19
oguro 0:866aafac0128 20 PwmOut Current_Ref(PB_4);
oguro 0:866aafac0128 21
oguro 0:866aafac0128 22 DigitalOut Vector(PC_4);
oguro 0:866aafac0128 23 DigitalOut EN1(PC_10);
oguro 0:866aafac0128 24 DigitalOut EN2(PC_11);
oguro 0:866aafac0128 25 DigitalOut EN3(PC_12);
oguro 0:866aafac0128 26
oguro 0:866aafac0128 27 InterruptIn HA(PA_15);
oguro 0:866aafac0128 28 InterruptIn HB(PB_3);
oguro 0:866aafac0128 29 InterruptIn HC(PB_10);
oguro 0:866aafac0128 30
oguro 0:866aafac0128 31 //AnalogIn V_adc(PB_1); //volume
oguro 0:866aafac0128 32 AnalogIn V_adc(PC_2); // Gaibu Potention
oguro 0:866aafac0128 33
oguro 0:866aafac0128 34
oguro 1:74d7a989b741 35 AnalogIn Vshuntu(PA_1);
oguro 1:74d7a989b741 36 AnalogIn Vshuntv(PA_0);
oguro 1:74d7a989b741 37 AnalogIn Vshuntw(PB_0);
oguro 0:866aafac0128 38
oguro 0:866aafac0128 39 AnalogIn BEMF1(PC_3);//C7_37
oguro 0:866aafac0128 40 AnalogIn BEMF2(PB_0);//C7_34
oguro 0:866aafac0128 41 AnalogIn BEMF3(PA_7);//C10_26
oguro 0:866aafac0128 42
oguro 0:866aafac0128 43
oguro 0:866aafac0128 44 DigitalIn GPIO_BEMF(PC_9);
oguro 0:866aafac0128 45 DigitalIn CPOUT(PA_12);
oguro 0:866aafac0128 46 AnalogIn Curr_ui(PA_0);
oguro 0:866aafac0128 47 AnalogIn Curr_vi(PC_1);
oguro 0:866aafac0128 48 AnalogIn Curr_wi(PC_0); //PA_1
oguro 0:866aafac0128 49 Timer uT;
oguro 0:866aafac0128 50 Timer vT;
oguro 0:866aafac0128 51 Timer wT;
oguro 3:cb3ad5a26986 52 AnalogOut SWAVE(PA_4);
oguro 0:866aafac0128 53
oguro 0:866aafac0128 54 Serial pc(USBTX,USBRX);
oguro 0:866aafac0128 55
oguro 0:866aafac0128 56 DigitalOut myled(LED1);
oguro 0:866aafac0128 57
oguro 0:866aafac0128 58 float r2=sqrt(2.0f),r3=sqrt(3.0f);
oguro 0:866aafac0128 59 float Cuvw[2][3]={{r2/r3,-1.0f/r2/r3,-1.0f/r2/r3},
oguro 0:866aafac0128 60 {0,1.0f/r2,-1.0f/r2}};
oguro 0:866aafac0128 61 float iuvw[3];
oguro 0:866aafac0128 62 float iab[2],Vab[2];
oguro 0:866aafac0128 63 float idq[2],idqi[2],idqo[2];
oguro 0:866aafac0128 64 float thave,th,thu,thv,thw,Ed,Vd,Vq,Vqi,West,Wz,Wo,Icom,Wcom;
oguro 0:866aafac0128 65 float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI);
oguro 0:866aafac0128 66 float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI);
oguro 0:866aafac0128 67 float Curr_u,Curr_v,Curr_w;
oguro 0:866aafac0128 68 float Xin,Xout;
oguro 0:866aafac0128 69 float Idin,Vdout,Iqin,Vqout,Xsi;
oguro 0:866aafac0128 70 int vstctle=0,vstctlz=0;
oguro 0:866aafac0128 71 float Vr_adc=0.0f,iso=0,Vqp;;
oguro 1:74d7a989b741 72 int adc;
oguro 0:866aafac0128 73
oguro 0:866aafac0128 74 float therr,dth,eth,phm;
oguro 0:866aafac0128 75 float sq32=sqrt(3.0f/2.0f);
oguro 0:866aafac0128 76 float sq23=2.0f/sqrt(3.0f);
oguro 0:866aafac0128 77 float sq3=1.0f/sqrt(3.0f);
oguro 1:74d7a989b741 78 float Vdlink=0.5; //3.0f/5.0f;
oguro 0:866aafac0128 79 float aVa;
oguro 0:866aafac0128 80 float a3Vb;
oguro 0:866aafac0128 81
oguro 1:74d7a989b741 82 float z=0.7;//0.7
oguro 0:866aafac0128 83 float Va,Vb;
oguro 0:866aafac0128 84 float d1,d2,d3,d4,d5,d6,d07;
oguro 0:866aafac0128 85 float du,dv,dw;
oguro 0:866aafac0128 86 bool cu,cv,cw;
oguro 0:866aafac0128 87
oguro 0:866aafac0128 88 Ticker sinTime;
oguro 0:866aafac0128 89
oguro 0:866aafac0128 90 void HAH(){
oguro 0:866aafac0128 91 ut1=uT.read_us();
oguro 0:866aafac0128 92 ut=0;
oguro 0:866aafac0128 93 ui=0;
oguro 0:866aafac0128 94 }
oguro 0:866aafac0128 95 void HAL(){
oguro 0:866aafac0128 96 ut2=uT.read_us();
oguro 0:866aafac0128 97 uT.reset();
oguro 0:866aafac0128 98
oguro 0:866aafac0128 99 }
oguro 0:866aafac0128 100 void HBH(){
oguro 0:866aafac0128 101
oguro 0:866aafac0128 102 vt1=vT.read_us();
oguro 0:866aafac0128 103 vi=0;
oguro 0:866aafac0128 104 }
oguro 0:866aafac0128 105 void HBL(){
oguro 0:866aafac0128 106 vt2=vT.read_us();
oguro 0:866aafac0128 107 vT.reset();
oguro 0:866aafac0128 108 }
oguro 0:866aafac0128 109 void HCH(){
oguro 0:866aafac0128 110 wt1=wT.read_us();
oguro 0:866aafac0128 111 wi=0;
oguro 0:866aafac0128 112 et=0;
oguro 0:866aafac0128 113 }
oguro 0:866aafac0128 114
oguro 0:866aafac0128 115 void HCL(){
oguro 0:866aafac0128 116 wt2=wT.read_us();
oguro 0:866aafac0128 117 wT.reset();
oguro 0:866aafac0128 118 }
oguro 0:866aafac0128 119
oguro 0:866aafac0128 120 void CPLT(){
oguro 0:866aafac0128 121 pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 0:866aafac0128 122 }
oguro 0:866aafac0128 123
oguro 0:866aafac0128 124 void timerTS1(void const*argument){
oguro 0:866aafac0128 125 CPLT();
oguro 0:866aafac0128 126 }
oguro 0:866aafac0128 127
oguro 0:866aafac0128 128 Timer Timer1;
oguro 0:866aafac0128 129 int main() {
oguro 0:866aafac0128 130 pc.baud(128000);
oguro 0:866aafac0128 131
oguro 0:866aafac0128 132 Timer1.start();
oguro 0:866aafac0128 133 EN1=1;
oguro 0:866aafac0128 134 EN2=1;
oguro 0:866aafac0128 135 EN3=1;
oguro 0:866aafac0128 136
oguro 1:74d7a989b741 137 mypwmA.period_us(20);
oguro 0:866aafac0128 138
oguro 0:866aafac0128 139 mypwmB.period_us(20);
oguro 0:866aafac0128 140
oguro 0:866aafac0128 141 mypwmC.period_us(20);
oguro 0:866aafac0128 142
oguro 0:866aafac0128 143 Current_Ref.period_us(50);
oguro 0:866aafac0128 144 Current_Ref.write(0.55);
oguro 0:866aafac0128 145 uT.start();
oguro 0:866aafac0128 146 vT.start();
oguro 0:866aafac0128 147 wT.start();
oguro 0:866aafac0128 148
oguro 0:866aafac0128 149 RtosTimer RtosTimerTS1(timerTS1);
oguro 0:866aafac0128 150 RtosTimerTS1.start((unsigned int)(TS1*5000)); //3000
oguro 0:866aafac0128 151 Thread::wait(100);
oguro 0:866aafac0128 152
oguro 0:866aafac0128 153 while(1) {
oguro 0:866aafac0128 154
oguro 0:866aafac0128 155 Vr_adc=V_adc.read();
oguro 0:866aafac0128 156
oguro 0:866aafac0128 157 if((Vr_adc>0.15f)&&(q==0)){
oguro 0:866aafac0128 158
oguro 0:866aafac0128 159 while(q<50){
oguro 0:866aafac0128 160
oguro 3:cb3ad5a26986 161 mypwmA.write(0.0);
oguro 0:866aafac0128 162 mypwmB.write(0);
oguro 3:cb3ad5a26986 163 mypwmC.write(0.5f);
oguro 0:866aafac0128 164 wait_ms(START);
oguro 0:866aafac0128 165
oguro 0:866aafac0128 166 mypwmA.write(0);
oguro 0:866aafac0128 167 mypwmB.write(0.5f);
oguro 0:866aafac0128 168 mypwmC.write(0.0);
oguro 0:866aafac0128 169 wait_ms(START);
oguro 0:866aafac0128 170
oguro 3:cb3ad5a26986 171 mypwmA.write(0.5f);
oguro 0:866aafac0128 172 mypwmB.write(0.0);
oguro 3:cb3ad5a26986 173 mypwmC.write(0.0);
oguro 0:866aafac0128 174 wait_ms(START);
oguro 0:866aafac0128 175 q++;
oguro 0:866aafac0128 176
oguro 0:866aafac0128 177 }
oguro 0:866aafac0128 178 }
oguro 0:866aafac0128 179 //i++;
oguro 0:866aafac0128 180
oguro 0:866aafac0128 181 HA.rise(&HAH);
oguro 0:866aafac0128 182 HC.fall(&HCL);
oguro 0:866aafac0128 183 HB.rise(&HBH);
oguro 0:866aafac0128 184 HA.fall(&HAL);
oguro 0:866aafac0128 185 HC.rise(&HCH);
oguro 0:866aafac0128 186 HB.fall(&HBL);
oguro 1:74d7a989b741 187 Speed=60*(1/(7.0*2.0*wsi*1E-6));
oguro 0:866aafac0128 188
oguro 0:866aafac0128 189 if(Vr_adc < 0.08f){
oguro 0:866aafac0128 190 q=0;
oguro 0:866aafac0128 191 Vr_adc=0;
oguro 0:866aafac0128 192 r=0;
oguro 0:866aafac0128 193 vst=0;
oguro 0:866aafac0128 194 i=0;
oguro 0:866aafac0128 195 }
oguro 0:866aafac0128 196
oguro 0:866aafac0128 197
oguro 0:866aafac0128 198 ui=ui+1;
oguro 0:866aafac0128 199 vi=vi+1;
oguro 0:866aafac0128 200 wi=wi+1;
oguro 0:866aafac0128 201 et=et+1;
oguro 1:74d7a989b741 202
oguro 1:74d7a989b741 203 thu=2*PI*(1/(2*usi*1E-6))*ui*2.30E-4; //usi 1.3E-4 vst
oguro 1:74d7a989b741 204 thv=2*PI*(1/(2*vsi*1E-6))*vi*2.30E-4;
oguro 1:74d7a989b741 205 thw=2*PI*(1/(2*wsi*1E-6))*wi*2.30E-4;
oguro 1:74d7a989b741 206 //th=(thu+thv+thw)/3;
oguro 0:866aafac0128 207 if(r==0){
oguro 0:866aafac0128 208 thave=(thu+thv+thw)/3;
oguro 0:866aafac0128 209 th=thu;
oguro 1:74d7a989b741 210 //th=thave;
oguro 0:866aafac0128 211 s=0;
oguro 0:866aafac0128 212 }
oguro 0:866aafac0128 213 if(r==1){
oguro 0:866aafac0128 214 if((Speed > 1000)&&(Speed <=1500)){
oguro 0:866aafac0128 215 iso=-0.3;
oguro 2:0058dae8606d 216 Vqp=0.3*Vq; //0.2
oguro 1:74d7a989b741 217 Vdlink=0.4;
oguro 1:74d7a989b741 218 Edw=3500;
oguro 0:866aafac0128 219 z=0.7;
oguro 0:866aafac0128 220 }
oguro 0:866aafac0128 221 if((Speed > 1500)&&(Speed <=2000)){
oguro 0:866aafac0128 222 iso=0.7;
oguro 2:0058dae8606d 223 Vqp=0.3*Vq; //0.2
oguro 1:74d7a989b741 224 Vdlink=0.45;
oguro 1:74d7a989b741 225 Edw=3600;
oguro 0:866aafac0128 226 z=0.7;
oguro 0:866aafac0128 227 }
oguro 0:866aafac0128 228 if((Speed > 2000)&&(Speed <=3000)){
oguro 0:866aafac0128 229 iso=0.8;
oguro 2:0058dae8606d 230 Vqp=0.3*Vq; //0.2
oguro 1:74d7a989b741 231 Vdlink=0.5;
oguro 1:74d7a989b741 232 Edw=3700;
oguro 0:866aafac0128 233 z=0.7;
oguro 0:866aafac0128 234 }
oguro 0:866aafac0128 235 if((Speed > 3000)&&(Speed <=4000)){
oguro 1:74d7a989b741 236 iso=1.2; //1.2
oguro 1:74d7a989b741 237 Vqp=0.3*Vq; //0.3
oguro 1:74d7a989b741 238 Vdlink=0.5; //0.5
oguro 1:74d7a989b741 239 Edw=3700; //3700
oguro 0:866aafac0128 240 z=0.7;
oguro 0:866aafac0128 241 }
oguro 0:866aafac0128 242 if((Speed > 4000)&&(Speed <=5000)){
oguro 1:74d7a989b741 243 iso=1.8; //1.8
oguro 1:74d7a989b741 244 Vqp=0.3*Vq; //0.3
oguro 1:74d7a989b741 245 Vdlink=0.6; //0.6
oguro 1:74d7a989b741 246 Edw=3800; //3800
oguro 0:866aafac0128 247 z=0.7;
oguro 0:866aafac0128 248 }
oguro 0:866aafac0128 249 if((Speed > 5000)&&(Speed <=5500)){
oguro 1:74d7a989b741 250 iso=2.2; //2.2
oguro 1:74d7a989b741 251 Vqp=0.3*Vq; //0.3
oguro 1:74d7a989b741 252 Vdlink=0.5; //0.7
oguro 1:74d7a989b741 253 Edw=3900; //3900
oguro 1:74d7a989b741 254 z=0.7; //0.66
oguro 0:866aafac0128 255 }
oguro 0:866aafac0128 256 if((Speed > 5500)&&(Speed <=6000)){
oguro 1:74d7a989b741 257 iso=2.8; //2.2
oguro 1:74d7a989b741 258 Vqp=1.0*Vq; //0.35
oguro 1:74d7a989b741 259 Vdlink=0.5; //0.8
oguro 1:74d7a989b741 260 Edw=6000; //6000
oguro 1:74d7a989b741 261 z=0.8; //0.7
oguro 0:866aafac0128 262 }
oguro 0:866aafac0128 263 if((Speed > 6000)){
oguro 1:74d7a989b741 264 iso=2.2; //2.2
oguro 1:74d7a989b741 265 Vqp=1.0*Vq; // 0.4
oguro 1:74d7a989b741 266 Vdlink=0.5; //0.9
oguro 1:74d7a989b741 267 Edw=7000;//7000
oguro 1:74d7a989b741 268 z=0.8; //0.8
oguro 0:866aafac0128 269 }
oguro 1:74d7a989b741 270 // th=(Wz*et*1.9E-4)+iso; //1.99E-4
oguro 3:cb3ad5a26986 271 Vdlink=0.2;
oguro 3:cb3ad5a26986 272 z=0.4;
oguro 3:cb3ad5a26986 273 th=(W*et*1.1E-4)+iso; //1.9E-4
oguro 1:74d7a989b741 274 // th=Wo*wi*1.85E-4;
oguro 0:866aafac0128 275 }
oguro 0:866aafac0128 276
oguro 0:866aafac0128 277 if(r==0){
oguro 1:74d7a989b741 278 su=sin(thu);//-0.2f; // p51 0.5
oguro 1:74d7a989b741 279 sv=sin(thv);//-0.05f;
oguro 1:74d7a989b741 280 sw=sin(thw);//-0.05f;//-0.7
oguro 0:866aafac0128 281
oguro 1:74d7a989b741 282 mypwmA.write(su*Vr_adc); //kaisha sv ier su
oguro 1:74d7a989b741 283 mypwmB.write(sv*Vr_adc); //kaisha sw ie sv
oguro 1:74d7a989b741 284 mypwmC.write(sw*Vr_adc); //kaisha su ie sw
oguro 0:866aafac0128 285 }
oguro 0:866aafac0128 286 if(r==1){
oguro 1:74d7a989b741 287 mypwmA.write(du); // dw ie no white and Black
oguro 1:74d7a989b741 288 mypwmB.write(dv); // du
oguro 1:74d7a989b741 289 mypwmC.write(dw); // dv
oguro 0:866aafac0128 290 }
oguro 0:866aafac0128 291
oguro 0:866aafac0128 292
oguro 0:866aafac0128 293 myled = !myled;
oguro 0:866aafac0128 294 // }
oguro 0:866aafac0128 295 // }
oguro 0:866aafac0128 296
oguro 1:74d7a989b741 297 Curr_u=(Curr_ui);//0.33f; kaisha v w u narabikae
oguro 1:74d7a989b741 298 Curr_v=(Curr_vi);///0.33f;
oguro 1:74d7a989b741 299 Curr_w=(Curr_wi);///0.33f;
oguro 1:74d7a989b741 300 /* iuvw[0]=(Curr_ui);
oguro 1:74d7a989b741 301 iuvw[1]=(Curr_vi);
oguro 1:74d7a989b741 302 iuvw[2]=(Curr_wi);*/
oguro 0:866aafac0128 303 float Itau=1.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6;
oguro 0:866aafac0128 304 /****Filter Iu********/
oguro 0:866aafac0128 305 float Iu1,Iu2,Iu3,Iu4;//0.01
oguro 0:866aafac0128 306 Iu1=Idt*(Curr_u-iuvw[0])/Itau;
oguro 0:866aafac0128 307 Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau;
oguro 0:866aafac0128 308 Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau;
oguro 0:866aafac0128 309 Iu4=Idt*(Curr_u-(iuvw[0]+Iu3/2.0))/Itau;
oguro 0:866aafac0128 310 iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0;
oguro 0:866aafac0128 311 /*************************************/
oguro 0:866aafac0128 312 /****Filter Iv********/
oguro 0:866aafac0128 313 float Iv1,Iv2,Iv3,Iv4;//0.01
oguro 0:866aafac0128 314 Iv1=Idt*(Curr_v-iuvw[1])/Itau;
oguro 0:866aafac0128 315 Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau;
oguro 0:866aafac0128 316 Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau;
oguro 0:866aafac0128 317 Iv4=Idt*(Curr_v-(iuvw[1]+Iv3/2.0))/Itau;
oguro 0:866aafac0128 318 iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0;
oguro 0:866aafac0128 319 /*************************************/
oguro 0:866aafac0128 320 /****Filter Iw********/
oguro 0:866aafac0128 321 float Iw1,Iw2,Iw3,Iw4;//0.01
oguro 0:866aafac0128 322 Iw1=Idt*(Curr_w-iuvw[2])/Itau;
oguro 0:866aafac0128 323 Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau;
oguro 0:866aafac0128 324 Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau;
oguro 0:866aafac0128 325 Iw4=Idt*(Curr_w-(iuvw[2]+Iw3/2.0))/Itau;
oguro 0:866aafac0128 326 iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
oguro 0:866aafac0128 327 /*************************************/
oguro 1:74d7a989b741 328
oguro 0:866aafac0128 329 iab[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;
oguro 0:866aafac0128 330 iab[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
oguro 0:866aafac0128 331
oguro 0:866aafac0128 332
oguro 1:74d7a989b741 333 idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; //thave
oguro 1:74d7a989b741 334 idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; //th
oguro 0:866aafac0128 335
oguro 0:866aafac0128 336
oguro 0:866aafac0128 337 /****Filter Id********/
oguro 0:866aafac0128 338 idqo[0]=idq[0];
oguro 0:866aafac0128 339 float Id1,Id2,Id3,Id4;//0.01
oguro 0:866aafac0128 340 float Idtau= 1.0E-6,Iddt=1.0E-6; //1E-1
oguro 0:866aafac0128 341 Id1=Iddt*(idqi[0]-idqo[0])/Idtau;
oguro 0:866aafac0128 342 Id2=Iddt*(idqi[0]-(idqo[0]+Id1/2.0))/Idtau;
oguro 0:866aafac0128 343 Id3=Iddt*(idqi[0]-(idqo[0]+Id2/2.0))/Idtau;
oguro 0:866aafac0128 344 Id4=Iddt*(idqi[0]-(idqo[0]+Id3))/Idtau;
oguro 0:866aafac0128 345 idq[0]=idqo[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;
oguro 0:866aafac0128 346 /*************************************/
oguro 0:866aafac0128 347
oguro 0:866aafac0128 348 /****Filter Iq********/
oguro 0:866aafac0128 349 idqo[1]=idq[1];
oguro 0:866aafac0128 350 float Iq1,Iq2,Iq3,Iq4;//0.01
oguro 0:866aafac0128 351 float Iqtau= 1.0E-6,Iqdt=1.0E-6;
oguro 0:866aafac0128 352 Iq1=Iqdt*(idqi[1]-idqo[1])/Iqtau;
oguro 0:866aafac0128 353 Iq2=Iqdt*(idqi[1]-(idqo[1]+Iq1/2.0))/Iqtau;
oguro 0:866aafac0128 354 Iq3=Iqdt*(idqi[1]-(idqo[1]+Iq2/2.0))/Iqtau;
oguro 0:866aafac0128 355 Iq4=Iqdt*(idqi[1]-(idqo[1]+Iq3/2.0))/Iqtau;
oguro 0:866aafac0128 356 idq[1]=idqo[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;
oguro 0:866aafac0128 357 /*************************************/
oguro 1:74d7a989b741 358
oguro 1:74d7a989b741 359 // Wcom=(2*PI)/(vstf*1E-6);
oguro 1:74d7a989b741 360
oguro 1:74d7a989b741 361
oguro 1:74d7a989b741 362 // Vd=(idq[0]);//tmp vdq
oguro 1:74d7a989b741 363 // Vq=(idq[1]);//tmp vdq
oguro 1:74d7a989b741 364
oguro 0:866aafac0128 365 /*****PID Id　*****/
oguro 2:0058dae8606d 366 Idin=(-0.25)-idq[0]; // -0.25
oguro 0:866aafac0128 367 float adi,bdi,cdi,workdi[2];
oguro 1:74d7a989b741 368 float kpdi=1.0,kidi=0.5,kddi=0.0; //p51 0.5 1.2 0.0
oguro 0:866aafac0128 369 float dtdi=1.0E-6;//1E-6
oguro 0:866aafac0128 370 adi=Idin;
oguro 0:866aafac0128 371 bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi;
oguro 0:866aafac0128 372 cdi=(Idin-workdi[0])/dtdi;
oguro 0:866aafac0128 373 workdi[0]=Idin;
oguro 0:866aafac0128 374 workdi[1]=bdi;
oguro 0:866aafac0128 375 Vd=adi*kpdi+bdi*kidi+cdi*kddi;
oguro 0:866aafac0128 376 /**********************************/
oguro 1:74d7a989b741 377 //Icom=0.5;
oguro 0:866aafac0128 378 /*****PID Iq　*****/
oguro 1:74d7a989b741 379 Iqin=Vr_adc-idq[1]; //kaisha 600 ie 500
oguro 0:866aafac0128 380 float aqi,bqi,cqi,workqi[2];
oguro 0:866aafac0128 381 float kpqi=2.0,kiqi=1.2,kdqi=0.0; // 1.5 0.8
oguro 0:866aafac0128 382 float dtqi=1.0E-6;//1E-2
oguro 0:866aafac0128 383 aqi=Iqin;
oguro 0:866aafac0128 384 bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi;
oguro 0:866aafac0128 385 cqi=(Iqin-workqi[0])/dtqi;
oguro 0:866aafac0128 386 workqi[0]=Iqin;
oguro 0:866aafac0128 387 workdi[1]=bqi;
oguro 0:866aafac0128 388 Vq=aqi*kpqi+bqi*kiqi+cqi*kdqi;
oguro 0:866aafac0128 389 /**********************************/
oguro 1:74d7a989b741 390 /****Filter Vq********/
oguro 1:74d7a989b741 391 /*float Vq1,Vq2,Vq3,Vq4;//0.01
oguro 1:74d7a989b741 392 float Vqtau= 1.0E-6,Vqdt=1.0E-6;
oguro 1:74d7a989b741 393 Vq1=Vqdt*(Vqi-Vq)/Vqtau;
oguro 1:74d7a989b741 394 Vq2=Vqdt*(Vqi-(Vq+Vq1/2.0))/Vqtau;
oguro 1:74d7a989b741 395 Vq3=Vqdt*(Vqi-(Vq+Vq2/2.0))/Vqtau;
oguro 1:74d7a989b741 396 Vq4=Vqdt*(Vqi-(Vq+Vq3/2.0))/Vqtau;
oguro 1:74d7a989b741 397 Vq=Vq+(Vq1+2.0*Vq2+2.0*Vq3+Vq4)/6.0;*/
oguro 1:74d7a989b741 398 /*************************************/
oguro 1:74d7a989b741 399 //Vq=Vr_adc;
oguro 0:866aafac0128 400 //if(i<10000){
oguro 0:866aafac0128 401 //if(r==0){
oguro 0:866aafac0128 402 usi=ut2-ut1;
oguro 0:866aafac0128 403 vsi=vt2-vt1;
oguro 0:866aafac0128 404 wsi=wt2-wt1;
oguro 0:866aafac0128 405 if(i<10000){
oguro 0:866aafac0128 406 vst=vsi;
oguro 0:866aafac0128 407 vstf=vst;
oguro 0:866aafac0128 408 }
oguro 0:866aafac0128 409 //}
oguro 0:866aafac0128 410 else { //kokokara else
oguro 0:866aafac0128 411 //if(r==1){
oguro 0:866aafac0128 412 // usi=vstf;vsi=vstf;wsi=vstf;
oguro 0:866aafac0128 413 vst=vstf;
oguro 0:866aafac0128 414
oguro 0:866aafac0128 415 i=10000;
oguro 0:866aafac0128 416 // }
oguro 0:866aafac0128 417 // r=i%100;
oguro 0:866aafac0128 418 // i++;
oguro 0:866aafac0128 419
oguro 1:74d7a989b741 420 // Vd=(idq[0]);//tmp vdq
oguro 1:74d7a989b741 421 // Vq=(idq[1]);//tmp vdq
oguro 1:74d7a989b741 422
oguro 1:74d7a989b741 423
oguro 1:74d7a989b741 424 /* Ed= (Vd)-0.11f*idq[0]+Wz*0.018E-3*idq[1];//0.018
oguro 1:74d7a989b741 425
oguro 1:74d7a989b741 426 if(Ed>=0){
oguro 1:74d7a989b741 427 Wz=(2*PI)/((vst+Edw*Ed*0.2)*1E-6);// 0.2
oguro 1:74d7a989b741 428 }
oguro 1:74d7a989b741 429 else{
oguro 1:74d7a989b741 430 Wz=(2*PI)/((vst-Edw*Ed*0.2)*1E-6); //0.45
oguro 1:74d7a989b741 431 } */
oguro 0:866aafac0128 432
oguro 0:866aafac0128 433 Wz=(2*PI)/(vst*1E-6); //vst
oguro 0:866aafac0128 434 Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3
oguro 0:866aafac0128 435 phm=Ed/(Wz);
oguro 0:866aafac0128 436 dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); //0.018E-3
oguro 0:866aafac0128 437 eth=asin(dth);
oguro 0:866aafac0128 438
oguro 0:866aafac0128 439 /*****PID θ　*****/
oguro 1:74d7a989b741 440 // PLL=(Speed/100); //7 21 28
oguro 1:74d7a989b741 441 //PLL=Speed*2*PI/(60*28);
oguro 1:74d7a989b741 442 // PLL=W/Wz;
oguro 1:74d7a989b741 443 PLL=1.0; //30
oguro 1:74d7a989b741 444 //float WPLL=150.0; // 170
oguro 0:866aafac0128 445 float as,bs,cs,works[2];
oguro 0:866aafac0128 446 float kps=PLL,kis=PLL*PLL/5.0,kds=0.0;
oguro 0:866aafac0128 447 float dts=1.0E-6;
oguro 0:866aafac0128 448 Xsi=5.0-eth;
oguro 0:866aafac0128 449 as=Xsi;
oguro 0:866aafac0128 450 bs=works[1]+(Xsi+works[0])/2.0*dts;
oguro 0:866aafac0128 451 cs=(Xsi-works[0])/dts;
oguro 0:866aafac0128 452 works[0]=Xsi;
oguro 0:866aafac0128 453 works[1]=bs;
oguro 0:866aafac0128 454 therr=as*kps+bs*kis+cs*kds;
oguro 0:866aafac0128 455 /*******PLL W ***********/
oguro 0:866aafac0128 456 if(therr>0.01){
oguro 0:866aafac0128 457 W=(2*PI)/((vst*1E-6)+(therr/Wz));
oguro 0:866aafac0128 458 }
oguro 0:866aafac0128 459 if(therr<-0.01){
oguro 0:866aafac0128 460 W=(2*PI)/((vst*1E-6)-(therr/Wz));
oguro 0:866aafac0128 461 }
oguro 0:866aafac0128 462
oguro 0:866aafac0128 463 /*****PID ω　*****/
oguro 3:cb3ad5a26986 464 Xin=5500*(1.15-Vr_adc)-vst; // 5500
oguro 0:866aafac0128 465 float a,b,c,work[2];
oguro 2:0058dae8606d 466 float kp=2.0,ki=0.5,kd=0.0; // 2.0 0.5
oguro 2:0058dae8606d 467 float dt=10.0E-6;//10E-6
oguro 0:866aafac0128 468 a=Xin;
oguro 0:866aafac0128 469 b=work[1]+(Xin+work[0])/2.0*dt;
oguro 0:866aafac0128 470 c=(Xin-work[0])/dt;
oguro 0:866aafac0128 471 work[0]=Xin;
oguro 0:866aafac0128 472 work[1]=b;
oguro 0:866aafac0128 473 Xout=a*kp+b*ki+c*kd;
oguro 1:74d7a989b741 474 /**********************************/
oguro 1:74d7a989b741 475 // float dtt=1000.0E-6;
oguro 1:74d7a989b741 476
oguro 1:74d7a989b741 477 /*float k1,k2,k3,k4,tau=0.001;//0.01
oguro 1:74d7a989b741 478 k1=dtt*(Xout-vstt)/tau;
oguro 1:74d7a989b741 479 k2=dtt*(Xout-(vstt+k1/2.0))/tau;
oguro 1:74d7a989b741 480 k3=dtt*(Xout-(vstt+k2/2.0))/tau;
oguro 1:74d7a989b741 481 k4=dtt*(Xout-(vstt+k3/2.0))/tau;
oguro 1:74d7a989b741 482 vstt=vstt+(k1+2.0*k2+2.0*k3+k4)/6.0;*/
oguro 0:866aafac0128 483 vstt=Xout;
oguro 0:866aafac0128 484 /********************************/
oguro 3:cb3ad5a26986 485 if(3500>abs(vst-vstt)){ // ie 1000
oguro 1:74d7a989b741 486 vsti=vstt;
oguro 0:866aafac0128 487 // vsti=vst;
oguro 0:866aafac0128 488 r=1;
oguro 0:866aafac0128 489 }
oguro 0:866aafac0128 490 /*else{
oguro 0:866aafac0128 491 vsti=vst;
oguro 0:866aafac0128 492 r=0;
oguro 0:866aafac0128 493 }*/
oguro 0:866aafac0128 494 /****Filter********/
oguro 0:866aafac0128 495 float vstfo=vstf;
oguro 2:0058dae8606d 496 float dttt=10.0E-6;//100E-6
oguro 1:74d7a989b741 497 float k11,k22,k33,k44,tau1=0.1;//0.01
oguro 0:866aafac0128 498 k11=dttt*(vsti-vstfo)/tau1;
oguro 0:866aafac0128 499 k22=dttt*(vsti-(vstfo+k11/2.0))/tau1;
oguro 0:866aafac0128 500 k33=dttt*(vsti-(vstfo+k22/2.0))/tau1;
oguro 0:866aafac0128 501 k44=dttt*(vsti-(vstfo+k33))/tau1;
oguro 0:866aafac0128 502 vstf=vstfo+(k11+2.0*k22+2.0*k33+k44)/6.0;
oguro 0:866aafac0128 503 //vstf=vsti;
oguro 0:866aafac0128 504 /*************************************/
oguro 0:866aafac0128 505
oguro 0:866aafac0128 506
oguro 0:866aafac0128 507
oguro 0:866aafac0128 508 }//else kokomade
oguro 1:74d7a989b741 509 /* if((Vr_adc > 0.9f)&&(Vr_adc <= 0.95f)){
oguro 1:74d7a989b741 510 vstf =800.0f;
oguro 1:74d7a989b741 511 }
oguro 1:74d7a989b741 512 if((Vr_adc > 0.95f)&&(Vr_adc < 0.975f)){
oguro 1:74d7a989b741 513 vstf=700.0f;
oguro 1:74d7a989b741 514 }
oguro 1:74d7a989b741 515 if(Vr_adc >= 0.975f){
oguro 1:74d7a989b741 516 vstf=600.0f;
oguro 1:74d7a989b741 517 } */
oguro 1:74d7a989b741 518
oguro 0:866aafac0128 519
oguro 0:866aafac0128 520
oguro 1:74d7a989b741 521 Va=cos(th)*Vd-sin(th)*Vqp; //Vqp 負荷小　Vq*0.8～1.0負荷大
oguro 1:74d7a989b741 522 Vb=sin(th)*Vd+cos(th)*Vqp; //Vqp
oguro 0:866aafac0128 523
oguro 0:866aafac0128 524 aVa=abs(Va);
oguro 0:866aafac0128 525 a3Vb=abs(sq3*Vb);
oguro 1:74d7a989b741 526
oguro 1:74d7a989b741 527 if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0
oguro 0:866aafac0128 528
oguro 0:866aafac0128 529 d1=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:866aafac0128 530 d2=sq32*(sq23*Vb)*Vdlink;
oguro 1:74d7a989b741 531 d07=z-(d1+d2);
oguro 0:866aafac0128 532 du=d1+d2+d07;
oguro 0:866aafac0128 533 dv=d2+d07;
oguro 0:866aafac0128 534 dw=d07;
oguro 0:866aafac0128 535 }
oguro 0:866aafac0128 536 if((aVa<=sq3*Vb)){ //sect 1
oguro 0:866aafac0128 537
oguro 0:866aafac0128 538 d3=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:866aafac0128 539 d2=sq32*(Va+sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 540 d07=z-(d2+d3);
oguro 0:866aafac0128 541 du=d2+d07;
oguro 0:866aafac0128 542 dv=d2+d3+d07;
oguro 0:866aafac0128 543 dw=d07;
oguro 0:866aafac0128 544 }
oguro 0:866aafac0128 545
oguro 0:866aafac0128 546 if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2
oguro 0:866aafac0128 547
oguro 0:866aafac0128 548 d3=sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 549 d4=sq32*(-Va-sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 550 d07=z-(d3+d4);
oguro 0:866aafac0128 551 du=d07;
oguro 1:74d7a989b741 552 dv=d3+d2+d07;
oguro 0:866aafac0128 553 dw=d4+d07;
oguro 0:866aafac0128 554 }
oguro 0:866aafac0128 555
oguro 0:866aafac0128 556 if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3
oguro 0:866aafac0128 557
oguro 0:866aafac0128 558 d5=-sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 559 d4=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 560 d07=z-(d4+d5);
oguro 0:866aafac0128 561 du=d07;
oguro 0:866aafac0128 562 dv=d4+d07;
oguro 0:866aafac0128 563 dw=d4+d5+d07;
oguro 0:866aafac0128 564 }
oguro 0:866aafac0128 565
oguro 0:866aafac0128 566 if((aVa<=-sq3*Vb)){ //sect 4
oguro 0:866aafac0128 567
oguro 0:866aafac0128 568 d5=sq32*(-Va-sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 569 d6=sq32*(Va-sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 570 d07=z-(d5+d6);
oguro 0:866aafac0128 571 du=d6+d07;
oguro 0:866aafac0128 572 dv=d07;
oguro 0:866aafac0128 573 dw=d5+d6+d07;
oguro 0:866aafac0128 574 }
oguro 0:866aafac0128 575
oguro 0:866aafac0128 576 if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5
oguro 0:866aafac0128 577
oguro 0:866aafac0128 578 d1=sq32*(Va+sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 579 d6=-sq32*sq23*Vb*Vdlink;;
oguro 1:74d7a989b741 580 d07=z-(d1+d6);
oguro 0:866aafac0128 581 du=d1+d6+d07;
oguro 0:866aafac0128 582 dv=d07;
oguro 0:866aafac0128 583 dw=d6+d07;
oguro 0:866aafac0128 584 }
oguro 1:74d7a989b741 585
oguro 1:74d7a989b741 586 /* mypwmA.write(du);
oguro 1:74d7a989b741 587 mypwmB.write(dv);
oguro 1:74d7a989b741 588 mypwmC.write(dw);*/
oguro 1:74d7a989b741 589
oguro 1:74d7a989b741 590
oguro 1:74d7a989b741 591 /* suv=Vab[0]*zet;
oguro 1:74d7a989b741 592 svv=(Vab[0]*cos23+Vab[1]*sin23)*zet;
oguro 1:74d7a989b741 593 swv=(Vab[0]*cos43+Vab[1]*sin43)*zet;*/
oguro 0:866aafac0128 594
oguro 0:866aafac0128 595 // SWAVE=Vd;
oguro 0:866aafac0128 596 // SWAVE=th/4;
oguro 0:866aafac0128 597 // SWAVE=Vq;
oguro 0:866aafac0128 598 // SWAVE=West/5000;
oguro 0:866aafac0128 599 //SWAVE=Wo/5000;
oguro 0:866aafac0128 600 //SWAVE=Wz/5000;
oguro 0:866aafac0128 601 // SWAVE=sin(suv);
oguro 0:866aafac0128 602 // SWAVE=iab[0]+0.5;
oguro 0:866aafac0128 603 // SWAVE=Ed/10;
oguro 0:866aafac0128 604 // SWAVE=idq[1];
oguro 0:866aafac0128 605 // SWAVE=sin(th);
oguro 0:866aafac0128 606 // SWAVE=vst/5000;
oguro 0:866aafac0128 607 //SWAVE=Vb;
oguro 3:cb3ad5a26986 608 //SWAVE=iuvw[0];
oguro 3:cb3ad5a26986 609 SWAVE=du;
oguro 0:866aafac0128 610 Vector=r;
oguro 0:866aafac0128 611 i++;
oguro 0:866aafac0128 612 }
oguro 0:866aafac0128 613 }