akiyoshi oguro / Mbed 2 deprecated Vector_SVPWM_drive_F302R8

Vector SVPWM Drive SPMSM

Dependencies:   mbed mbed-rtos

main.cpp@2:0058dae8606d, 2019-06-08 (annotated)

Committer:
oguro
Date:
Sat Jun 08 03:52:20 2019 +0000
Revision:
2:0058dae8606d
Parent:
1:74d7a989b741
Child:
3:cb3ad5a26986
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 1:74d7a989b741 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 0:866aafac0128 161 mypwmA.write(0.5f);
oguro 0:866aafac0128 162 mypwmB.write(0);
oguro 0:866aafac0128 163 mypwmC.write(0);
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 0:866aafac0128 171 mypwmA.write(0);
oguro 0:866aafac0128 172 mypwmB.write(0.0);
oguro 0:866aafac0128 173 mypwmC.write(0.5f);
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 2:0058dae8606d 271 th=(W*et*1.9E-4)+iso; //1.9E-4
oguro 1:74d7a989b741 272 // th=Wo*wi*1.85E-4;
oguro 0:866aafac0128 273 }
oguro 0:866aafac0128 274
oguro 0:866aafac0128 275 if(r==0){
oguro 1:74d7a989b741 276 su=sin(thu);//-0.2f; // p51 0.5
oguro 1:74d7a989b741 277 sv=sin(thv);//-0.05f;
oguro 1:74d7a989b741 278 sw=sin(thw);//-0.05f;//-0.7
oguro 0:866aafac0128 279
oguro 1:74d7a989b741 280 mypwmA.write(su*Vr_adc); //kaisha sv ier su
oguro 1:74d7a989b741 281 mypwmB.write(sv*Vr_adc); //kaisha sw ie sv
oguro 1:74d7a989b741 282 mypwmC.write(sw*Vr_adc); //kaisha su ie sw
oguro 0:866aafac0128 283 }
oguro 0:866aafac0128 284 if(r==1){
oguro 1:74d7a989b741 285 mypwmA.write(du); // dw ie no white and Black
oguro 1:74d7a989b741 286 mypwmB.write(dv); // du
oguro 1:74d7a989b741 287 mypwmC.write(dw); // dv
oguro 0:866aafac0128 288 }
oguro 0:866aafac0128 289
oguro 0:866aafac0128 290
oguro 0:866aafac0128 291 myled = !myled;
oguro 0:866aafac0128 292 // }
oguro 0:866aafac0128 293 // }
oguro 0:866aafac0128 294
oguro 1:74d7a989b741 295 Curr_u=(Curr_ui);//0.33f; kaisha v w u narabikae
oguro 1:74d7a989b741 296 Curr_v=(Curr_vi);///0.33f;
oguro 1:74d7a989b741 297 Curr_w=(Curr_wi);///0.33f;
oguro 1:74d7a989b741 298 /* iuvw[0]=(Curr_ui);
oguro 1:74d7a989b741 299 iuvw[1]=(Curr_vi);
oguro 1:74d7a989b741 300 iuvw[2]=(Curr_wi);*/
oguro 0:866aafac0128 301 float Itau=1.0E-6,Idt=1.0E-6; // Itau=1.0E-6,Idt=1.0E-6;
oguro 0:866aafac0128 302 /****Filter Iu********/
oguro 0:866aafac0128 303 float Iu1,Iu2,Iu3,Iu4;//0.01
oguro 0:866aafac0128 304 Iu1=Idt*(Curr_u-iuvw[0])/Itau;
oguro 0:866aafac0128 305 Iu2=Idt*(Curr_u-(iuvw[0]+Iu1/2.0))/Itau;
oguro 0:866aafac0128 306 Iu3=Idt*(Curr_u-(iuvw[0]+Iu2/2.0))/Itau;
oguro 0:866aafac0128 307 Iu4=Idt*(Curr_u-(iuvw[0]+Iu3/2.0))/Itau;
oguro 0:866aafac0128 308 iuvw[0]=iuvw[0]+(Iu1+2.0*Iu2+2.0*Iu3+Iu4)/6.0;
oguro 0:866aafac0128 309 /*************************************/
oguro 0:866aafac0128 310 /****Filter Iv********/
oguro 0:866aafac0128 311 float Iv1,Iv2,Iv3,Iv4;//0.01
oguro 0:866aafac0128 312 Iv1=Idt*(Curr_v-iuvw[1])/Itau;
oguro 0:866aafac0128 313 Iv2=Idt*(Curr_v-(iuvw[1]+Iv1/2.0))/Itau;
oguro 0:866aafac0128 314 Iv3=Idt*(Curr_v-(iuvw[1]+Iv2/2.0))/Itau;
oguro 0:866aafac0128 315 Iv4=Idt*(Curr_v-(iuvw[1]+Iv3/2.0))/Itau;
oguro 0:866aafac0128 316 iuvw[1]=iuvw[1]+(Iv1+2.0*Iv2+2.0*Iv3+Iv4)/6.0;
oguro 0:866aafac0128 317 /*************************************/
oguro 0:866aafac0128 318 /****Filter Iw********/
oguro 0:866aafac0128 319 float Iw1,Iw2,Iw3,Iw4;//0.01
oguro 0:866aafac0128 320 Iw1=Idt*(Curr_w-iuvw[2])/Itau;
oguro 0:866aafac0128 321 Iw2=Idt*(Curr_w-(iuvw[2]+Iw1/2.0))/Itau;
oguro 0:866aafac0128 322 Iw3=Idt*(Curr_w-(iuvw[2]+Iw2/2.0))/Itau;
oguro 0:866aafac0128 323 Iw4=Idt*(Curr_w-(iuvw[2]+Iw3/2.0))/Itau;
oguro 0:866aafac0128 324 iuvw[2]=iuvw[2]+(Iw1+2.0*Iw2+2.0*Iw3+Iw4)/6.0;
oguro 0:866aafac0128 325 /*************************************/
oguro 1:74d7a989b741 326
oguro 0:866aafac0128 327 iab[0]=(iuvw[0]+iuvw[1]*cos23+iuvw[2]*cos43)*zet;
oguro 0:866aafac0128 328 iab[1]=(iuvw[1]*sin23+iuvw[2]*sin43)*zet;
oguro 0:866aafac0128 329
oguro 0:866aafac0128 330
oguro 1:74d7a989b741 331 idq[0]=cos(th)*iab[0]+sin(th)*iab[1]; //thave
oguro 1:74d7a989b741 332 idq[1]=-sin(th)*iab[0]+cos(th)*iab[1]; //th
oguro 0:866aafac0128 333
oguro 0:866aafac0128 334
oguro 0:866aafac0128 335 /****Filter Id********/
oguro 0:866aafac0128 336 idqo[0]=idq[0];
oguro 0:866aafac0128 337 float Id1,Id2,Id3,Id4;//0.01
oguro 0:866aafac0128 338 float Idtau= 1.0E-6,Iddt=1.0E-6; //1E-1
oguro 0:866aafac0128 339 Id1=Iddt*(idqi[0]-idqo[0])/Idtau;
oguro 0:866aafac0128 340 Id2=Iddt*(idqi[0]-(idqo[0]+Id1/2.0))/Idtau;
oguro 0:866aafac0128 341 Id3=Iddt*(idqi[0]-(idqo[0]+Id2/2.0))/Idtau;
oguro 0:866aafac0128 342 Id4=Iddt*(idqi[0]-(idqo[0]+Id3))/Idtau;
oguro 0:866aafac0128 343 idq[0]=idqo[0]+(Id1+2.0*Id2+2.0*Id3+Id4)/6.0;
oguro 0:866aafac0128 344 /*************************************/
oguro 0:866aafac0128 345
oguro 0:866aafac0128 346 /****Filter Iq********/
oguro 0:866aafac0128 347 idqo[1]=idq[1];
oguro 0:866aafac0128 348 float Iq1,Iq2,Iq3,Iq4;//0.01
oguro 0:866aafac0128 349 float Iqtau= 1.0E-6,Iqdt=1.0E-6;
oguro 0:866aafac0128 350 Iq1=Iqdt*(idqi[1]-idqo[1])/Iqtau;
oguro 0:866aafac0128 351 Iq2=Iqdt*(idqi[1]-(idqo[1]+Iq1/2.0))/Iqtau;
oguro 0:866aafac0128 352 Iq3=Iqdt*(idqi[1]-(idqo[1]+Iq2/2.0))/Iqtau;
oguro 0:866aafac0128 353 Iq4=Iqdt*(idqi[1]-(idqo[1]+Iq3/2.0))/Iqtau;
oguro 0:866aafac0128 354 idq[1]=idqo[1]+(Iq1+2.0*Iq2+2.0*Iq3+Iq4)/6.0;
oguro 0:866aafac0128 355 /*************************************/
oguro 1:74d7a989b741 356
oguro 1:74d7a989b741 357 // Wcom=(2*PI)/(vstf*1E-6);
oguro 1:74d7a989b741 358
oguro 1:74d7a989b741 359
oguro 1:74d7a989b741 360 // Vd=(idq[0]);//tmp vdq
oguro 1:74d7a989b741 361 // Vq=(idq[1]);//tmp vdq
oguro 1:74d7a989b741 362
oguro 0:866aafac0128 363 /*****PID Id　*****/
oguro 2:0058dae8606d 364 Idin=(-0.25)-idq[0]; // -0.25
oguro 0:866aafac0128 365 float adi,bdi,cdi,workdi[2];
oguro 1:74d7a989b741 366 float kpdi=1.0,kidi=0.5,kddi=0.0; //p51 0.5 1.2 0.0
oguro 0:866aafac0128 367 float dtdi=1.0E-6;//1E-6
oguro 0:866aafac0128 368 adi=Idin;
oguro 0:866aafac0128 369 bdi=workdi[1]+(Idin+workdi[0])/2.0*dtdi;
oguro 0:866aafac0128 370 cdi=(Idin-workdi[0])/dtdi;
oguro 0:866aafac0128 371 workdi[0]=Idin;
oguro 0:866aafac0128 372 workdi[1]=bdi;
oguro 0:866aafac0128 373 Vd=adi*kpdi+bdi*kidi+cdi*kddi;
oguro 0:866aafac0128 374 /**********************************/
oguro 1:74d7a989b741 375 //Icom=0.5;
oguro 0:866aafac0128 376 /*****PID Iq　*****/
oguro 1:74d7a989b741 377 Iqin=Vr_adc-idq[1]; //kaisha 600 ie 500
oguro 0:866aafac0128 378 float aqi,bqi,cqi,workqi[2];
oguro 0:866aafac0128 379 float kpqi=2.0,kiqi=1.2,kdqi=0.0; // 1.5 0.8
oguro 0:866aafac0128 380 float dtqi=1.0E-6;//1E-2
oguro 0:866aafac0128 381 aqi=Iqin;
oguro 0:866aafac0128 382 bqi=workqi[1]+(Iqin+workqi[0])/2.0*dtqi;
oguro 0:866aafac0128 383 cqi=(Iqin-workqi[0])/dtqi;
oguro 0:866aafac0128 384 workqi[0]=Iqin;
oguro 0:866aafac0128 385 workdi[1]=bqi;
oguro 0:866aafac0128 386 Vq=aqi*kpqi+bqi*kiqi+cqi*kdqi;
oguro 0:866aafac0128 387 /**********************************/
oguro 1:74d7a989b741 388 /****Filter Vq********/
oguro 1:74d7a989b741 389 /*float Vq1,Vq2,Vq3,Vq4;//0.01
oguro 1:74d7a989b741 390 float Vqtau= 1.0E-6,Vqdt=1.0E-6;
oguro 1:74d7a989b741 391 Vq1=Vqdt*(Vqi-Vq)/Vqtau;
oguro 1:74d7a989b741 392 Vq2=Vqdt*(Vqi-(Vq+Vq1/2.0))/Vqtau;
oguro 1:74d7a989b741 393 Vq3=Vqdt*(Vqi-(Vq+Vq2/2.0))/Vqtau;
oguro 1:74d7a989b741 394 Vq4=Vqdt*(Vqi-(Vq+Vq3/2.0))/Vqtau;
oguro 1:74d7a989b741 395 Vq=Vq+(Vq1+2.0*Vq2+2.0*Vq3+Vq4)/6.0;*/
oguro 1:74d7a989b741 396 /*************************************/
oguro 1:74d7a989b741 397 //Vq=Vr_adc;
oguro 0:866aafac0128 398 //if(i<10000){
oguro 0:866aafac0128 399 //if(r==0){
oguro 0:866aafac0128 400 usi=ut2-ut1;
oguro 0:866aafac0128 401 vsi=vt2-vt1;
oguro 0:866aafac0128 402 wsi=wt2-wt1;
oguro 0:866aafac0128 403 if(i<10000){
oguro 0:866aafac0128 404 vst=vsi;
oguro 0:866aafac0128 405 vstf=vst;
oguro 0:866aafac0128 406 }
oguro 0:866aafac0128 407 //}
oguro 0:866aafac0128 408 else { //kokokara else
oguro 0:866aafac0128 409 //if(r==1){
oguro 0:866aafac0128 410 // usi=vstf;vsi=vstf;wsi=vstf;
oguro 0:866aafac0128 411 vst=vstf;
oguro 0:866aafac0128 412
oguro 0:866aafac0128 413 i=10000;
oguro 0:866aafac0128 414 // }
oguro 0:866aafac0128 415 // r=i%100;
oguro 0:866aafac0128 416 // i++;
oguro 0:866aafac0128 417
oguro 1:74d7a989b741 418 // Vd=(idq[0]);//tmp vdq
oguro 1:74d7a989b741 419 // Vq=(idq[1]);//tmp vdq
oguro 1:74d7a989b741 420
oguro 1:74d7a989b741 421
oguro 1:74d7a989b741 422 /* Ed= (Vd)-0.11f*idq[0]+Wz*0.018E-3*idq[1];//0.018
oguro 1:74d7a989b741 423
oguro 1:74d7a989b741 424 if(Ed>=0){
oguro 1:74d7a989b741 425 Wz=(2*PI)/((vst+Edw*Ed*0.2)*1E-6);// 0.2
oguro 1:74d7a989b741 426 }
oguro 1:74d7a989b741 427 else{
oguro 1:74d7a989b741 428 Wz=(2*PI)/((vst-Edw*Ed*0.2)*1E-6); //0.45
oguro 1:74d7a989b741 429 } */
oguro 0:866aafac0128 430
oguro 0:866aafac0128 431 Wz=(2*PI)/(vst*1E-6); //vst
oguro 0:866aafac0128 432 Ed= (Vq)-0.11f*idq[1]-Wz*0.018E-3*idq[0];//0.018E-3
oguro 0:866aafac0128 433 phm=Ed/(Wz);
oguro 0:866aafac0128 434 dth=(Vd-0.11f*(idq[0])+Wz*0.018E-3*(idq[1]))/(Wz*phm); //0.018E-3
oguro 0:866aafac0128 435 eth=asin(dth);
oguro 0:866aafac0128 436
oguro 0:866aafac0128 437 /*****PID θ　*****/
oguro 1:74d7a989b741 438 // PLL=(Speed/100); //7 21 28
oguro 1:74d7a989b741 439 //PLL=Speed*2*PI/(60*28);
oguro 1:74d7a989b741 440 // PLL=W/Wz;
oguro 1:74d7a989b741 441 PLL=1.0; //30
oguro 1:74d7a989b741 442 //float WPLL=150.0; // 170
oguro 0:866aafac0128 443 float as,bs,cs,works[2];
oguro 0:866aafac0128 444 float kps=PLL,kis=PLL*PLL/5.0,kds=0.0;
oguro 0:866aafac0128 445 float dts=1.0E-6;
oguro 0:866aafac0128 446 Xsi=5.0-eth;
oguro 0:866aafac0128 447 as=Xsi;
oguro 0:866aafac0128 448 bs=works[1]+(Xsi+works[0])/2.0*dts;
oguro 0:866aafac0128 449 cs=(Xsi-works[0])/dts;
oguro 0:866aafac0128 450 works[0]=Xsi;
oguro 0:866aafac0128 451 works[1]=bs;
oguro 0:866aafac0128 452 therr=as*kps+bs*kis+cs*kds;
oguro 0:866aafac0128 453 /*******PLL W ***********/
oguro 0:866aafac0128 454 if(therr>0.01){
oguro 0:866aafac0128 455 W=(2*PI)/((vst*1E-6)+(therr/Wz));
oguro 0:866aafac0128 456 }
oguro 0:866aafac0128 457 if(therr<-0.01){
oguro 0:866aafac0128 458 W=(2*PI)/((vst*1E-6)-(therr/Wz));
oguro 0:866aafac0128 459 }
oguro 0:866aafac0128 460
oguro 0:866aafac0128 461 /*****PID ω　*****/
oguro 2:0058dae8606d 462 Xin=5000*(1.05-Vr_adc)-vst; // 5500
oguro 0:866aafac0128 463 float a,b,c,work[2];
oguro 2:0058dae8606d 464 float kp=2.0,ki=0.5,kd=0.0; // 2.0 0.5
oguro 2:0058dae8606d 465 float dt=10.0E-6;//10E-6
oguro 0:866aafac0128 466 a=Xin;
oguro 0:866aafac0128 467 b=work[1]+(Xin+work[0])/2.0*dt;
oguro 0:866aafac0128 468 c=(Xin-work[0])/dt;
oguro 0:866aafac0128 469 work[0]=Xin;
oguro 0:866aafac0128 470 work[1]=b;
oguro 0:866aafac0128 471 Xout=a*kp+b*ki+c*kd;
oguro 1:74d7a989b741 472 /**********************************/
oguro 1:74d7a989b741 473 // float dtt=1000.0E-6;
oguro 1:74d7a989b741 474
oguro 1:74d7a989b741 475 /*float k1,k2,k3,k4,tau=0.001;//0.01
oguro 1:74d7a989b741 476 k1=dtt*(Xout-vstt)/tau;
oguro 1:74d7a989b741 477 k2=dtt*(Xout-(vstt+k1/2.0))/tau;
oguro 1:74d7a989b741 478 k3=dtt*(Xout-(vstt+k2/2.0))/tau;
oguro 1:74d7a989b741 479 k4=dtt*(Xout-(vstt+k3/2.0))/tau;
oguro 1:74d7a989b741 480 vstt=vstt+(k1+2.0*k2+2.0*k3+k4)/6.0;*/
oguro 0:866aafac0128 481 vstt=Xout;
oguro 0:866aafac0128 482 /********************************/
oguro 0:866aafac0128 483 if(2000>abs(vst-vstt)){ // ie 1000
oguro 1:74d7a989b741 484 vsti=vstt;
oguro 0:866aafac0128 485 // vsti=vst;
oguro 0:866aafac0128 486 r=1;
oguro 0:866aafac0128 487 }
oguro 0:866aafac0128 488 /*else{
oguro 0:866aafac0128 489 vsti=vst;
oguro 0:866aafac0128 490 r=0;
oguro 0:866aafac0128 491 }*/
oguro 0:866aafac0128 492 /****Filter********/
oguro 0:866aafac0128 493 float vstfo=vstf;
oguro 2:0058dae8606d 494 float dttt=10.0E-6;//100E-6
oguro 1:74d7a989b741 495 float k11,k22,k33,k44,tau1=0.1;//0.01
oguro 0:866aafac0128 496 k11=dttt*(vsti-vstfo)/tau1;
oguro 0:866aafac0128 497 k22=dttt*(vsti-(vstfo+k11/2.0))/tau1;
oguro 0:866aafac0128 498 k33=dttt*(vsti-(vstfo+k22/2.0))/tau1;
oguro 0:866aafac0128 499 k44=dttt*(vsti-(vstfo+k33))/tau1;
oguro 0:866aafac0128 500 vstf=vstfo+(k11+2.0*k22+2.0*k33+k44)/6.0;
oguro 0:866aafac0128 501 //vstf=vsti;
oguro 0:866aafac0128 502 /*************************************/
oguro 0:866aafac0128 503
oguro 0:866aafac0128 504
oguro 0:866aafac0128 505
oguro 0:866aafac0128 506 }//else kokomade
oguro 1:74d7a989b741 507 /* if((Vr_adc > 0.9f)&&(Vr_adc <= 0.95f)){
oguro 1:74d7a989b741 508 vstf =800.0f;
oguro 1:74d7a989b741 509 }
oguro 1:74d7a989b741 510 if((Vr_adc > 0.95f)&&(Vr_adc < 0.975f)){
oguro 1:74d7a989b741 511 vstf=700.0f;
oguro 1:74d7a989b741 512 }
oguro 1:74d7a989b741 513 if(Vr_adc >= 0.975f){
oguro 1:74d7a989b741 514 vstf=600.0f;
oguro 1:74d7a989b741 515 } */
oguro 1:74d7a989b741 516
oguro 0:866aafac0128 517
oguro 0:866aafac0128 518
oguro 1:74d7a989b741 519 Va=cos(th)*Vd-sin(th)*Vqp; //Vqp 負荷小　Vq*0.8～1.0負荷大
oguro 1:74d7a989b741 520 Vb=sin(th)*Vd+cos(th)*Vqp; //Vqp
oguro 0:866aafac0128 521
oguro 0:866aafac0128 522 aVa=abs(Va);
oguro 0:866aafac0128 523 a3Vb=abs(sq3*Vb);
oguro 1:74d7a989b741 524
oguro 1:74d7a989b741 525 if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0
oguro 0:866aafac0128 526
oguro 0:866aafac0128 527 d1=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:866aafac0128 528 d2=sq32*(sq23*Vb)*Vdlink;
oguro 1:74d7a989b741 529 d07=z-(d1+d2);
oguro 0:866aafac0128 530 du=d1+d2+d07;
oguro 0:866aafac0128 531 dv=d2+d07;
oguro 0:866aafac0128 532 dw=d07;
oguro 0:866aafac0128 533 }
oguro 0:866aafac0128 534 if((aVa<=sq3*Vb)){ //sect 1
oguro 0:866aafac0128 535
oguro 0:866aafac0128 536 d3=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:866aafac0128 537 d2=sq32*(Va+sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 538 d07=z-(d2+d3);
oguro 0:866aafac0128 539 du=d2+d07;
oguro 0:866aafac0128 540 dv=d2+d3+d07;
oguro 0:866aafac0128 541 dw=d07;
oguro 0:866aafac0128 542 }
oguro 0:866aafac0128 543
oguro 0:866aafac0128 544 if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2
oguro 0:866aafac0128 545
oguro 0:866aafac0128 546 d3=sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 547 d4=sq32*(-Va-sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 548 d07=z-(d3+d4);
oguro 0:866aafac0128 549 du=d07;
oguro 1:74d7a989b741 550 dv=d3+d2+d07;
oguro 0:866aafac0128 551 dw=d4+d07;
oguro 0:866aafac0128 552 }
oguro 0:866aafac0128 553
oguro 0:866aafac0128 554 if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3
oguro 0:866aafac0128 555
oguro 0:866aafac0128 556 d5=-sq32*sq23*Vb*Vdlink;
oguro 0:866aafac0128 557 d4=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 558 d07=z-(d4+d5);
oguro 0:866aafac0128 559 du=d07;
oguro 0:866aafac0128 560 dv=d4+d07;
oguro 0:866aafac0128 561 dw=d4+d5+d07;
oguro 0:866aafac0128 562 }
oguro 0:866aafac0128 563
oguro 0:866aafac0128 564 if((aVa<=-sq3*Vb)){ //sect 4
oguro 0:866aafac0128 565
oguro 0:866aafac0128 566 d5=sq32*(-Va-sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 567 d6=sq32*(Va-sq3*Vb)*Vdlink;
oguro 1:74d7a989b741 568 d07=z-(d5+d6);
oguro 0:866aafac0128 569 du=d6+d07;
oguro 0:866aafac0128 570 dv=d07;
oguro 0:866aafac0128 571 dw=d5+d6+d07;
oguro 0:866aafac0128 572 }
oguro 0:866aafac0128 573
oguro 0:866aafac0128 574 if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5
oguro 0:866aafac0128 575
oguro 0:866aafac0128 576 d1=sq32*(Va+sq3*Vb)*Vdlink;;
oguro 0:866aafac0128 577 d6=-sq32*sq23*Vb*Vdlink;;
oguro 1:74d7a989b741 578 d07=z-(d1+d6);
oguro 0:866aafac0128 579 du=d1+d6+d07;
oguro 0:866aafac0128 580 dv=d07;
oguro 0:866aafac0128 581 dw=d6+d07;
oguro 0:866aafac0128 582 }
oguro 1:74d7a989b741 583
oguro 1:74d7a989b741 584 /* mypwmA.write(du);
oguro 1:74d7a989b741 585 mypwmB.write(dv);
oguro 1:74d7a989b741 586 mypwmC.write(dw);*/
oguro 1:74d7a989b741 587
oguro 1:74d7a989b741 588
oguro 1:74d7a989b741 589 /* suv=Vab[0]*zet;
oguro 1:74d7a989b741 590 svv=(Vab[0]*cos23+Vab[1]*sin23)*zet;
oguro 1:74d7a989b741 591 swv=(Vab[0]*cos43+Vab[1]*sin43)*zet;*/
oguro 0:866aafac0128 592
oguro 0:866aafac0128 593 // SWAVE=Vd;
oguro 0:866aafac0128 594 // SWAVE=th/4;
oguro 0:866aafac0128 595 // SWAVE=Vq;
oguro 0:866aafac0128 596 // SWAVE=West/5000;
oguro 0:866aafac0128 597 //SWAVE=Wo/5000;
oguro 0:866aafac0128 598 //SWAVE=Wz/5000;
oguro 0:866aafac0128 599 // SWAVE=sin(suv);
oguro 0:866aafac0128 600 // SWAVE=iab[0]+0.5;
oguro 0:866aafac0128 601 // SWAVE=Ed/10;
oguro 0:866aafac0128 602 // SWAVE=idq[1];
oguro 0:866aafac0128 603 // SWAVE=sin(th);
oguro 0:866aafac0128 604 // SWAVE=vst/5000;
oguro 0:866aafac0128 605 //SWAVE=Vb;
oguro 1:74d7a989b741 606 // SWAVE=iuvw[0];
oguro 0:866aafac0128 607 Vector=r;
oguro 0:866aafac0128 608 i++;
oguro 0:866aafac0128 609 }
oguro 0:866aafac0128 610 }