akiyoshi oguro
/
Nucleo_Z_SIN_Full_BLDC
sin wave Ztransform BLDC
main.cpp
- Committer:
- oguro
- Date:
- 2020-11-13
- Revision:
- 0:d7338aa3fbcd
File content as of revision 0:d7338aa3fbcd:
#include "mbed.h" #include <math.h> #include "rtos.h" #define PI 3.14159265358979 #define TS1 0.01//0.2 int i=0,q=0; short f1=0,a1=0; int START=10;//8 float ut1=0,ut2=0,usi=0; float vt1=0,vt2=0,vsi=0; float wt1=0,wt2=0,wsi=0; float Speed; float tau=100; //60 float su,sv,sw; PwmOut mypwmA(PA_8); //PWM_OUT 8 PwmOut mypwmB(PA_9); //9 PwmOut mypwmC(PA_10);//10 DigitalOut EN1(PC_10); DigitalOut EN2(PC_11); DigitalOut EN3(PC_12); InterruptIn HA(PA_15); InterruptIn HB(PB_3); InterruptIn HC(PB_10); AnalogOut aout(PA_4); //AnalogIn V_adc(PB_1);//Volume AnalogIn V_adc(PC_2); // gaibu Volume AnalogIn Current_W(PC_0); float Vr_adc,output,Current_Ws; float zint=tau*1E-6; //40E-6 MaX Serial pc(USBTX,USBRX); DigitalOut myled(LED1); Timer uT; Timer vT; Timer wT; Ticker zt; short u[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 short v[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 short w[5]={0x7FFF,0xC000,0,0x0001,0};//a1,a2,f0,f1,f2 void ztrans(){ u[2] = ((u[1]*u[4])>>14) + ((u[0]*u[3])>>14); u[4] = u[3]; u[3] = u[2]; v[2] = ((v[1]*v[4])>>14) + ((v[0]*v[3])>>14); v[4] = v[3]; v[3] = v[2]; w[2] = ((w[1]*w[4])>>14) + ((w[0]*w[3])>>14); w[4] = w[3]; w[3] = w[2]; } void HAH(){ ut1=uT.read_us(); f1=(sin(2*3.14159*(1/(usi*1E-6))*zint)*16384); //125E-6 a1=(2*cos(2*3.14159*(1/(usi*1E-6))*zint)*16384); u[0]=a1;u[1]=0xC000;u[2]=0; u[3]=f1; u[4]=0; } void HAL(){ ut2=uT.read_us(); uT.reset(); } void HBH(){ vt1=vT.read_us(); f1=(sin(2*3.14159*(1/(vsi*1E-6))*zint)*16384); a1=(2*cos(2*3.14159*(1/(vsi*1E-6))*zint)*16384); v[0]=a1;v[1]=0xC000;v[2]=0; v[3]=f1; v[4]=0; } void HBL(){ vt2=vT.read_us(); vT.reset(); } void HCH(){ wt1=wT.read_us(); f1=(sin(2*3.14159*(1/(wsi*1E-6))*zint)*16384); a1=(2*cos(2*3.14159*(1/(wsi*1E-6))*zint)*16384); w[0]=a1;w[1]=0xC000;w[2]=0; w[3]=f1; w[4]=0; } void HCL(){ wt2=wT.read_us(); wT.reset(); } void CPLT(){ //pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); // pc.printf("%.3f , %.3f, %.3f\r" ,su,sv,sw); } void timerTS1(void const*argument){ CPLT(); } int main(){ pc.baud(128000); zt.attach_us(&ztrans, tau); // 40MAX EN1=1; EN2=1; EN3=1; mypwmA.period_us(20); mypwmB.period_us(20); mypwmC.period_us(20); uT.start(); vT.start(); wT.start(); RtosTimer RtosTimerTS1(timerTS1); RtosTimerTS1.start((unsigned int)(TS1*300));//5000 Thread::wait(1);//100 while(1){ Vr_adc=V_adc.read(); if((Vr_adc>0.15f)&&(q==0)){ while(q<50){ mypwmA.write(0); //0.8f mypwmB.write(0); mypwmC.write(0.7f); wait_ms(START); mypwmA.write(0); mypwmB.write(0.7f); mypwmC.write(0); wait_ms(START); mypwmA.write(0.7f); mypwmB.write(0); mypwmC.write(0);//0.8f wait_ms(START); q++; } } HA.rise(&HAH); HC.fall(&HCL); HB.rise(&HBH); HA.fall(&HAL); HC.rise(&HCH); HB.fall(&HBL); if(Vr_adc < 0.08f){ q=0; } usi=2*(ut2-ut1); vsi=2*(vt2-vt1); wsi=2*(wt2-wt1); if((u[2])<=-16383){ //飽和処理 u[2]=-16383; } if(u[2]>=16383){ u[2]=16383; } if(v[2]<=-16383){ //飽和処理 v[2]=-16383; } if(v[2]>=16383){ v[2]=16383; } if(w[2]<=-16383){ //飽和処理 w[2]=-16383; } if(w[2]>=16383){ w[2]=16383; } #if 1 aout=(float(v[2])/(16383*2))*(Vr_adc)+0.5; mypwmA.write(((float(u[2])/(16383*2)))*(Vr_adc)+0.5); mypwmB.write(((float(v[2])/(16383*2)))*(Vr_adc)+0.5); mypwmC.write(((float(w[2])/(16383*2)))*(Vr_adc)+0.5); #endif #if 0 su=(float(u[2])/(16383*2))*(Vr_adc); sv=(float(v[2])/(16383*2))*(Vr_adc); sw=(float(w[2])/(16383*2))*(Vr_adc); mypwmA.write(su); mypwmB.write(sv); mypwmC.write(sw); aout=su; //aout=(float(wz[2])/(16383*2))*(power*STOP)+0.5;//16383 #endif Speed=60*(1/(7.0*usi*1E-6)); myled = !myled; } }