akiyoshi oguro / Mbed 2 deprecated Vector_sin_drive_F302R8_2

FOC sinewave drive ver2

Dependencies:   mbed mbed-rtos

main.cpp@1:e45063ee4be0, 2020-11-18 (annotated)

Committer:
oguro
Date:
Wed Nov 18 06:24:19 2020 +0000
Revision:
1:e45063ee4be0
Parent:
0:dcfc6150c1d6 
Refine Vector Sine Wave 2

Who changed what in which revision?

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