akiyoshi oguro / Mbed 2 deprecated Nucleo_Z_SIN_Full_BLDC

sin wave Ztransform BLDC

Dependencies:   mbed mbed-rtos

main.cpp@0:d7338aa3fbcd, 2020-11-13 (annotated)

Committer:
oguro
Date:
Fri Nov 13 06:00:01 2020 +0000
Revision:
0:d7338aa3fbcd
Nucleo Ztrans SIN  Full

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:d7338aa3fbcd 1 #include "mbed.h"
oguro 0:d7338aa3fbcd 2 #include <math.h>
oguro 0:d7338aa3fbcd 3 #include "rtos.h"
oguro 0:d7338aa3fbcd 4
oguro 0:d7338aa3fbcd 5 #define PI 3.14159265358979
oguro 0:d7338aa3fbcd 6 #define TS1 0.01//0.2
oguro 0:d7338aa3fbcd 7
oguro 0:d7338aa3fbcd 8 int i=0,q=0;
oguro 0:d7338aa3fbcd 9
oguro 0:d7338aa3fbcd 10 short f1=0,a1=0;
oguro 0:d7338aa3fbcd 11 int START=10;//8
oguro 0:d7338aa3fbcd 12 float ut1=0,ut2=0,usi=0;
oguro 0:d7338aa3fbcd 13 float vt1=0,vt2=0,vsi=0;
oguro 0:d7338aa3fbcd 14 float wt1=0,wt2=0,wsi=0;
oguro 0:d7338aa3fbcd 15
oguro 0:d7338aa3fbcd 16 float Speed;
oguro 0:d7338aa3fbcd 17 float tau=100; //60
oguro 0:d7338aa3fbcd 18 float su,sv,sw;
oguro 0:d7338aa3fbcd 19
oguro 0:d7338aa3fbcd 20 PwmOut mypwmA(PA_8); //PWM_OUT 8
oguro 0:d7338aa3fbcd 21 PwmOut mypwmB(PA_9); //9
oguro 0:d7338aa3fbcd 22 PwmOut mypwmC(PA_10);//10
oguro 0:d7338aa3fbcd 23
oguro 0:d7338aa3fbcd 24 DigitalOut EN1(PC_10);
oguro 0:d7338aa3fbcd 25 DigitalOut EN2(PC_11);
oguro 0:d7338aa3fbcd 26 DigitalOut EN3(PC_12);
oguro 0:d7338aa3fbcd 27
oguro 0:d7338aa3fbcd 28 InterruptIn HA(PA_15);
oguro 0:d7338aa3fbcd 29 InterruptIn HB(PB_3);
oguro 0:d7338aa3fbcd 30 InterruptIn HC(PB_10);
oguro 0:d7338aa3fbcd 31
oguro 0:d7338aa3fbcd 32 AnalogOut aout(PA_4);
oguro 0:d7338aa3fbcd 33
oguro 0:d7338aa3fbcd 34 //AnalogIn V_adc(PB_1);//Volume
oguro 0:d7338aa3fbcd 35 AnalogIn V_adc(PC_2); // gaibu Volume
oguro 0:d7338aa3fbcd 36
oguro 0:d7338aa3fbcd 37 AnalogIn Current_W(PC_0);
oguro 0:d7338aa3fbcd 38
oguro 0:d7338aa3fbcd 39 float Vr_adc,output,Current_Ws;
oguro 0:d7338aa3fbcd 40 float zint=tau*1E-6; //40E-6 MaX
oguro 0:d7338aa3fbcd 41 Serial pc(USBTX,USBRX);
oguro 0:d7338aa3fbcd 42
oguro 0:d7338aa3fbcd 43 DigitalOut myled(LED1);
oguro 0:d7338aa3fbcd 44
oguro 0:d7338aa3fbcd 45 Timer uT;
oguro 0:d7338aa3fbcd 46 Timer vT;
oguro 0:d7338aa3fbcd 47 Timer wT;
oguro 0:d7338aa3fbcd 48
oguro 0:d7338aa3fbcd 49 Ticker zt;
oguro 0:d7338aa3fbcd 50
oguro 0:d7338aa3fbcd 51 short u[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
oguro 0:d7338aa3fbcd 52 short v[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
oguro 0:d7338aa3fbcd 53 short w[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2
oguro 0:d7338aa3fbcd 54
oguro 0:d7338aa3fbcd 55 void ztrans(){
oguro 0:d7338aa3fbcd 56
oguro 0:d7338aa3fbcd 57 u[2] = ((u[1]*u[4])>>14) + ((u[0]*u[3])>>14);
oguro 0:d7338aa3fbcd 58 u[4] = u[3];
oguro 0:d7338aa3fbcd 59 u[3] = u[2];
oguro 0:d7338aa3fbcd 60
oguro 0:d7338aa3fbcd 61 v[2] = ((v[1]*v[4])>>14) + ((v[0]*v[3])>>14);
oguro 0:d7338aa3fbcd 62 v[4] = v[3];
oguro 0:d7338aa3fbcd 63 v[3] = v[2];
oguro 0:d7338aa3fbcd 64
oguro 0:d7338aa3fbcd 65 w[2] = ((w[1]*w[4])>>14) + ((w[0]*w[3])>>14);
oguro 0:d7338aa3fbcd 66 w[4] = w[3];
oguro 0:d7338aa3fbcd 67 w[3] = w[2];
oguro 0:d7338aa3fbcd 68
oguro 0:d7338aa3fbcd 69 }
oguro 0:d7338aa3fbcd 70
oguro 0:d7338aa3fbcd 71 void HAH(){
oguro 0:d7338aa3fbcd 72 ut1=uT.read_us();
oguro 0:d7338aa3fbcd 73
oguro 0:d7338aa3fbcd 74 f1=(sin(2*3.14159*(1/(usi*1E-6))*zint)*16384); //125E-6
oguro 0:d7338aa3fbcd 75 a1=(2*cos(2*3.14159*(1/(usi*1E-6))*zint)*16384);
oguro 0:d7338aa3fbcd 76
oguro 0:d7338aa3fbcd 77 u[0]=a1;u[1]=0xC000;u[2]=0; u[3]=f1; u[4]=0;
oguro 0:d7338aa3fbcd 78 }
oguro 0:d7338aa3fbcd 79 void HAL(){
oguro 0:d7338aa3fbcd 80 ut2=uT.read_us();
oguro 0:d7338aa3fbcd 81 uT.reset();
oguro 0:d7338aa3fbcd 82 }
oguro 0:d7338aa3fbcd 83 void HBH(){
oguro 0:d7338aa3fbcd 84
oguro 0:d7338aa3fbcd 85 vt1=vT.read_us();
oguro 0:d7338aa3fbcd 86
oguro 0:d7338aa3fbcd 87 f1=(sin(2*3.14159*(1/(vsi*1E-6))*zint)*16384);
oguro 0:d7338aa3fbcd 88 a1=(2*cos(2*3.14159*(1/(vsi*1E-6))*zint)*16384);
oguro 0:d7338aa3fbcd 89
oguro 0:d7338aa3fbcd 90 v[0]=a1;v[1]=0xC000;v[2]=0; v[3]=f1; v[4]=0;
oguro 0:d7338aa3fbcd 91
oguro 0:d7338aa3fbcd 92 }
oguro 0:d7338aa3fbcd 93 void HBL(){
oguro 0:d7338aa3fbcd 94 vt2=vT.read_us();
oguro 0:d7338aa3fbcd 95 vT.reset();
oguro 0:d7338aa3fbcd 96 }
oguro 0:d7338aa3fbcd 97 void HCH(){
oguro 0:d7338aa3fbcd 98 wt1=wT.read_us();
oguro 0:d7338aa3fbcd 99
oguro 0:d7338aa3fbcd 100 f1=(sin(2*3.14159*(1/(wsi*1E-6))*zint)*16384);
oguro 0:d7338aa3fbcd 101 a1=(2*cos(2*3.14159*(1/(wsi*1E-6))*zint)*16384);
oguro 0:d7338aa3fbcd 102
oguro 0:d7338aa3fbcd 103 w[0]=a1;w[1]=0xC000;w[2]=0; w[3]=f1; w[4]=0;
oguro 0:d7338aa3fbcd 104 }
oguro 0:d7338aa3fbcd 105
oguro 0:d7338aa3fbcd 106 void HCL(){
oguro 0:d7338aa3fbcd 107 wt2=wT.read_us();
oguro 0:d7338aa3fbcd 108 wT.reset();
oguro 0:d7338aa3fbcd 109 }
oguro 0:d7338aa3fbcd 110
oguro 0:d7338aa3fbcd 111 void CPLT(){
oguro 0:d7338aa3fbcd 112 //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 0:d7338aa3fbcd 113 // pc.printf("%.3f , %.3f, %.3f\r" ,su,sv,sw);
oguro 0:d7338aa3fbcd 114 }
oguro 0:d7338aa3fbcd 115
oguro 0:d7338aa3fbcd 116 void timerTS1(void const*argument){
oguro 0:d7338aa3fbcd 117 CPLT();
oguro 0:d7338aa3fbcd 118 }
oguro 0:d7338aa3fbcd 119
oguro 0:d7338aa3fbcd 120
oguro 0:d7338aa3fbcd 121
oguro 0:d7338aa3fbcd 122 int main(){
oguro 0:d7338aa3fbcd 123
oguro 0:d7338aa3fbcd 124 pc.baud(128000);
oguro 0:d7338aa3fbcd 125
oguro 0:d7338aa3fbcd 126 zt.attach_us(&ztrans, tau); // 40MAX
oguro 0:d7338aa3fbcd 127
oguro 0:d7338aa3fbcd 128 EN1=1;
oguro 0:d7338aa3fbcd 129 EN2=1;
oguro 0:d7338aa3fbcd 130 EN3=1;
oguro 0:d7338aa3fbcd 131
oguro 0:d7338aa3fbcd 132 mypwmA.period_us(20);
oguro 0:d7338aa3fbcd 133 mypwmB.period_us(20);
oguro 0:d7338aa3fbcd 134 mypwmC.period_us(20);
oguro 0:d7338aa3fbcd 135
oguro 0:d7338aa3fbcd 136 uT.start();
oguro 0:d7338aa3fbcd 137 vT.start();
oguro 0:d7338aa3fbcd 138 wT.start();
oguro 0:d7338aa3fbcd 139
oguro 0:d7338aa3fbcd 140 RtosTimer RtosTimerTS1(timerTS1);
oguro 0:d7338aa3fbcd 141 RtosTimerTS1.start((unsigned int)(TS1*300));//5000
oguro 0:d7338aa3fbcd 142 Thread::wait(1);//100
oguro 0:d7338aa3fbcd 143
oguro 0:d7338aa3fbcd 144
oguro 0:d7338aa3fbcd 145 while(1){
oguro 0:d7338aa3fbcd 146
oguro 0:d7338aa3fbcd 147
oguro 0:d7338aa3fbcd 148 Vr_adc=V_adc.read();
oguro 0:d7338aa3fbcd 149
oguro 0:d7338aa3fbcd 150 if((Vr_adc>0.15f)&&(q==0)){
oguro 0:d7338aa3fbcd 151 while(q<50){
oguro 0:d7338aa3fbcd 152
oguro 0:d7338aa3fbcd 153 mypwmA.write(0); //0.8f
oguro 0:d7338aa3fbcd 154 mypwmB.write(0);
oguro 0:d7338aa3fbcd 155 mypwmC.write(0.7f);
oguro 0:d7338aa3fbcd 156 wait_ms(START);
oguro 0:d7338aa3fbcd 157
oguro 0:d7338aa3fbcd 158 mypwmA.write(0);
oguro 0:d7338aa3fbcd 159 mypwmB.write(0.7f);
oguro 0:d7338aa3fbcd 160 mypwmC.write(0);
oguro 0:d7338aa3fbcd 161 wait_ms(START);
oguro 0:d7338aa3fbcd 162
oguro 0:d7338aa3fbcd 163 mypwmA.write(0.7f);
oguro 0:d7338aa3fbcd 164 mypwmB.write(0);
oguro 0:d7338aa3fbcd 165 mypwmC.write(0);//0.8f
oguro 0:d7338aa3fbcd 166 wait_ms(START);
oguro 0:d7338aa3fbcd 167 q++;
oguro 0:d7338aa3fbcd 168 }
oguro 0:d7338aa3fbcd 169
oguro 0:d7338aa3fbcd 170 }
oguro 0:d7338aa3fbcd 171 HA.rise(&HAH);
oguro 0:d7338aa3fbcd 172 HC.fall(&HCL);
oguro 0:d7338aa3fbcd 173 HB.rise(&HBH);
oguro 0:d7338aa3fbcd 174 HA.fall(&HAL);
oguro 0:d7338aa3fbcd 175 HC.rise(&HCH);
oguro 0:d7338aa3fbcd 176 HB.fall(&HBL);
oguro 0:d7338aa3fbcd 177
oguro 0:d7338aa3fbcd 178 if(Vr_adc < 0.08f){
oguro 0:d7338aa3fbcd 179 q=0;
oguro 0:d7338aa3fbcd 180 }
oguro 0:d7338aa3fbcd 181
oguro 0:d7338aa3fbcd 182 usi=2*(ut2-ut1);
oguro 0:d7338aa3fbcd 183 vsi=2*(vt2-vt1);
oguro 0:d7338aa3fbcd 184 wsi=2*(wt2-wt1);
oguro 0:d7338aa3fbcd 185
oguro 0:d7338aa3fbcd 186
oguro 0:d7338aa3fbcd 187 if((u[2])<=-16383){ //飽和処理
oguro 0:d7338aa3fbcd 188 u[2]=-16383;
oguro 0:d7338aa3fbcd 189 }
oguro 0:d7338aa3fbcd 190 if(u[2]>=16383){
oguro 0:d7338aa3fbcd 191 u[2]=16383;
oguro 0:d7338aa3fbcd 192 }
oguro 0:d7338aa3fbcd 193
oguro 0:d7338aa3fbcd 194 if(v[2]<=-16383){ //飽和処理
oguro 0:d7338aa3fbcd 195 v[2]=-16383;
oguro 0:d7338aa3fbcd 196 }
oguro 0:d7338aa3fbcd 197 if(v[2]>=16383){
oguro 0:d7338aa3fbcd 198 v[2]=16383;
oguro 0:d7338aa3fbcd 199 }
oguro 0:d7338aa3fbcd 200 if(w[2]<=-16383){ //飽和処理
oguro 0:d7338aa3fbcd 201 w[2]=-16383;
oguro 0:d7338aa3fbcd 202 }
oguro 0:d7338aa3fbcd 203 if(w[2]>=16383){
oguro 0:d7338aa3fbcd 204 w[2]=16383;
oguro 0:d7338aa3fbcd 205 }
oguro 0:d7338aa3fbcd 206
oguro 0:d7338aa3fbcd 207 #if 1
oguro 0:d7338aa3fbcd 208 aout=(float(v[2])/(16383*2))*(Vr_adc)+0.5;
oguro 0:d7338aa3fbcd 209
oguro 0:d7338aa3fbcd 210 mypwmA.write(((float(u[2])/(16383*2)))*(Vr_adc)+0.5);
oguro 0:d7338aa3fbcd 211 mypwmB.write(((float(v[2])/(16383*2)))*(Vr_adc)+0.5);
oguro 0:d7338aa3fbcd 212 mypwmC.write(((float(w[2])/(16383*2)))*(Vr_adc)+0.5);
oguro 0:d7338aa3fbcd 213 #endif
oguro 0:d7338aa3fbcd 214
oguro 0:d7338aa3fbcd 215 #if 0
oguro 0:d7338aa3fbcd 216 su=(float(u[2])/(16383*2))*(Vr_adc);
oguro 0:d7338aa3fbcd 217 sv=(float(v[2])/(16383*2))*(Vr_adc);
oguro 0:d7338aa3fbcd 218 sw=(float(w[2])/(16383*2))*(Vr_adc);
oguro 0:d7338aa3fbcd 219
oguro 0:d7338aa3fbcd 220 mypwmA.write(su);
oguro 0:d7338aa3fbcd 221 mypwmB.write(sv);
oguro 0:d7338aa3fbcd 222 mypwmC.write(sw);
oguro 0:d7338aa3fbcd 223
oguro 0:d7338aa3fbcd 224 aout=su;
oguro 0:d7338aa3fbcd 225 //aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383
oguro 0:d7338aa3fbcd 226 #endif
oguro 0:d7338aa3fbcd 227
oguro 0:d7338aa3fbcd 228 Speed=60*(1/(7.0*usi*1E-6));
oguro 0:d7338aa3fbcd 229
oguro 0:d7338aa3fbcd 230 myled = !myled;
oguro 0:d7338aa3fbcd 231
oguro 0:d7338aa3fbcd 232 }
oguro 0:d7338aa3fbcd 233
oguro 0:d7338aa3fbcd 234
oguro 0:d7338aa3fbcd 235 }
oguro 0:d7338aa3fbcd 236
oguro 0:d7338aa3fbcd 237
oguro 0:d7338aa3fbcd 238