CCW CW Control BLDC

Dependencies:   mbed mbed-rtos

Committer:
oguro
Date:
Tue Nov 17 01:31:35 2020 +0000
Revision:
2:f23351f7af0b
Parent:
1:333d2cdd26d0
New forward Revers sine Wave 2020 11 17

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:faa58403944a 1 //Vector制御 工事中 2月末 公開予定
oguro 0:faa58403944a 2 //矩形波かから正弦波に移行 Speedの範囲を118行、173行目を
oguro 0:faa58403944a 3 //変更して矩形波→正弦波 への移行回転数を制御します。
oguro 0:faa58403944a 4 // ボリュームを中間値設定後、電源投入またはリセットしてください
oguro 0:faa58403944a 5 //ボリューム前後操作で正転/逆転を制御できます。
oguro 0:faa58403944a 6
oguro 0:faa58403944a 7 #include "mbed.h"
oguro 0:faa58403944a 8 #include <math.h>
oguro 0:faa58403944a 9
oguro 0:faa58403944a 10 #include <IO_define.h>
oguro 0:faa58403944a 11 #include <kukei.h>
oguro 0:faa58403944a 12 #include <sin.h>
oguro 0:faa58403944a 13 #include <cos.h>
oguro 0:faa58403944a 14 #include <vector.h>
oguro 0:faa58403944a 15
oguro 0:faa58403944a 16
oguro 0:faa58403944a 17 int main(){
oguro 1:333d2cdd26d0 18 pc.baud(128000);
oguro 0:faa58403944a 19 /* RtosTimer RtosTimerTS1(timerTS1);
oguro 0:faa58403944a 20 RtosTimerTS1.start((unsigned int)(TS1*5000));//2000
oguro 0:faa58403944a 21 Thread::wait(100);//1000*/
oguro 0:faa58403944a 22
oguro 0:faa58403944a 23 uT.start();
oguro 0:faa58403944a 24 vT.start();
oguro 0:faa58403944a 25 wT.start();
oguro 0:faa58403944a 26
oguro 0:faa58403944a 27 PWM_IN1_U.period_us(50);
oguro 0:faa58403944a 28 PWM_IN2_V.period_us(50);
oguro 0:faa58403944a 29 PWM_IN3_W.period_us(50);
oguro 0:faa58403944a 30
oguro 0:faa58403944a 31 zt.attach_us(&ztr, tau);
oguro 0:faa58403944a 32 ztc.attach_us(&ztrc,tau);
oguro 0:faa58403944a 33
oguro 0:faa58403944a 34 Vr_adc_i=V_adc.read();
oguro 0:faa58403944a 35 wait_ms(100);
oguro 0:faa58403944a 36 SH=HALL_U<<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 37 EN1_U=1;
oguro 0:faa58403944a 38 EN2_V=1;
oguro 0:faa58403944a 39 EN3_W=1;
oguro 0:faa58403944a 40
oguro 0:faa58403944a 41 while(1){
oguro 0:faa58403944a 42
oguro 0:faa58403944a 43 SH=HALL_U<<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 44
oguro 0:faa58403944a 45 Vr_adc=V_adc.read();
oguro 0:faa58403944a 46 y=Vr_adc- Vr_adc_i;
oguro 0:faa58403944a 47 ay=fabs(y);
oguro 1:333d2cdd26d0 48 power=ay*2.0;
oguro 1:333d2cdd26d0 49 if(y>0.05){
oguro 0:faa58403944a 50 frd=0;
oguro 0:faa58403944a 51 //aout=0.5;
oguro 0:faa58403944a 52 }
oguro 1:333d2cdd26d0 53 if(y<-0.05){
oguro 0:faa58403944a 54 frd=1;
oguro 0:faa58403944a 55 //aout=0.9;
oguro 0:faa58403944a 56 }
oguro 0:faa58403944a 57
oguro 1:333d2cdd26d0 58 if(ay<0.05){
oguro 0:faa58403944a 59 q=0;
oguro 0:faa58403944a 60 aout=0.0;
oguro 0:faa58403944a 61 //SH=HALL_U<<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 62 //power=0.0;
oguro 0:faa58403944a 63 }
oguro 0:faa58403944a 64 /************起動********************/
oguro 0:faa58403944a 65 if(((y<-0.1)||(y>0.1))&&(q==0)){
oguro 0:faa58403944a 66 //if(q==0){
oguro 0:faa58403944a 67 while(q<20){
oguro 0:faa58403944a 68 SH=HALL_U<<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 69
oguro 0:faa58403944a 70 /*****Forword 起動******/
oguro 0:faa58403944a 71 if((frd==0)){
oguro 0:faa58403944a 72 //SH=HALL_U<<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 73 if(SH==5){
oguro 0:faa58403944a 74 start_F5();
oguro 0:faa58403944a 75 }
oguro 0:faa58403944a 76 if(SH==4){
oguro 0:faa58403944a 77 start_F4();
oguro 0:faa58403944a 78 }
oguro 0:faa58403944a 79 if(SH==6){
oguro 0:faa58403944a 80 start_F6();
oguro 0:faa58403944a 81 }
oguro 0:faa58403944a 82 if(SH==2){
oguro 0:faa58403944a 83 start_F2();
oguro 0:faa58403944a 84 }
oguro 0:faa58403944a 85 if(SH==3){
oguro 0:faa58403944a 86 start_F3();
oguro 0:faa58403944a 87 }
oguro 0:faa58403944a 88 if(SH==1){
oguro 0:faa58403944a 89 start_F1();
oguro 0:faa58403944a 90 }
oguro 0:faa58403944a 91 }
oguro 0:faa58403944a 92
oguro 0:faa58403944a 93 /*******Reversal 起動************/
oguro 0:faa58403944a 94 if((frd==1)){
oguro 0:faa58403944a 95 //SH=HALL_U<2|HALL_V<<1|HALL_W;
oguro 0:faa58403944a 96 if(SH==3){
oguro 0:faa58403944a 97 start_R3();
oguro 0:faa58403944a 98 }
oguro 0:faa58403944a 99 if(SH==2){
oguro 0:faa58403944a 100 start_R2();
oguro 0:faa58403944a 101 }
oguro 0:faa58403944a 102 if(SH==6){
oguro 0:faa58403944a 103 start_R6();
oguro 0:faa58403944a 104 }
oguro 0:faa58403944a 105 if(SH==4){
oguro 0:faa58403944a 106 start_R4;
oguro 0:faa58403944a 107 }
oguro 0:faa58403944a 108 if(SH==5){
oguro 0:faa58403944a 109 start_R5();
oguro 0:faa58403944a 110 }
oguro 0:faa58403944a 111 if(SH==1){
oguro 0:faa58403944a 112 start_R1();
oguro 0:faa58403944a 113 }
oguro 0:faa58403944a 114 }
oguro 0:faa58403944a 115 q++;
oguro 0:faa58403944a 116 }
oguro 0:faa58403944a 117 }
oguro 0:faa58403944a 118
oguro 1:333d2cdd26d0 119 if(Speed<500){//2000
oguro 0:faa58403944a 120 /**************矩形波駆動始動***************/
oguro 0:faa58403944a 121 /*******Forward******************/
oguro 0:faa58403944a 122 if(frd==0){
oguro 0:faa58403944a 123 switch(SH){
oguro 1:333d2cdd26d0 124 case 5: PWM_W();//W5
oguro 0:faa58403944a 125 break;
oguro 1:333d2cdd26d0 126 case 4: EN_W();//W4
oguro 0:faa58403944a 127 break;
oguro 1:333d2cdd26d0 128 case 6: PWM_U();//U6
oguro 0:faa58403944a 129 break;
oguro 1:333d2cdd26d0 130 case 2: EN_U();//U2
oguro 0:faa58403944a 131 break;
oguro 1:333d2cdd26d0 132 case 3: PWM_V();//V3
oguro 0:faa58403944a 133 break;
oguro 1:333d2cdd26d0 134 case 1: EN_V();//V1
oguro 0:faa58403944a 135 break;
oguro 0:faa58403944a 136 default :PWM_W();//W
oguro 0:faa58403944a 137 break;
oguro 0:faa58403944a 138 }
oguro 0:faa58403944a 139 HALL_Ui.rise(time_WP);
oguro 0:faa58403944a 140 HALL_Wi.fall(time_VN);
oguro 0:faa58403944a 141 HALL_Vi.rise(time_UP);
oguro 0:faa58403944a 142 HALL_Ui.fall(time_WN);
oguro 0:faa58403944a 143 HALL_Wi.rise(time_VP);
oguro 0:faa58403944a 144 HALL_Vi.fall(time_UN);
oguro 0:faa58403944a 145 }
oguro 0:faa58403944a 146 /*******Reversal******************/
oguro 0:faa58403944a 147 if(frd==1){
oguro 0:faa58403944a 148 switch(SH){
oguro 0:faa58403944a 149 case 5: PWM_W();
oguro 0:faa58403944a 150 break;
oguro 0:faa58403944a 151 case 1: EN_W();
oguro 0:faa58403944a 152 break;
oguro 0:faa58403944a 153 case 3: PWM_V();
oguro 0:faa58403944a 154 break;
oguro 0:faa58403944a 155 case 2: EN_V();
oguro 0:faa58403944a 156 break;
oguro 0:faa58403944a 157 case 6: PWM_U();
oguro 0:faa58403944a 158 break;
oguro 0:faa58403944a 159 case 4: EN_U();
oguro 0:faa58403944a 160 break;
oguro 0:faa58403944a 161 default :PWM_W();
oguro 0:faa58403944a 162 break;
oguro 0:faa58403944a 163 }
oguro 0:faa58403944a 164 HALL_Ui.rise(time_UP);
oguro 0:faa58403944a 165 HALL_Wi.fall(time_WN);
oguro 0:faa58403944a 166 HALL_Vi.rise(time_VP);
oguro 0:faa58403944a 167 HALL_Ui.fall(time_UN);
oguro 0:faa58403944a 168 HALL_Wi.rise(time_WP);
oguro 0:faa58403944a 169 HALL_Vi.fall(time_VN);
oguro 0:faa58403944a 170 }
oguro 0:faa58403944a 171
oguro 0:faa58403944a 172 }//Speed<2000
oguro 0:faa58403944a 173
oguro 1:333d2cdd26d0 174 if((Speed>=500)){//&&(Speed<4000)){//2000
oguro 0:faa58403944a 175 /************sin drive Forward************/
oguro 0:faa58403944a 176 if(frd==1){
oguro 0:faa58403944a 177 HALL_Ui.rise(&PWM_sinU);
oguro 0:faa58403944a 178 HALL_Wi.fall(&EN_sinW);
oguro 0:faa58403944a 179 HALL_Vi.rise(&PWM_sinV);
oguro 0:faa58403944a 180 HALL_Ui.fall(&EN_sinU);
oguro 0:faa58403944a 181 HALL_Wi.rise(&PWM_sinW);
oguro 0:faa58403944a 182 HALL_Vi.fall(&EN_sinV);
oguro 0:faa58403944a 183
oguro 0:faa58403944a 184 }
oguro 0:faa58403944a 185
oguro 0:faa58403944a 186 /*******sin drive Reversal******************/
oguro 0:faa58403944a 187 if(frd==0){
oguro 0:faa58403944a 188 HALL_Ui.rise(&PWM_sinW);
oguro 0:faa58403944a 189 HALL_Wi.fall(&EN_sinV);
oguro 0:faa58403944a 190 HALL_Vi.rise(&PWM_sinU);
oguro 0:faa58403944a 191 HALL_Ui.fall(&EN_sinW);
oguro 0:faa58403944a 192 HALL_Wi.rise(&PWM_sinV);
oguro 0:faa58403944a 193 HALL_Vi.fall(&EN_sinU);
oguro 0:faa58403944a 194
oguro 0:faa58403944a 195 }
oguro 0:faa58403944a 196 /****************************/
oguro 0:faa58403944a 197
oguro 0:faa58403944a 198 if((uz[2])<=-16383){ //飽和処理
oguro 0:faa58403944a 199 uz[2]=-16383;
oguro 0:faa58403944a 200 }
oguro 0:faa58403944a 201 if(uz[2]>=16383){
oguro 0:faa58403944a 202 uz[2]=16383;
oguro 0:faa58403944a 203 }
oguro 0:faa58403944a 204
oguro 0:faa58403944a 205 if(vz[2]<=-16383){ //飽和処理
oguro 0:faa58403944a 206 vz[2]=-16383;
oguro 0:faa58403944a 207 }
oguro 0:faa58403944a 208 if(vz[2]>=16383){
oguro 0:faa58403944a 209 vz[2]=16383;
oguro 0:faa58403944a 210 }
oguro 0:faa58403944a 211 if(wz[2]<=-16383){ //飽和処理
oguro 0:faa58403944a 212 wz[2]=-16383;
oguro 0:faa58403944a 213 }
oguro 0:faa58403944a 214 if(wz[2]>=16383){
oguro 0:faa58403944a 215 wz[2]=16383;
oguro 0:faa58403944a 216 }
oguro 1:333d2cdd26d0 217 #if 1
oguro 0:faa58403944a 218 PWM_IN1_U.write(((float(uz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 219 PWM_IN2_V.write(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 220 PWM_IN3_W.write(((float(wz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 221
oguro 1:333d2cdd26d0 222 aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
oguro 1:333d2cdd26d0 223 #endif
oguro 1:333d2cdd26d0 224 #if 0
oguro 1:333d2cdd26d0 225 su=(((float(uz[2])/(16383*2))*(power*STOP))+0.5);
oguro 1:333d2cdd26d0 226 sv=(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
oguro 1:333d2cdd26d0 227 sw=(((float(wz[2])/(16383*2))*(power*STOP))+0.5);
oguro 1:333d2cdd26d0 228
oguro 1:333d2cdd26d0 229 PWM_IN1_U.write(su);
oguro 1:333d2cdd26d0 230 PWM_IN2_V.write(sv);
oguro 1:333d2cdd26d0 231 PWM_IN3_W.write(sw);
oguro 1:333d2cdd26d0 232 aout=su;
oguro 1:333d2cdd26d0 233 //aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
oguro 1:333d2cdd26d0 234 #endif
oguro 0:faa58403944a 235 //filterCurrent();
oguro 0:faa58403944a 236 //aout=Curr_wf;
oguro 0:faa58403944a 237 //HALL_Ui.rise(&cosU);
oguro 0:faa58403944a 238 //HALL_Ui.fall(&cosUN);
oguro 0:faa58403944a 239
oguro 0:faa58403944a 240 }//sin
oguro 0:faa58403944a 241
oguro 0:faa58403944a 242 //if(Speed>=4000){
oguro 0:faa58403944a 243
oguro 0:faa58403944a 244 //HALL_Ui.rise(&PWM_sinU);
oguro 0:faa58403944a 245 //HALL_Ui.fall(&EN_sinU);
oguro 0:faa58403944a 246 // HALL_Ui.rise(&cosU);
oguro 0:faa58403944a 247 // HALL_Ui.fall(&cosUN);
oguro 0:faa58403944a 248
oguro 0:faa58403944a 249 //filterCurrent();
oguro 0:faa58403944a 250
oguro 0:faa58403944a 251
oguro 0:faa58403944a 252 // sinth=(float(uz[2])/(16383*2))+0.5;
oguro 0:faa58403944a 253 //costh=(float(uc[2])/(16383*2))+0.5;
oguro 0:faa58403944a 254
oguro 0:faa58403944a 255 //aout=sinth;
oguro 0:faa58403944a 256
oguro 0:faa58403944a 257 /* ia=(iu+iv*cos23+iw*cos43)*zet;
oguro 0:faa58403944a 258 ib=(iv*sin23+iw*sin43)*zet;
oguro 0:faa58403944a 259
oguro 0:faa58403944a 260 id=costh*ia+sinth*ib;
oguro 0:faa58403944a 261 iq=-sinth*ia+costh*ib;
oguro 0:faa58403944a 262
oguro 0:faa58403944a 263 id_p=0;
oguro 0:faa58403944a 264 iq_p=Vr_adc;
oguro 0:faa58403944a 265 id_diff=id_p-id;
oguro 0:faa58403944a 266 iq_diff=iq_p-iq;
oguro 0:faa58403944a 267
oguro 0:faa58403944a 268 */
oguro 0:faa58403944a 269
oguro 0:faa58403944a 270 /********PI Control id,iq*********************/
oguro 0:faa58403944a 271 /* s_ki_id += ki_id*id_diff;
oguro 0:faa58403944a 272 Vd = s_ki_id + kp_id*id_diff;
oguro 0:faa58403944a 273
oguro 0:faa58403944a 274 s_ki_iq += ki_iq*(iq_diff+Vr_adc);
oguro 0:faa58403944a 275 Vq = s_ki_iq + kp_iq*(iq_diff+Vr_adc);*/
oguro 0:faa58403944a 276 /*******************************************/
oguro 0:faa58403944a 277 /****** detect rotor position*************/
oguro 0:faa58403944a 278 /* Wz=(2*PI)/(2*vst*1E-6);
oguro 0:faa58403944a 279 Ed= (Vq)-0.11f*iq-Wz*0.018E-3*id;
oguro 0:faa58403944a 280 phm=Ed/(Wz);
oguro 0:faa58403944a 281 dth=(Vd-0.11f*(id)+Wz*0.018E-3*(iq))/(Wz*phm);
oguro 0:faa58403944a 282 derrth=asin(dth);*/
oguro 0:faa58403944a 283
oguro 0:faa58403944a 284 /*****PID error θ *****/
oguro 0:faa58403944a 285
oguro 0:faa58403944a 286 /* float ki_PLL=0.01;
oguro 0:faa58403944a 287 float kp_PLL=0.2;
oguro 0:faa58403944a 288 s_ki_errth += ki_PLL*derrth;
oguro 0:faa58403944a 289 therr = s_ki_errth + kp_PLL*errth;*/
oguro 0:faa58403944a 290
oguro 0:faa58403944a 291 /*******PLL W ***********/
oguro 0:faa58403944a 292 /* if(therr>0.01){
oguro 0:faa58403944a 293 W=(2*PI)/((2*vst*1E-6)+(therr/Wz));
oguro 0:faa58403944a 294 }
oguro 0:faa58403944a 295 if(therr<-0.01){
oguro 0:faa58403944a 296 W=(2*PI)/((2*vst*1E-6)-(therr/Wz));
oguro 0:faa58403944a 297 } */
oguro 0:faa58403944a 298 /************************************/
oguro 0:faa58403944a 299 /* th += W*zint;
oguro 0:faa58403944a 300 Va=cos(th)*Vd-sin(th)*Vq;
oguro 0:faa58403944a 301 Vb=sin(th)*Vd+cos(th)*Vq;
oguro 0:faa58403944a 302
oguro 0:faa58403944a 303 SVPWM();
oguro 0:faa58403944a 304
oguro 0:faa58403944a 305 aout=iq;
oguro 0:faa58403944a 306
oguro 0:faa58403944a 307
oguro 0:faa58403944a 308 }//vector*/
oguro 0:faa58403944a 309 /*******************************************************************/
oguro 0:faa58403944a 310 usi=2*(ut2-ut1);
oguro 0:faa58403944a 311 vsi=2*(vt2-vt1);
oguro 0:faa58403944a 312 wsi=2*(wt2-wt1);
oguro 0:faa58403944a 313 usic=2*(ut2c-ut1c);
oguro 1:333d2cdd26d0 314 //Speed=60*(1/(7.0*(usi*0.5)*1E-6));//BullRun
oguro 0:faa58403944a 315 /************************************************************/
oguro 1:333d2cdd26d0 316 Speed=60*(1/(7.0*usi*1E-6));
oguro 1:333d2cdd26d0 317 //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 2:f23351f7af0b 318 UP=HALL_Ui;VP=HALL_Vi;WP=HALL_Wi;
oguro 2:f23351f7af0b 319 pc.printf("%d , %d , %d \r" , UP,VP,WP);
oguro 0:faa58403944a 320
oguro 0:faa58403944a 321 }
oguro 0:faa58403944a 322 }