akiyoshi oguro
/
Nucleo_Hall_rect_sin
CCW CW Control BLDC
main.cpp@2:f23351f7af0b, 2020-11-17 (annotated)
- 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?
User | Revision | Line number | New 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 | } |