akiyoshi oguro
CCW CW Control BLDC
Diff: main.cpp
- Revision:
- 0:faa58403944a
- Child:
- 1:333d2cdd26d0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Feb 17 02:58:08 2019 +0000
@@ -0,0 +1,306 @@
+//Vector制御 工事中 2月末 公開予定
+//矩形波かから正弦波に移行 Speedの範囲を118行、173行目を
+//変更して矩形波→正弦波 への移行回転数を制御します。
+// ボリュームを中間値設定後、電源投入またはリセットしてください
+//ボリューム前後操作で正転/逆転を制御できます。
+
+#include "mbed.h"
+#include <math.h>
+
+#include <IO_define.h>
+#include <kukei.h>
+#include <sin.h>
+#include <cos.h>
+#include <vector.h>
+
+
+int main(){
+ // pc.baud(128000);
+ /* RtosTimer RtosTimerTS1(timerTS1);
+ RtosTimerTS1.start((unsigned int)(TS1*5000));//2000
+ Thread::wait(100);//1000*/
+
+ uT.start();
+ vT.start();
+ wT.start();
+
+ PWM_IN1_U.period_us(50);
+ PWM_IN2_V.period_us(50);
+ PWM_IN3_W.period_us(50);
+
+ zt.attach_us(&ztr, tau);
+ ztc.attach_us(&ztrc,tau);
+
+ Vr_adc_i=V_adc.read();
+ wait_ms(100);
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ EN1_U=1;
+ EN2_V=1;
+ EN3_W=1;
+
+while(1){
+
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+
+ Vr_adc=V_adc.read();
+ y=Vr_adc- Vr_adc_i;
+ ay=fabs(y);
+ power=ay;
+ if(y>0.1){
+ frd=0;
+ //aout=0.5;
+ }
+ if(y<-0.1){
+ frd=1;
+ //aout=0.9;
+ }
+
+ if(ay<0.1){
+ q=0;
+ aout=0.0;
+ //SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ //power=0.0;
+ }
+ /************起動********************/
+if(((y<-0.1)||(y>0.1))&&(q==0)){
+//if(q==0){
+ while(q<20){
+ SH=HALL_U<<2|HALL_V<<1|HALL_W;
+
+/*****Forword 起動******/
+ if((frd==0)){
+ //SH=HALL_U<<2|HALL_V<<1|HALL_W;
+ if(SH==5){
+ start_F5();
+ }
+ if(SH==4){
+ start_F4();
+ }
+ if(SH==6){
+ start_F6();
+ }
+ if(SH==2){
+ start_F2();
+ }
+ if(SH==3){
+ start_F3();
+ }
+ if(SH==1){
+ start_F1();
+ }
+ }
+
+/*******Reversal 起動************/
+ if((frd==1)){
+ //SH=HALL_U<2|HALL_V<<1|HALL_W;
+ if(SH==3){
+ start_R3();
+ }
+ if(SH==2){
+ start_R2();
+ }
+ if(SH==6){
+ start_R6();
+ }
+ if(SH==4){
+ start_R4;
+ }
+ if(SH==5){
+ start_R5();
+ }
+ if(SH==1){
+ start_R1();
+ }
+ }
+ q++;
+ }
+ }
+
+ if(Speed<1000){//2000
+ /**************矩形波駆動始動***************/
+ /*******Forward******************/
+ if(frd==0){
+ switch(SH){
+ case 5: PWM_W();//W
+ break;
+ case 4: EN_W();//W
+ break;
+ case 6: PWM_U();//U
+ break;
+ case 2: EN_U();//U
+ break;
+ case 3: PWM_V();//V
+ break;
+ case 1: EN_V();//V
+ break;
+ default :PWM_W();//W
+ break;
+ }
+ HALL_Ui.rise(time_WP);
+ HALL_Wi.fall(time_VN);
+ HALL_Vi.rise(time_UP);
+ HALL_Ui.fall(time_WN);
+ HALL_Wi.rise(time_VP);
+ HALL_Vi.fall(time_UN);
+ }
+/*******Reversal******************/
+ if(frd==1){
+ switch(SH){
+ case 5: PWM_W();
+ break;
+ case 1: EN_W();
+ break;
+ case 3: PWM_V();
+ break;
+ case 2: EN_V();
+ break;
+ case 6: PWM_U();
+ break;
+ case 4: EN_U();
+ break;
+ default :PWM_W();
+ break;
+ }
+ HALL_Ui.rise(time_UP);
+ HALL_Wi.fall(time_WN);
+ HALL_Vi.rise(time_VP);
+ HALL_Ui.fall(time_UN);
+ HALL_Wi.rise(time_WP);
+ HALL_Vi.fall(time_VN);
+ }
+
+ }//Speed<2000
+
+ if((Speed>=1000)){//&&(Speed<4000)){//2000
+/************sin drive Forward************/
+if(frd==1){
+ HALL_Ui.rise(&PWM_sinU);
+ HALL_Wi.fall(&EN_sinW);
+ HALL_Vi.rise(&PWM_sinV);
+ HALL_Ui.fall(&EN_sinU);
+ HALL_Wi.rise(&PWM_sinW);
+ HALL_Vi.fall(&EN_sinV);
+
+ }
+
+/*******sin drive Reversal******************/
+if(frd==0){
+ HALL_Ui.rise(&PWM_sinW);
+ HALL_Wi.fall(&EN_sinV);
+ HALL_Vi.rise(&PWM_sinU);
+ HALL_Ui.fall(&EN_sinW);
+ HALL_Wi.rise(&PWM_sinV);
+ HALL_Vi.fall(&EN_sinU);
+
+ }
+ /****************************/
+
+ if((uz[2])<=-16383){ //飽和処理
+ uz[2]=-16383;
+ }
+ if(uz[2]>=16383){
+ uz[2]=16383;
+ }
+
+ if(vz[2]<=-16383){ //飽和処理
+ vz[2]=-16383;
+ }
+ if(vz[2]>=16383){
+ vz[2]=16383;
+ }
+ if(wz[2]<=-16383){ //飽和処理
+ wz[2]=-16383;
+ }
+ if(wz[2]>=16383){
+ wz[2]=16383;
+ }
+ PWM_IN1_U.write(((float(uz[2])/(16383*2))*(power*STOP))+0.5);
+ PWM_IN2_V.write(((float(vz[2])/(16383*2))*(power*STOP))+0.5);
+ PWM_IN3_W.write(((float(wz[2])/(16383*2))*(power*STOP))+0.5);
+
+ aout=(float(uz[2])/(16383*2))*(power*STOP)+0.5;//16383
+ //filterCurrent();
+ //aout=Curr_wf;
+ //HALL_Ui.rise(&cosU);
+ //HALL_Ui.fall(&cosUN);
+
+}//sin
+
+//if(Speed>=4000){
+
+ //HALL_Ui.rise(&PWM_sinU);
+ //HALL_Ui.fall(&EN_sinU);
+ // HALL_Ui.rise(&cosU);
+ // HALL_Ui.fall(&cosUN);
+
+ //filterCurrent();
+
+
+ // sinth=(float(uz[2])/(16383*2))+0.5;
+ //costh=(float(uc[2])/(16383*2))+0.5;
+
+ //aout=sinth;
+
+ /* ia=(iu+iv*cos23+iw*cos43)*zet;
+ ib=(iv*sin23+iw*sin43)*zet;
+
+ id=costh*ia+sinth*ib;
+ iq=-sinth*ia+costh*ib;
+
+ id_p=0;
+ iq_p=Vr_adc;
+ id_diff=id_p-id;
+ iq_diff=iq_p-iq;
+
+ */
+
+ /********PI Control id,iq*********************/
+ /* s_ki_id += ki_id*id_diff;
+ Vd = s_ki_id + kp_id*id_diff;
+
+ s_ki_iq += ki_iq*(iq_diff+Vr_adc);
+ Vq = s_ki_iq + kp_iq*(iq_diff+Vr_adc);*/
+ /*******************************************/
+/****** detect rotor position*************/
+ /* Wz=(2*PI)/(2*vst*1E-6);
+ Ed= (Vq)-0.11f*iq-Wz*0.018E-3*id;
+ phm=Ed/(Wz);
+ dth=(Vd-0.11f*(id)+Wz*0.018E-3*(iq))/(Wz*phm);
+ derrth=asin(dth);*/
+
+ /*****PID error θ *****/
+
+ /* float ki_PLL=0.01;
+ float kp_PLL=0.2;
+ s_ki_errth += ki_PLL*derrth;
+ therr = s_ki_errth + kp_PLL*errth;*/
+
+ /*******PLL W ***********/
+ /* if(therr>0.01){
+ W=(2*PI)/((2*vst*1E-6)+(therr/Wz));
+ }
+ if(therr<-0.01){
+ W=(2*PI)/((2*vst*1E-6)-(therr/Wz));
+ } */
+ /************************************/
+ /* th += W*zint;
+ Va=cos(th)*Vd-sin(th)*Vq;
+ Vb=sin(th)*Vd+cos(th)*Vq;
+
+ SVPWM();
+
+ aout=iq;
+
+
+ }//vector*/
+/*******************************************************************/
+ usi=2*(ut2-ut1);
+ vsi=2*(vt2-vt1);
+ wsi=2*(wt2-wt1);
+ usic=2*(ut2c-ut1c);
+ Speed=60*(1/(6.0*fabs(usi)*1E-6));//CQ 6
+ /************************************************************/
+
+
+ }
+}
\ No newline at end of file