akiyoshi oguro / Mbed 2 deprecated Nucleo_Hall_rect_sin

CCW CW Control BLDC

Dependencies:   mbed mbed-rtos

main.cpp@0:faa58403944a, 2019-02-17 (annotated)

Committer:
oguro
Date:
Sun Feb 17 02:58:08 2019 +0000
Revision:
0:faa58403944a
Child:
1:333d2cdd26d0
CCW/CW Control  Kukei seigen HA

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 0:faa58403944a 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 0:faa58403944a 48 power=ay;
oguro 0:faa58403944a 49 if(y>0.1){
oguro 0:faa58403944a 50 frd=0;
oguro 0:faa58403944a 51 //aout=0.5;
oguro 0:faa58403944a 52 }
oguro 0:faa58403944a 53 if(y<-0.1){
oguro 0:faa58403944a 54 frd=1;
oguro 0:faa58403944a 55 //aout=0.9;
oguro 0:faa58403944a 56 }
oguro 0:faa58403944a 57
oguro 0:faa58403944a 58 if(ay<0.1){
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 0:faa58403944a 119 if(Speed<1000){//2000
oguro 0:faa58403944a 120 /**************矩形波駆動始動***************/
oguro 0:faa58403944a 121 /*******Forward******************/
oguro 0:faa58403944a 122 if(frd==0){
oguro 0:faa58403944a 123 switch(SH){
oguro 0:faa58403944a 124 case 5: PWM_W();//W
oguro 0:faa58403944a 125 break;
oguro 0:faa58403944a 126 case 4: EN_W();//W
oguro 0:faa58403944a 127 break;
oguro 0:faa58403944a 128 case 6: PWM_U();//U
oguro 0:faa58403944a 129 break;
oguro 0:faa58403944a 130 case 2: EN_U();//U
oguro 0:faa58403944a 131 break;
oguro 0:faa58403944a 132 case 3: PWM_V();//V
oguro 0:faa58403944a 133 break;
oguro 0:faa58403944a 134 case 1: EN_V();//V
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 0:faa58403944a 174 if((Speed>=1000)){//&&(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 0:faa58403944a 217 PWM_IN1_U.write(((float(uz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 218 PWM_IN2_V.write(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 219 PWM_IN3_W.write(((float(wz[2])/(16383*2))*(power*STOP))+0.5);
oguro 0:faa58403944a 220
oguro 0:faa58403944a 221 aout=(float(uz[2])/(16383*2))*(power*STOP)+0.5;//16383
oguro 0:faa58403944a 222 //filterCurrent();
oguro 0:faa58403944a 223 //aout=Curr_wf;
oguro 0:faa58403944a 224 //HALL_Ui.rise(&cosU);
oguro 0:faa58403944a 225 //HALL_Ui.fall(&cosUN);
oguro 0:faa58403944a 226
oguro 0:faa58403944a 227 }//sin
oguro 0:faa58403944a 228
oguro 0:faa58403944a 229 //if(Speed>=4000){
oguro 0:faa58403944a 230
oguro 0:faa58403944a 231 //HALL_Ui.rise(&PWM_sinU);
oguro 0:faa58403944a 232 //HALL_Ui.fall(&EN_sinU);
oguro 0:faa58403944a 233 // HALL_Ui.rise(&cosU);
oguro 0:faa58403944a 234 // HALL_Ui.fall(&cosUN);
oguro 0:faa58403944a 235
oguro 0:faa58403944a 236 //filterCurrent();
oguro 0:faa58403944a 237
oguro 0:faa58403944a 238
oguro 0:faa58403944a 239 // sinth=(float(uz[2])/(16383*2))+0.5;
oguro 0:faa58403944a 240 //costh=(float(uc[2])/(16383*2))+0.5;
oguro 0:faa58403944a 241
oguro 0:faa58403944a 242 //aout=sinth;
oguro 0:faa58403944a 243
oguro 0:faa58403944a 244 /* ia=(iu+iv*cos23+iw*cos43)*zet;
oguro 0:faa58403944a 245 ib=(iv*sin23+iw*sin43)*zet;
oguro 0:faa58403944a 246
oguro 0:faa58403944a 247 id=costh*ia+sinth*ib;
oguro 0:faa58403944a 248 iq=-sinth*ia+costh*ib;
oguro 0:faa58403944a 249
oguro 0:faa58403944a 250 id_p=0;
oguro 0:faa58403944a 251 iq_p=Vr_adc;
oguro 0:faa58403944a 252 id_diff=id_p-id;
oguro 0:faa58403944a 253 iq_diff=iq_p-iq;
oguro 0:faa58403944a 254
oguro 0:faa58403944a 255 */
oguro 0:faa58403944a 256
oguro 0:faa58403944a 257 /********PI Control id,iq*********************/
oguro 0:faa58403944a 258 /* s_ki_id += ki_id*id_diff;
oguro 0:faa58403944a 259 Vd = s_ki_id + kp_id*id_diff;
oguro 0:faa58403944a 260
oguro 0:faa58403944a 261 s_ki_iq += ki_iq*(iq_diff+Vr_adc);
oguro 0:faa58403944a 262 Vq = s_ki_iq + kp_iq*(iq_diff+Vr_adc);*/
oguro 0:faa58403944a 263 /*******************************************/
oguro 0:faa58403944a 264 /****** detect rotor position*************/
oguro 0:faa58403944a 265 /* Wz=(2*PI)/(2*vst*1E-6);
oguro 0:faa58403944a 266 Ed= (Vq)-0.11f*iq-Wz*0.018E-3*id;
oguro 0:faa58403944a 267 phm=Ed/(Wz);
oguro 0:faa58403944a 268 dth=(Vd-0.11f*(id)+Wz*0.018E-3*(iq))/(Wz*phm);
oguro 0:faa58403944a 269 derrth=asin(dth);*/
oguro 0:faa58403944a 270
oguro 0:faa58403944a 271 /*****PID error θ　*****/
oguro 0:faa58403944a 272
oguro 0:faa58403944a 273 /* float ki_PLL=0.01;
oguro 0:faa58403944a 274 float kp_PLL=0.2;
oguro 0:faa58403944a 275 s_ki_errth += ki_PLL*derrth;
oguro 0:faa58403944a 276 therr = s_ki_errth + kp_PLL*errth;*/
oguro 0:faa58403944a 277
oguro 0:faa58403944a 278 /*******PLL W ***********/
oguro 0:faa58403944a 279 /* if(therr>0.01){
oguro 0:faa58403944a 280 W=(2*PI)/((2*vst*1E-6)+(therr/Wz));
oguro 0:faa58403944a 281 }
oguro 0:faa58403944a 282 if(therr<-0.01){
oguro 0:faa58403944a 283 W=(2*PI)/((2*vst*1E-6)-(therr/Wz));
oguro 0:faa58403944a 284 } */
oguro 0:faa58403944a 285 /************************************/
oguro 0:faa58403944a 286 /* th += W*zint;
oguro 0:faa58403944a 287 Va=cos(th)*Vd-sin(th)*Vq;
oguro 0:faa58403944a 288 Vb=sin(th)*Vd+cos(th)*Vq;
oguro 0:faa58403944a 289
oguro 0:faa58403944a 290 SVPWM();
oguro 0:faa58403944a 291
oguro 0:faa58403944a 292 aout=iq;
oguro 0:faa58403944a 293
oguro 0:faa58403944a 294
oguro 0:faa58403944a 295 }//vector*/
oguro 0:faa58403944a 296 /*******************************************************************/
oguro 0:faa58403944a 297 usi=2*(ut2-ut1);
oguro 0:faa58403944a 298 vsi=2*(vt2-vt1);
oguro 0:faa58403944a 299 wsi=2*(wt2-wt1);
oguro 0:faa58403944a 300 usic=2*(ut2c-ut1c);
oguro 0:faa58403944a 301 Speed=60*(1/(6.0*fabs(usi)*1E-6));//CQ 6
oguro 0:faa58403944a 302 /************************************************************/
oguro 0:faa58403944a 303
oguro 0:faa58403944a 304
oguro 0:faa58403944a 305 }
oguro 0:faa58403944a 306 }