Diff: controller.cpp

Revision:

15:427f5ae8e957

Parent:

14:8e205264baa8

Child:

16:d05404eef8ee

--- a/controller.cpp	Thu Jun 13 06:41:16 2013 +0000
+++ b/controller.cpp	Tue Sep 03 07:40:50 2013 +0000
@@ -1,5 +1,5 @@
 //  controller.cpp: 各種３相同期モータに対するセンサあり運転のシミュレーション
-//      Kosaka Lab. 130214
+//      Kosaka Lab. 130903
 #include "mbed.h"
 #include "QEI.h"
 
@@ -145,8 +145,7 @@
 void current_loop(){    // 電流制御マイナーループ
     float  th, c, s, Cdq[2][2], iu, iv, iab[2], idq_act[2], vab_ref[2], tmp, prev[2];
   if( f_find_origin==1 ){
-//    th = 0;
-    th = debug[2];
+    th = p.th_const;
   }else{
     th = p.th[0];
   }
@@ -208,10 +207,12 @@
 il.vdq_ref[0]=0;
 il.vdq_ref[1]=vdqMAX;
 #endif
+//analog_out=il.vdq_ref[1]/3.3+0.4;//koko
     // ｄｑ座標指令電圧 vd_ref, vq_refからiα, iβを計算
     // vab_ref = Cdq'*vdq_ref;
     vab_ref[0] = Cdq[0][0]*il.vdq_ref[0] + Cdq[1][0]*il.vdq_ref[1];
     vab_ref[1] = Cdq[0][1]*il.vdq_ref[0] + Cdq[1][1]*il.vdq_ref[1];
+//analog_out=vab_ref[1]/3.3+0.4;
 
     // モータに印加するUVW相電圧を計算 （vα, vβからvu, vv, vwを計算）
     //  vu = √(2/3)*va;
@@ -256,6 +257,8 @@
     while( tmp> PI ){ tmp -= 2*PI;}
     while( tmp<-PI ){ tmp += 2*PI;}
     vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF);
+tmp=vl.w_lpf/(2*PI) /20; if(tmp>1) tmp=1;else if(tmp<0) tmp=0;
+analog_out=tmp;//tmp;//koko
 
     // 速度制御：速度偏差が入力され、q軸電流指令を出力。
 //  [iq_ref,eI] = vel_control(w_ref,w_act,eI,ts);
@@ -280,6 +283,10 @@
     // ｄｑ軸電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
     il.idq_ref[0] = idq_ref[0];
     il.idq_ref[1] = idq_ref[1];
+  if( f_find_origin==1 ){
+    il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
+    il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
+  }
 }
 
 void    vuvw2pwm(){ // vu, vv, vwより、UVW相の上アームPWMを発生
@@ -424,12 +431,16 @@
 #ifdef DOUKI
 led1=1;
 p.th[0] += 2*PI*TS0 * 1; if(p.th[0]>4*PI){   p.th[0]-=4*PI;}
-debug[0]=p.th[0]/PI*180;
+//debug[0]=p.th[0]/PI*180;
 analog_out=debug[0]/180*PI/4/PI;
 led1=0;
 #else
     // 位置θをセンサで検出
     p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
+debug[0]=p.th[0]/PI*180;
+debug[1]=p.th[0]/(2*PI);    debug[1]=debug[1]-(int)debug[1];    if(debug[1]<0)  debug[1]+=1;
+debug[0]=debug[1]*360;
+//analog_out=debug[1];
 #endif
  #endif
     current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)