akiyoshi oguro / Mbed 2 deprecated Vector_Open_SVPWM_2

Space Vector PWM Ver2

Dependencies:   mbed mbed-rtos

main.cpp@0:fd2cb1721c58, 2020-08-21 (annotated)

Committer:
oguro
Date:
Fri Aug 21 01:39:52 2020 +0000
Revision:
0:fd2cb1721c58
Space Vector PWM Ver2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:fd2cb1721c58 1 #include "mbed.h"
oguro 0:fd2cb1721c58 2 #include "rtos.h"
oguro 0:fd2cb1721c58 3 #include <math.h>
oguro 0:fd2cb1721c58 4 #define TS1 0.2
oguro 0:fd2cb1721c58 5
oguro 0:fd2cb1721c58 6 float PI=3.141592;
oguro 0:fd2cb1721c58 7 int t=0,q=0,i=0;
oguro 0:fd2cb1721c58 8 int START=10; //7
oguro 0:fd2cb1721c58 9 PwmOut mypwmA(PA_8);
oguro 0:fd2cb1721c58 10 PwmOut mypwmB(PA_9);
oguro 0:fd2cb1721c58 11 PwmOut mypwmC(PA_10);
oguro 0:fd2cb1721c58 12
oguro 0:fd2cb1721c58 13 PwmOut Current_Ref(PB_4);
oguro 0:fd2cb1721c58 14
oguro 0:fd2cb1721c58 15 DigitalOut Vector(PC_4);
oguro 0:fd2cb1721c58 16 DigitalOut EN1(PC_10);
oguro 0:fd2cb1721c58 17 DigitalOut EN2(PC_11);
oguro 0:fd2cb1721c58 18 DigitalOut EN3(PC_12);
oguro 0:fd2cb1721c58 19
oguro 0:fd2cb1721c58 20
oguro 0:fd2cb1721c58 21
oguro 0:fd2cb1721c58 22 //AnalogIn V_adc(PB_1); //volume
oguro 0:fd2cb1721c58 23 AnalogIn V_adc(PC_2); // Gaibu Potention
oguro 0:fd2cb1721c58 24
oguro 0:fd2cb1721c58 25 AnalogIn Vshuntu(PA_1);
oguro 0:fd2cb1721c58 26 AnalogIn Vshuntv(PA_0);
oguro 0:fd2cb1721c58 27 AnalogIn Vshuntw(PB_0);
oguro 0:fd2cb1721c58 28
oguro 0:fd2cb1721c58 29 AnalogIn BEMF1(PC_3);//C7_37
oguro 0:fd2cb1721c58 30 AnalogIn BEMF2(PB_0);//C7_34
oguro 0:fd2cb1721c58 31 AnalogIn BEMF3(PA_7);//C10_26
oguro 0:fd2cb1721c58 32
oguro 0:fd2cb1721c58 33 DigitalIn GPIO_BEMF(PC_9);
oguro 0:fd2cb1721c58 34 DigitalIn CPOUT(PA_12);
oguro 0:fd2cb1721c58 35 AnalogIn Curr_ui(PA_0);
oguro 0:fd2cb1721c58 36 AnalogIn Curr_vi(PC_1);
oguro 0:fd2cb1721c58 37 AnalogIn Curr_wi(PA_1);
oguro 0:fd2cb1721c58 38
oguro 0:fd2cb1721c58 39 InterruptIn HA(PA_15);
oguro 0:fd2cb1721c58 40 InterruptIn HB(PB_3);
oguro 0:fd2cb1721c58 41 InterruptIn HC(PB_10);
oguro 0:fd2cb1721c58 42 Timer uT;
oguro 0:fd2cb1721c58 43 Timer vT;
oguro 0:fd2cb1721c58 44 Timer wT;
oguro 0:fd2cb1721c58 45
oguro 0:fd2cb1721c58 46 AnalogOut SWAVE(PA_4);
oguro 0:fd2cb1721c58 47
oguro 0:fd2cb1721c58 48 Serial pc(USBTX,USBRX);
oguro 0:fd2cb1721c58 49
oguro 0:fd2cb1721c58 50 DigitalOut myled(LED1);
oguro 0:fd2cb1721c58 51 float sq32=sqrt(3.0f/2.0f);
oguro 0:fd2cb1721c58 52 float sq23=2.0f/sqrt(3.0f);
oguro 0:fd2cb1721c58 53 float sq3=1.0f/sqrt(3.0f);
oguro 0:fd2cb1721c58 54 float VDC=1.8;
oguro 0:fd2cb1721c58 55 float Vdlink=1/VDC;
oguro 0:fd2cb1721c58 56 float aVa;
oguro 0:fd2cb1721c58 57 float a3Vb;
oguro 0:fd2cb1721c58 58 float Vr_adc;
oguro 0:fd2cb1721c58 59
oguro 0:fd2cb1721c58 60 Timer Timer1;
oguro 0:fd2cb1721c58 61 float freq=0;
oguro 0:fd2cb1721c58 62 float z=1.0; //0.5
oguro 0:fd2cb1721c58 63 float Wt,Va,Vb,Vq,Vd;
oguro 0:fd2cb1721c58 64 float d1,d2,d3,d4,d5,d6,d07;
oguro 0:fd2cb1721c58 65 float du,dv,dw;
oguro 0:fd2cb1721c58 66 bool cu,cv,cw;
oguro 0:fd2cb1721c58 67 float ave=0,aveo=0,avsum=0;
oguro 0:fd2cb1721c58 68
oguro 0:fd2cb1721c58 69 float ut=0,ut1=0,ut2=0,usi=0;
oguro 0:fd2cb1721c58 70 float vt=0,vt1=0,vt2=0,vsi=0;
oguro 0:fd2cb1721c58 71 float wt=0,wt1=0,wt2=0,wsi=0;
oguro 0:fd2cb1721c58 72 float Speed;
oguro 0:fd2cb1721c58 73
oguro 0:fd2cb1721c58 74 void HAH(){
oguro 0:fd2cb1721c58 75
oguro 0:fd2cb1721c58 76 ut1=uT.read_us();
oguro 0:fd2cb1721c58 77 ut=0;
oguro 0:fd2cb1721c58 78
oguro 0:fd2cb1721c58 79 }
oguro 0:fd2cb1721c58 80 void HAL(){
oguro 0:fd2cb1721c58 81
oguro 0:fd2cb1721c58 82 ut2=uT.read_us();
oguro 0:fd2cb1721c58 83 uT.reset();
oguro 0:fd2cb1721c58 84
oguro 0:fd2cb1721c58 85 }
oguro 0:fd2cb1721c58 86 void HBH(){
oguro 0:fd2cb1721c58 87
oguro 0:fd2cb1721c58 88 vt1=vT.read_us();
oguro 0:fd2cb1721c58 89
oguro 0:fd2cb1721c58 90 }
oguro 0:fd2cb1721c58 91 void HBL(){
oguro 0:fd2cb1721c58 92
oguro 0:fd2cb1721c58 93 vt2=vT.read_us();
oguro 0:fd2cb1721c58 94
oguro 0:fd2cb1721c58 95 vT.reset();
oguro 0:fd2cb1721c58 96 }
oguro 0:fd2cb1721c58 97 void HCH(){
oguro 0:fd2cb1721c58 98 wt1=wT.read_us();
oguro 0:fd2cb1721c58 99
oguro 0:fd2cb1721c58 100 }
oguro 0:fd2cb1721c58 101
oguro 0:fd2cb1721c58 102 void HCL(){
oguro 0:fd2cb1721c58 103 wt2=wT.read_us();
oguro 0:fd2cb1721c58 104
oguro 0:fd2cb1721c58 105 wT.reset();
oguro 0:fd2cb1721c58 106 }
oguro 0:fd2cb1721c58 107
oguro 0:fd2cb1721c58 108
oguro 0:fd2cb1721c58 109
oguro 0:fd2cb1721c58 110 void CPLT(){
oguro 0:fd2cb1721c58 111 //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 0:fd2cb1721c58 112 //pc.printf("%.3f,%.3f,%.3f \r" ,du,dv,dw);
oguro 0:fd2cb1721c58 113 }
oguro 0:fd2cb1721c58 114
oguro 0:fd2cb1721c58 115 void timerTS1(void const*argument){
oguro 0:fd2cb1721c58 116 CPLT();
oguro 0:fd2cb1721c58 117 }
oguro 0:fd2cb1721c58 118 int main() {
oguro 0:fd2cb1721c58 119 Timer1.start();
oguro 0:fd2cb1721c58 120 EN1=1;
oguro 0:fd2cb1721c58 121 EN2=1;
oguro 0:fd2cb1721c58 122 EN3=1;
oguro 0:fd2cb1721c58 123 uT.start();
oguro 0:fd2cb1721c58 124 vT.start();
oguro 0:fd2cb1721c58 125 wT.start();
oguro 0:fd2cb1721c58 126
oguro 0:fd2cb1721c58 127 mypwmA.period_us(20);
oguro 0:fd2cb1721c58 128
oguro 0:fd2cb1721c58 129 mypwmB.period_us(20);
oguro 0:fd2cb1721c58 130
oguro 0:fd2cb1721c58 131 mypwmC.period_us(20);
oguro 0:fd2cb1721c58 132
oguro 0:fd2cb1721c58 133 pc.baud(128000);
oguro 0:fd2cb1721c58 134
oguro 0:fd2cb1721c58 135 /* RtosTimer RtosTimerTS1(timerTS1);
oguro 0:fd2cb1721c58 136 RtosTimerTS1.start((unsigned int)(TS1*5000)); //3000
oguro 0:fd2cb1721c58 137 Thread::wait(100); */
oguro 0:fd2cb1721c58 138
oguro 0:fd2cb1721c58 139
oguro 0:fd2cb1721c58 140 while(1) {
oguro 0:fd2cb1721c58 141
oguro 0:fd2cb1721c58 142 HA.rise(&HAH);
oguro 0:fd2cb1721c58 143 HC.fall(&HCL);
oguro 0:fd2cb1721c58 144 HB.rise(&HBH);
oguro 0:fd2cb1721c58 145 HA.fall(&HAL);
oguro 0:fd2cb1721c58 146 HC.rise(&HCH);
oguro 0:fd2cb1721c58 147 HB.fall(&HBL);
oguro 0:fd2cb1721c58 148 Speed=60*(1/(7.0*2.0*usi*1E-6));
oguro 0:fd2cb1721c58 149 usi=ut2-ut1;
oguro 0:fd2cb1721c58 150 vsi=vt2-vt1;
oguro 0:fd2cb1721c58 151 wsi=wt2-wt1;
oguro 0:fd2cb1721c58 152 Vd=0;
oguro 0:fd2cb1721c58 153
oguro 0:fd2cb1721c58 154 float s = Timer1.read();
oguro 0:fd2cb1721c58 155 Vr_adc=V_adc.read();
oguro 0:fd2cb1721c58 156 myled = ((int)s) % 2 == 0; /* LED output(0.5Hz) */
oguro 0:fd2cb1721c58 157
oguro 0:fd2cb1721c58 158
oguro 0:fd2cb1721c58 159 if((Vr_adc>0.08f)&&(q==0)){
oguro 0:fd2cb1721c58 160 while(q<30){
oguro 0:fd2cb1721c58 161
oguro 0:fd2cb1721c58 162 mypwmA.write(0.5f);
oguro 0:fd2cb1721c58 163 mypwmB.write(0);
oguro 0:fd2cb1721c58 164 mypwmC.write(0);
oguro 0:fd2cb1721c58 165 wait_ms(START);
oguro 0:fd2cb1721c58 166
oguro 0:fd2cb1721c58 167 mypwmA.write(0);
oguro 0:fd2cb1721c58 168 mypwmB.write(0.5f);
oguro 0:fd2cb1721c58 169 mypwmC.write(0.0);
oguro 0:fd2cb1721c58 170 wait_ms(START);
oguro 0:fd2cb1721c58 171
oguro 0:fd2cb1721c58 172 mypwmA.write(0);
oguro 0:fd2cb1721c58 173 mypwmB.write(0.0);
oguro 0:fd2cb1721c58 174 mypwmC.write(0.5f);
oguro 0:fd2cb1721c58 175 wait_ms(START);
oguro 0:fd2cb1721c58 176 q++;
oguro 0:fd2cb1721c58 177
oguro 0:fd2cb1721c58 178 }
oguro 0:fd2cb1721c58 179 }
oguro 0:fd2cb1721c58 180 if(Vr_adc < 0.05f){
oguro 0:fd2cb1721c58 181 q=0;
oguro 0:fd2cb1721c58 182 Vr_adc=0;
oguro 0:fd2cb1721c58 183
oguro 0:fd2cb1721c58 184 }
oguro 0:fd2cb1721c58 185
oguro 0:fd2cb1721c58 186 aveo=Vr_adc;
oguro 0:fd2cb1721c58 187
oguro 0:fd2cb1721c58 188 if((aveo<=0.15)&&(aveo>0.1)){
oguro 0:fd2cb1721c58 189 Wt=fmodf(s * 45, 1) * PI* 2;
oguro 0:fd2cb1721c58 190 Vq=0.4;
oguro 0:fd2cb1721c58 191 }
oguro 0:fd2cb1721c58 192 if((aveo<=0.2)&&(aveo>0.15)){
oguro 0:fd2cb1721c58 193 Wt=fmodf(s * 50, 1) * PI* 2;
oguro 0:fd2cb1721c58 194 Vq=0.42;
oguro 0:fd2cb1721c58 195 }
oguro 0:fd2cb1721c58 196 if((aveo<=0.25)&&(aveo>0.2)){
oguro 0:fd2cb1721c58 197 Wt=fmodf(s * 55, 1) * PI* 2;
oguro 0:fd2cb1721c58 198 Vq=0.45;
oguro 0:fd2cb1721c58 199 }
oguro 0:fd2cb1721c58 200 if((aveo<=0.3)&&(aveo>0.25)){
oguro 0:fd2cb1721c58 201 Wt=fmodf(s * 60, 1) * PI* 2;
oguro 0:fd2cb1721c58 202 Vq=0.48;
oguro 0:fd2cb1721c58 203 }
oguro 0:fd2cb1721c58 204 if((aveo<=0.35)&&(aveo>0.3)){
oguro 0:fd2cb1721c58 205 Wt=fmodf(s * 65, 1) * PI* 2;
oguro 0:fd2cb1721c58 206 Vq=0.5;
oguro 0:fd2cb1721c58 207 }
oguro 0:fd2cb1721c58 208 if((aveo<=0.4)&&(aveo>0.35)){
oguro 0:fd2cb1721c58 209 Wt=fmodf(s * 70, 1) * PI* 2;
oguro 0:fd2cb1721c58 210 Vq=0.52;
oguro 0:fd2cb1721c58 211 }
oguro 0:fd2cb1721c58 212 if((aveo<=0.45)&&(aveo>0.4)){
oguro 0:fd2cb1721c58 213 Wt=fmodf(s * 75, 1) * PI* 2;
oguro 0:fd2cb1721c58 214 Vq=0.55;
oguro 0:fd2cb1721c58 215 }
oguro 0:fd2cb1721c58 216 if((aveo<=0.5)&&(aveo>0.45)){
oguro 0:fd2cb1721c58 217 Wt=fmodf(s * 80, 1) * PI* 2;
oguro 0:fd2cb1721c58 218 Vq=0.58;
oguro 0:fd2cb1721c58 219 }
oguro 0:fd2cb1721c58 220 if((aveo<=0.55)&&(aveo>0.5)){
oguro 0:fd2cb1721c58 221 Wt=fmodf(s * 85, 1) * PI* 2;
oguro 0:fd2cb1721c58 222 Vq=0.6;
oguro 0:fd2cb1721c58 223 }
oguro 0:fd2cb1721c58 224 if((aveo<=0.6)&&(aveo>0.55)){
oguro 0:fd2cb1721c58 225 Wt=fmodf(s * 90, 1) * PI* 2;
oguro 0:fd2cb1721c58 226 Vq=0.62;
oguro 0:fd2cb1721c58 227 }
oguro 0:fd2cb1721c58 228 if((aveo<=0.65)&&(aveo>0.6)){
oguro 0:fd2cb1721c58 229 Wt=fmodf(s * 95, 1) * PI* 2;
oguro 0:fd2cb1721c58 230 Vq=0.65;
oguro 0:fd2cb1721c58 231 }
oguro 0:fd2cb1721c58 232 if((aveo<=0.7)&&(aveo>0.65)){
oguro 0:fd2cb1721c58 233 Wt=fmodf(s * 100, 1) * PI* 2;
oguro 0:fd2cb1721c58 234 Vq=0.67;
oguro 0:fd2cb1721c58 235 }
oguro 0:fd2cb1721c58 236 if((aveo<=0.75)&&(aveo>0.7)){
oguro 0:fd2cb1721c58 237 Wt=fmodf(s * 105, 1) * PI* 2;
oguro 0:fd2cb1721c58 238 Vq=0.7;
oguro 0:fd2cb1721c58 239 }
oguro 0:fd2cb1721c58 240 if((aveo<=0.8)&&(aveo>0.75)){
oguro 0:fd2cb1721c58 241 Wt=fmodf(s * 110, 1) * PI* 2;
oguro 0:fd2cb1721c58 242 Vq=0.72;
oguro 0:fd2cb1721c58 243 }
oguro 0:fd2cb1721c58 244 if((aveo<=0.85)&&(aveo>0.8)){
oguro 0:fd2cb1721c58 245 Wt=fmodf(s * 120, 1) * PI* 2;
oguro 0:fd2cb1721c58 246 Vq=0.75;
oguro 0:fd2cb1721c58 247 }
oguro 0:fd2cb1721c58 248 if((aveo<=0.9)&&(aveo>0.85)){
oguro 0:fd2cb1721c58 249 Wt=fmodf(s * 130, 1) * PI* 2;
oguro 0:fd2cb1721c58 250 Vq=0.78;
oguro 0:fd2cb1721c58 251 }
oguro 0:fd2cb1721c58 252 if((aveo<=0.95)&&(aveo>0.9)){
oguro 0:fd2cb1721c58 253 Wt=fmodf(s * 140, 1) * PI* 2;
oguro 0:fd2cb1721c58 254 Vq=0.8;
oguro 0:fd2cb1721c58 255 }
oguro 0:fd2cb1721c58 256 if(aveo>0.95){
oguro 0:fd2cb1721c58 257 Wt=fmodf(s * 150, 1) * PI* 2;
oguro 0:fd2cb1721c58 258 Vq=0.82;
oguro 0:fd2cb1721c58 259 }
oguro 0:fd2cb1721c58 260 Vq=0.9;
oguro 0:fd2cb1721c58 261 if(aveo<0.1){
oguro 0:fd2cb1721c58 262 mypwmA.write(0);
oguro 0:fd2cb1721c58 263 mypwmB.write(0);
oguro 0:fd2cb1721c58 264 mypwmC.write(0);
oguro 0:fd2cb1721c58 265
oguro 0:fd2cb1721c58 266 }
oguro 0:fd2cb1721c58 267 else{
oguro 0:fd2cb1721c58 268 Va=cos(Wt)*Vd-sin(Wt)*Vq;
oguro 0:fd2cb1721c58 269 Vb=sin(Wt)*Vd+cos(Wt)*Vq;
oguro 0:fd2cb1721c58 270 aVa=abs(Va);
oguro 0:fd2cb1721c58 271 a3Vb=abs(sq3*Vb);
oguro 0:fd2cb1721c58 272
oguro 0:fd2cb1721c58 273 if((Va>=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 0
oguro 0:fd2cb1721c58 274
oguro 0:fd2cb1721c58 275 d1=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 276 d2=sq32*(sq23*Vb)*Vdlink;
oguro 0:fd2cb1721c58 277 d07=(z-(d1+d2))*0.5;
oguro 0:fd2cb1721c58 278 // d07=0;
oguro 0:fd2cb1721c58 279 du=d1+d2+d07;
oguro 0:fd2cb1721c58 280 dv=d2+d07;
oguro 0:fd2cb1721c58 281 dw=d07;
oguro 0:fd2cb1721c58 282
oguro 0:fd2cb1721c58 283 }
oguro 0:fd2cb1721c58 284 if((aVa<=sq3*Vb)){ //sect 1
oguro 0:fd2cb1721c58 285
oguro 0:fd2cb1721c58 286 d3=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 287 d2=sq32*(Va+sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 288 d07=(z-(d2+d3))*0.5;
oguro 0:fd2cb1721c58 289 //d07=0;
oguro 0:fd2cb1721c58 290 du=d2+d07;
oguro 0:fd2cb1721c58 291 dv=d2+d3+d07;
oguro 0:fd2cb1721c58 292 dw=d07;
oguro 0:fd2cb1721c58 293 }
oguro 0:fd2cb1721c58 294
oguro 0:fd2cb1721c58 295 if((Va<=0)&&(Vb>=0)&&(aVa>=a3Vb)){ //sect 2
oguro 0:fd2cb1721c58 296
oguro 0:fd2cb1721c58 297 d3=sq32*sq23*Vb*Vdlink;
oguro 0:fd2cb1721c58 298 d4=sq32*(-Va-sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 299 d07=(z-(d3+d4))*0.5;
oguro 0:fd2cb1721c58 300 // d07=0;
oguro 0:fd2cb1721c58 301 du=d07;
oguro 0:fd2cb1721c58 302 dv=d3+d4+d07;
oguro 0:fd2cb1721c58 303 dw=d4+d07;
oguro 0:fd2cb1721c58 304 }
oguro 0:fd2cb1721c58 305
oguro 0:fd2cb1721c58 306 if((Va<=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 3
oguro 0:fd2cb1721c58 307
oguro 0:fd2cb1721c58 308 d5=-sq32*sq23*Vb*Vdlink;
oguro 0:fd2cb1721c58 309 d4=sq32*(-Va+sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 310 d07=(z-(d4+d5))*0.5;
oguro 0:fd2cb1721c58 311 //d07=0;
oguro 0:fd2cb1721c58 312 du=d07;
oguro 0:fd2cb1721c58 313 dv=d4+d07;
oguro 0:fd2cb1721c58 314 dw=d4+d5+d07;
oguro 0:fd2cb1721c58 315 }
oguro 0:fd2cb1721c58 316
oguro 0:fd2cb1721c58 317 if((aVa<=-sq3*Vb)){ //sect 4
oguro 0:fd2cb1721c58 318
oguro 0:fd2cb1721c58 319 d5=sq32*(-Va-sq3*Vb)*Vdlink;;
oguro 0:fd2cb1721c58 320 d6=sq32*(Va-sq3*Vb)*Vdlink;
oguro 0:fd2cb1721c58 321 d07=(z-(d5+d6))*0.5;
oguro 0:fd2cb1721c58 322 //d07=0;
oguro 0:fd2cb1721c58 323 du=d6+d07;
oguro 0:fd2cb1721c58 324 dv=d07;
oguro 0:fd2cb1721c58 325 dw=d5+d6+d07;
oguro 0:fd2cb1721c58 326 }
oguro 0:fd2cb1721c58 327
oguro 0:fd2cb1721c58 328 if((Va>=0)&&(Vb<=0)&&(aVa>=a3Vb)){ //sect 5
oguro 0:fd2cb1721c58 329
oguro 0:fd2cb1721c58 330 d1=sq32*(Va+sq3*Vb)*Vdlink;;
oguro 0:fd2cb1721c58 331 d6=-sq32*sq23*Vb*Vdlink;;
oguro 0:fd2cb1721c58 332 d07=(z-(d1+d6))*0.5;
oguro 0:fd2cb1721c58 333 //d07=0;
oguro 0:fd2cb1721c58 334 du=d1+d6+d07;
oguro 0:fd2cb1721c58 335 dv=d07;
oguro 0:fd2cb1721c58 336 dw=d6+d07;
oguro 0:fd2cb1721c58 337 }
oguro 0:fd2cb1721c58 338
oguro 0:fd2cb1721c58 339 mypwmA.write(du);
oguro 0:fd2cb1721c58 340 mypwmB.write(dv);
oguro 0:fd2cb1721c58 341 mypwmC.write(dw);
oguro 0:fd2cb1721c58 342 }
oguro 0:fd2cb1721c58 343
oguro 0:fd2cb1721c58 344 SWAVE=dw;
oguro 0:fd2cb1721c58 345 //pc.printf("%.3f,%.3f,%.3f \r" ,du,dv,dw);
oguro 0:fd2cb1721c58 346 }
oguro 0:fd2cb1721c58 347 }