akiyoshi oguro / Mbed 2 deprecated Vector_sin_drive_F302R8

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

main.cpp@0:ce0449e386ce, 2018-02-24 (annotated)

Committer:
oguro
Date:
Sat Feb 24 08:25:42 2018 +0000
Revision:
0:ce0449e386ce
Child:
1:0768eefd7892
Vector sin

Who changed what in which revision?

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