akiyoshi oguro / Mbed 2 deprecated Nucleo_Sensorless_Blushless_DC

Sensorless DC Blushless Motor

Dependencies:   mbed-rtos mbed

Fork of Nucleo_Sensorless_Blushless_DC_ by akiyoshi oguro

main.cpp@0:320025ba637f, 2017-07-24 (annotated)

Committer:
oguro
Date:
Mon Jul 24 23:41:45 2017 +0000
Revision:
0:320025ba637f
Sensorless DC Bulushless

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:320025ba637f 1 #include "mbed.h"
oguro 0:320025ba637f 2 #include <math.h>
oguro 0:320025ba637f 3 #include "rtos.h"
oguro 0:320025ba637f 4 #define TS1 0.2
oguro 0:320025ba637f 5
oguro 0:320025ba637f 6 int i=0,q=0,s=0,r=0,rr=0,START=3; //5
oguro 0:320025ba637f 7 int sensorless=0;
oguro 0:320025ba637f 8 float wsi=0,wsj=0;
oguro 0:320025ba637f 9 float t=0,tt=0,tp=0;
oguro 0:320025ba637f 10 float t1=0,t2=0,t3=0,t4=0,t5=0,t6=0,t7=0,t8=0,t9=0,t10=0;
oguro 0:320025ba637f 11 float adc_s=0;
oguro 0:320025ba637f 12 float Speed=0,Speed_h=0;
oguro 0:320025ba637f 13 float ad_sensorless;
oguro 0:320025ba637f 14 float power=1.0;
oguro 0:320025ba637f 15
oguro 0:320025ba637f 16 PwmOut mypwmA(PA_8); //PWM_OUT
oguro 0:320025ba637f 17 PwmOut mypwmB(PA_9);
oguro 0:320025ba637f 18 PwmOut mypwmC(PA_10);
oguro 0:320025ba637f 19
oguro 0:320025ba637f 20 PwmOut Current_Ref(PB_4);
oguro 0:320025ba637f 21 DigitalOut Sensorless(PC_4);
oguro 0:320025ba637f 22
oguro 0:320025ba637f 23 DigitalOut EN1(PC_10);
oguro 0:320025ba637f 24 DigitalOut EN2(PC_11);
oguro 0:320025ba637f 25 DigitalOut EN3(PC_12);
oguro 0:320025ba637f 26 DigitalOut OUTC(PC_8);
oguro 0:320025ba637f 27
oguro 0:320025ba637f 28
oguro 0:320025ba637f 29 AnalogIn V_adc(PC_2); //Outside Volume
oguro 0:320025ba637f 30 AnalogIn V_sinkaku(PB_1); //Blue Volume
oguro 0:320025ba637f 31 AnalogOut SWAVE(PA_4);
oguro 0:320025ba637f 32
oguro 0:320025ba637f 33
oguro 0:320025ba637f 34 AnalogIn BEMF1(PC_3);//C7_37
oguro 0:320025ba637f 35 AnalogIn BEMF2(PB_0);//C7_34
oguro 0:320025ba637f 36 AnalogIn BEMF3(PA_7);//C10_26 PA_7
oguro 0:320025ba637f 37
oguro 0:320025ba637f 38 DigitalOut GPIO_BEMF(PC_9);
oguro 0:320025ba637f 39
oguro 0:320025ba637f 40 InterruptIn HA(PA_15);
oguro 0:320025ba637f 41
oguro 0:320025ba637f 42 Timer timer2;
oguro 0:320025ba637f 43 Timer timer_uT;
oguro 0:320025ba637f 44 float ut1=0,ut2=0,usi=0;
oguro 0:320025ba637f 45
oguro 0:320025ba637f 46 Serial pc(USBTX,USBRX);
oguro 0:320025ba637f 47
oguro 0:320025ba637f 48 DigitalOut myled(LED1);
oguro 0:320025ba637f 49
oguro 0:320025ba637f 50 float Vr_adc=0.0f;
oguro 0:320025ba637f 51 float adc;
oguro 0:320025ba637f 52 float sinkaku;
oguro 0:320025ba637f 53
oguro 0:320025ba637f 54 void HAH(){
oguro 0:320025ba637f 55
oguro 0:320025ba637f 56 rr=r%2;
oguro 0:320025ba637f 57 if(rr==0){
oguro 0:320025ba637f 58 ut1=timer_uT.read_us();
oguro 0:320025ba637f 59 r++;
oguro 0:320025ba637f 60 }
oguro 0:320025ba637f 61
oguro 0:320025ba637f 62 if(rr==1){
oguro 0:320025ba637f 63 ut2=timer_uT.read_us();
oguro 0:320025ba637f 64 r++;
oguro 0:320025ba637f 65 timer_uT.reset();
oguro 0:320025ba637f 66 }
oguro 0:320025ba637f 67 }
oguro 0:320025ba637f 68
oguro 0:320025ba637f 69 void CPLT(){
oguro 0:320025ba637f 70 pc.printf("%.3f , %.3f \r" ,Speed_h ,Vr_adc);
oguro 0:320025ba637f 71 }
oguro 0:320025ba637f 72
oguro 0:320025ba637f 73 void timerTS1(void const*argument){
oguro 0:320025ba637f 74 CPLT();
oguro 0:320025ba637f 75 }
oguro 0:320025ba637f 76
oguro 0:320025ba637f 77 int main() {
oguro 0:320025ba637f 78
oguro 0:320025ba637f 79 EN1=1;
oguro 0:320025ba637f 80 EN2=1;
oguro 0:320025ba637f 81 EN3=1;
oguro 0:320025ba637f 82
oguro 0:320025ba637f 83 timer2.start();
oguro 0:320025ba637f 84 timer_uT.start();
oguro 0:320025ba637f 85 mypwmA.period_us(20);
oguro 0:320025ba637f 86 mypwmB.period_us(20);
oguro 0:320025ba637f 87 mypwmC.period_us(20);
oguro 0:320025ba637f 88
oguro 0:320025ba637f 89 OUTC=0;
oguro 0:320025ba637f 90 GPIO_BEMF=0;
oguro 0:320025ba637f 91 pc.baud(128000);
oguro 0:320025ba637f 92
oguro 0:320025ba637f 93 /* RtosTimer RtosTimerTS1(timerTS1);
oguro 0:320025ba637f 94 RtosTimerTS1.start((unsigned int)(TS1*5000));
oguro 0:320025ba637f 95 Thread::wait(100); */
oguro 0:320025ba637f 96
oguro 0:320025ba637f 97 while(1) {
oguro 0:320025ba637f 98
oguro 0:320025ba637f 99 HA.rise(&HAH);
oguro 0:320025ba637f 100 Vr_adc=V_adc.read();
oguro 0:320025ba637f 101 adc= V_adc.read_u16()>>4 ;
oguro 0:320025ba637f 102
oguro 0:320025ba637f 103 sinkaku=((V_sinkaku.read_u16())>>7)+20;//7
oguro 0:320025ba637f 104
oguro 0:320025ba637f 105 if(i==0){
oguro 0:320025ba637f 106 mypwmA.write(0.5f);
oguro 0:320025ba637f 107 wait_ms(100);
oguro 0:320025ba637f 108 i=1;
oguro 0:320025ba637f 109 }
oguro 0:320025ba637f 110
oguro 0:320025ba637f 111 tp=3800-(adc);// 3800 3300-4000
oguro 0:320025ba637f 112
oguro 0:320025ba637f 113 if(sensorless==0){
oguro 0:320025ba637f 114 t=tp;
oguro 0:320025ba637f 115 tt=t;
oguro 0:320025ba637f 116 }
oguro 0:320025ba637f 117
oguro 0:320025ba637f 118
oguro 0:320025ba637f 119
oguro 0:320025ba637f 120 if(Speed<900){ //900
oguro 0:320025ba637f 121 // t=tp;
oguro 0:320025ba637f 122 sensorless=0;
oguro 0:320025ba637f 123 s=1;
oguro 0:320025ba637f 124 }
oguro 0:320025ba637f 125 if(Speed>=900){ //900
oguro 0:320025ba637f 126
oguro 0:320025ba637f 127 if(s==1){
oguro 0:320025ba637f 128 ad_sensorless=Vr_adc;
oguro 0:320025ba637f 129 s=0;
oguro 0:320025ba637f 130 sensorless=1;
oguro 0:320025ba637f 131 t1=(abs(wsj-wsi));
oguro 0:320025ba637f 132
oguro 0:320025ba637f 133 t=t1;
oguro 0:320025ba637f 134 // tt=t;
oguro 0:320025ba637f 135 }
oguro 0:320025ba637f 136
oguro 0:320025ba637f 137 if((Speed >= 900)&&(Speed<=1500)){
oguro 0:320025ba637f 138 power=0.8; //0.5
oguro 0:320025ba637f 139 }
oguro 0:320025ba637f 140 if((Speed >= 1500)&&(Speed<=2000)){
oguro 0:320025ba637f 141 power=0.6; //0.5
oguro 0:320025ba637f 142 }
oguro 0:320025ba637f 143 if((Speed >= 2000)&&(Speed<=2500)){
oguro 0:320025ba637f 144 power=0.8; //0.6
oguro 0:320025ba637f 145 }
oguro 0:320025ba637f 146 if((Speed >= 2500)){
oguro 0:320025ba637f 147 power=1.0; //0.7
oguro 0:320025ba637f 148
oguro 0:320025ba637f 149 }
oguro 0:320025ba637f 150
oguro 0:320025ba637f 151
oguro 0:320025ba637f 152 adc_s=Vr_adc-ad_sensorless;
oguro 0:320025ba637f 153
oguro 0:320025ba637f 154 if(adc_s < -0.01){
oguro 0:320025ba637f 155 t2=t1+100;
oguro 0:320025ba637f 156 t=t2;
oguro 0:320025ba637f 157 tt=t-sinkaku;
oguro 0:320025ba637f 158 }
oguro 0:320025ba637f 159
oguro 0:320025ba637f 160 if((adc_s > 0.0)&&(adc_s <= 0.05)){
oguro 0:320025ba637f 161 t2=t1-200; //200
oguro 0:320025ba637f 162 t=t2;
oguro 0:320025ba637f 163 tt=t-sinkaku;
oguro 0:320025ba637f 164 }
oguro 0:320025ba637f 165 if((adc_s > 0.05)&&(adc_s <= 0.1)){
oguro 0:320025ba637f 166 t3=t2-150; //150
oguro 0:320025ba637f 167 t=t3;
oguro 0:320025ba637f 168 tt=t-sinkaku;
oguro 0:320025ba637f 169 }
oguro 0:320025ba637f 170 if((adc_s > 0.1)&&(adc_s <= 0.15)){
oguro 0:320025ba637f 171 t4=t3-130; //130
oguro 0:320025ba637f 172 t=t4;
oguro 0:320025ba637f 173 tt=t-sinkaku;
oguro 0:320025ba637f 174 }
oguro 0:320025ba637f 175 if((adc_s > 0.15)&&(adc_s <= 0.2)){
oguro 0:320025ba637f 176 t5=t4-100; //120
oguro 0:320025ba637f 177 t=t5;
oguro 0:320025ba637f 178 tt=t-sinkaku;
oguro 0:320025ba637f 179 }
oguro 0:320025ba637f 180 if((adc_s > 0.2)&&(adc_s <= 0.25)){
oguro 0:320025ba637f 181 t6=t5-100; //100
oguro 0:320025ba637f 182 t=t6;
oguro 0:320025ba637f 183 tt=t-sinkaku;
oguro 0:320025ba637f 184 }
oguro 0:320025ba637f 185 if((adc_s > 0.25)&&(adc_s <= 0.3)){
oguro 0:320025ba637f 186 t7=t6-80; //80
oguro 0:320025ba637f 187 t=t7;
oguro 0:320025ba637f 188 tt=t-sinkaku;
oguro 0:320025ba637f 189 }
oguro 0:320025ba637f 190 if((adc_s > 0.3)&&(adc_s <= 0.35)){
oguro 0:320025ba637f 191 t8=t7-70; //70
oguro 0:320025ba637f 192 t=t8;
oguro 0:320025ba637f 193 tt=t-sinkaku;
oguro 0:320025ba637f 194 }
oguro 0:320025ba637f 195 if((adc_s > 0.35)&&(adc_s <= 0.4)){
oguro 0:320025ba637f 196 t9=t8-70; //70
oguro 0:320025ba637f 197 t=t9;
oguro 0:320025ba637f 198 tt=t-sinkaku;
oguro 0:320025ba637f 199 }
oguro 0:320025ba637f 200 if((adc_s > 0.4)&&(adc_s <= 0.45)){
oguro 0:320025ba637f 201 t10=t9-50; //50
oguro 0:320025ba637f 202 t=t10;
oguro 0:320025ba637f 203 tt=t-sinkaku;
oguro 0:320025ba637f 204 }
oguro 0:320025ba637f 205 if(adc_s > 0.45){
oguro 0:320025ba637f 206 t=t10-50; //50
oguro 0:320025ba637f 207 tt=t-sinkaku;
oguro 0:320025ba637f 208 }
oguro 0:320025ba637f 209 }
oguro 0:320025ba637f 210
oguro 0:320025ba637f 211 // tt=t-100;
oguro 0:320025ba637f 212
oguro 0:320025ba637f 213
oguro 0:320025ba637f 214
oguro 0:320025ba637f 215 if((Vr_adc>0.15f)&&(q==0)){
oguro 0:320025ba637f 216 while(q<100){
oguro 0:320025ba637f 217
oguro 0:320025ba637f 218 EN1=1;
oguro 0:320025ba637f 219 EN2=1;
oguro 0:320025ba637f 220 EN3=0;
oguro 0:320025ba637f 221 mypwmA.write(0.5f);//0.5
oguro 0:320025ba637f 222 mypwmB=0;
oguro 0:320025ba637f 223 mypwmC=0;
oguro 0:320025ba637f 224
oguro 0:320025ba637f 225 wait_ms(START);
oguro 0:320025ba637f 226
oguro 0:320025ba637f 227 EN1=1;
oguro 0:320025ba637f 228 EN2=0;
oguro 0:320025ba637f 229 EN3=1;
oguro 0:320025ba637f 230
oguro 0:320025ba637f 231 wait_ms(START);
oguro 0:320025ba637f 232
oguro 0:320025ba637f 233 EN1=0;
oguro 0:320025ba637f 234 EN2=1;
oguro 0:320025ba637f 235 EN3=1;
oguro 0:320025ba637f 236 mypwmA=0;
oguro 0:320025ba637f 237 mypwmB.write(0.5f);
oguro 0:320025ba637f 238 mypwmC=0;
oguro 0:320025ba637f 239 wait_ms(START);
oguro 0:320025ba637f 240
oguro 0:320025ba637f 241 EN1=1;
oguro 0:320025ba637f 242 EN2=1;
oguro 0:320025ba637f 243 EN3=0;
oguro 0:320025ba637f 244
oguro 0:320025ba637f 245 wait_ms(START);
oguro 0:320025ba637f 246
oguro 0:320025ba637f 247 EN1=1;
oguro 0:320025ba637f 248 EN2=0;
oguro 0:320025ba637f 249 EN3=1;
oguro 0:320025ba637f 250
oguro 0:320025ba637f 251 mypwmA=0;
oguro 0:320025ba637f 252 mypwmB=0;
oguro 0:320025ba637f 253 mypwmC.write(0.5f);
oguro 0:320025ba637f 254
oguro 0:320025ba637f 255 wait_ms(START);
oguro 0:320025ba637f 256
oguro 0:320025ba637f 257 EN1=0;
oguro 0:320025ba637f 258 EN2=1;
oguro 0:320025ba637f 259 EN3=1;
oguro 0:320025ba637f 260
oguro 0:320025ba637f 261 q++;
oguro 0:320025ba637f 262 wait_ms(START);
oguro 0:320025ba637f 263
oguro 0:320025ba637f 264 }
oguro 0:320025ba637f 265 }
oguro 0:320025ba637f 266
oguro 0:320025ba637f 267 if(Vr_adc < 0.05f){ //0.005
oguro 0:320025ba637f 268 q=0;
oguro 0:320025ba637f 269 s=0;
oguro 0:320025ba637f 270 r=0;
oguro 0:320025ba637f 271 rr=0;
oguro 0:320025ba637f 272 i=0;
oguro 0:320025ba637f 273 sensorless=0;
oguro 0:320025ba637f 274 power=1.0;
oguro 0:320025ba637f 275 }
oguro 0:320025ba637f 276
oguro 0:320025ba637f 277
oguro 0:320025ba637f 278
oguro 0:320025ba637f 279 if(Vr_adc >0.05f){
oguro 0:320025ba637f 280 EN1=1;
oguro 0:320025ba637f 281 EN2=1;
oguro 0:320025ba637f 282 EN3=0;
oguro 0:320025ba637f 283
oguro 0:320025ba637f 284 mypwmA.write(Vr_adc*power);
oguro 0:320025ba637f 285 mypwmB.write(0.0f);
oguro 0:320025ba637f 286 mypwmC.write(0.0f);
oguro 0:320025ba637f 287
oguro 0:320025ba637f 288 wait_us(t-200);
oguro 0:320025ba637f 289 EN1=1;
oguro 0:320025ba637f 290 EN2=0;
oguro 0:320025ba637f 291 EN3=1;
oguro 0:320025ba637f 292
oguro 0:320025ba637f 293 if(BEMF1>0.5f){
oguro 0:320025ba637f 294 OUTC=0;
oguro 0:320025ba637f 295 wsi=timer2.read_us();
oguro 0:320025ba637f 296 }
oguro 0:320025ba637f 297 wait_us(tt);
oguro 0:320025ba637f 298 if(OUTC==0){
oguro 0:320025ba637f 299 wsj=timer2.read_us();
oguro 0:320025ba637f 300 }
oguro 0:320025ba637f 301 OUTC=1;
oguro 0:320025ba637f 302
oguro 0:320025ba637f 303 EN1=0;
oguro 0:320025ba637f 304 EN2=1;
oguro 0:320025ba637f 305 EN3=1;
oguro 0:320025ba637f 306
oguro 0:320025ba637f 307 mypwmA.write(0.0f);
oguro 0:320025ba637f 308 mypwmB.write(Vr_adc*power);
oguro 0:320025ba637f 309 mypwmC.write(0.0f);
oguro 0:320025ba637f 310
oguro 0:320025ba637f 311 wait_us(t);
oguro 0:320025ba637f 312
oguro 0:320025ba637f 313 EN1=1;
oguro 0:320025ba637f 314 EN2=1;
oguro 0:320025ba637f 315 EN3=0;
oguro 0:320025ba637f 316
oguro 0:320025ba637f 317 if(BEMF2>0.5f){
oguro 0:320025ba637f 318 OUTC=0;
oguro 0:320025ba637f 319 wsi=timer2.read_us();
oguro 0:320025ba637f 320 }
oguro 0:320025ba637f 321 wait_us(tt);
oguro 0:320025ba637f 322 if(OUTC==0){
oguro 0:320025ba637f 323 wsj=timer2.read_us();
oguro 0:320025ba637f 324 }
oguro 0:320025ba637f 325 OUTC=1;
oguro 0:320025ba637f 326
oguro 0:320025ba637f 327 EN1=1;
oguro 0:320025ba637f 328 EN2=0;
oguro 0:320025ba637f 329 EN3=1;
oguro 0:320025ba637f 330
oguro 0:320025ba637f 331 mypwmA.write(0.0f);
oguro 0:320025ba637f 332 mypwmB.write(0.0f);
oguro 0:320025ba637f 333 mypwmC.write(Vr_adc*power);
oguro 0:320025ba637f 334
oguro 0:320025ba637f 335 wait_us(t);
oguro 0:320025ba637f 336
oguro 0:320025ba637f 337 EN1=0;
oguro 0:320025ba637f 338 EN2=1;
oguro 0:320025ba637f 339 EN3=1;
oguro 0:320025ba637f 340
oguro 0:320025ba637f 341
oguro 0:320025ba637f 342 if(BEMF3>0.5f){
oguro 0:320025ba637f 343 OUTC=0;
oguro 0:320025ba637f 344 wsi=timer2.read_us();
oguro 0:320025ba637f 345 }
oguro 0:320025ba637f 346 wait_us(tt);
oguro 0:320025ba637f 347 if(OUTC==0){
oguro 0:320025ba637f 348 wsj=timer2.read_us();
oguro 0:320025ba637f 349 }
oguro 0:320025ba637f 350 OUTC=1;
oguro 0:320025ba637f 351
oguro 0:320025ba637f 352 }
oguro 0:320025ba637f 353
oguro 0:320025ba637f 354 Sensorless=sensorless;
oguro 0:320025ba637f 355 usi=abs(ut2-ut1);
oguro 0:320025ba637f 356 Speed_h=60*(1/(7.0*usi*1E-6));
oguro 0:320025ba637f 357 Speed=60*(1/(7.0*6.0*t*1E-6));
oguro 0:320025ba637f 358
oguro 0:320025ba637f 359 myled = !myled;
oguro 0:320025ba637f 360
oguro 0:320025ba637f 361 }
oguro 0:320025ba637f 362 }