akiyoshi oguro / Mbed 2 deprecated Nucleo_Hall_rect_sin_vector

Trapezoid sin Vector

Dependencies:   mbed mbed-rtos

main.cpp@0:fa432f8ea1a6, 2019-06-28 (annotated)

Committer:
oguro
Date:
Fri Jun 28 11:52:27 2019 +0000
Revision:
0:fa432f8ea1a6
Trapezoid sin Vector Control

Who changed what in which revision?

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