Diff: main.cpp

Revision:

13:791e20f1af43

Parent:

12:a4b17bb682eb

Child:

14:8e205264baa8

--- a/main.cpp	Fri Dec 21 22:06:56 2012 +0000
+++ b/main.cpp	Sun Mar 03 09:09:34 2013 +0000
@@ -1,5 +1,5 @@
-//  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. BD6211F) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121206 by Kosaka lab.
+//  UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. TA7291P) and incremental rotary encoder with A, B phase.
+//      ver. 130214 by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 
@@ -17,10 +17,6 @@
 
 extern "C" void mbed_reset();
 
-FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    //save data to PC
-Timeout emergencyStop;          // kill fprintf process when bug
-
-
 void CallTimerTS2(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
     int ms;
     unsigned long c;
@@ -66,11 +62,8 @@
 
 //#define OLD
 int main(){
-    int ms=1;
-    unsigned long c, c2;
     unsigned short  i, i2=0;
-//    FILE *fp;   // save data to PC
-//    FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); 
+    FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    // save data to PC
     char chr[100];
     RtosTimer RtosTimerTS1(timerTS1);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
     Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
@@ -85,32 +78,27 @@
 //  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
 //  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
 
-    // シミュレーション総サンプル数
-    int N;// = 5000;
     // 指令速度
     float  w_ref_req[2] = {20* 2*PI, 40* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
-    float  w_ref;
     // 指令ｄｑ電流位相
-    float  beta_ref = 30*PI/180;   // rad
+    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
     float  tan_beta_ref1;
     float  tan_beta_ref2,tan_beta_ref;
+    float  t;   // current time
 
 //    start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
 
-    N = (int)(TMAX/TS0);
-pc2.printf("N=%d\r\n",N);
     // IPMSMの機器定数等の設定, 制御器の初期化
     init_parameters();
     p.th[0] = 2*PI/3;   //θの初期値
 
-
 //  p.Lq0 = p.Ld;   // SPMSMの場合
 //  p.phi = 0;      // SynRMの場合
 
 //    w_ref=p.w;          // 速度指令[rad/s]
-    tan_beta_ref1 = tan(beta_ref);
-    tan_beta_ref2 = tan(beta_ref-30*PI/180);
-    tan_beta_ref = tan_beta_ref1;
+    tan_beta_ref1 = tan(beta_ref);           // tan30°を計算
+    tan_beta_ref2 = tan(beta_ref-30*PI/180); // tan0°を計算
+    tan_beta_ref = tan_beta_ref1;            // βの指令値をtan30°に
     // シミュレーション開始
     pc2.printf("Simulation start!!\r\n");
 #ifndef OLD
@@ -123,11 +111,11 @@
 #endif
     
     // set th by moving rotor to th=zero.
-    f_find_origin=1;
-    while( _count*TS0<0.1 ){  // while( time<1 ){
+    f_find_origin=1;    // 回転角原点復帰フラグをセット
+    while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){  // TMAX秒経過するまで制御実行
 //        debug_p24 = 1;
-        il.idq_ref[0] = iqMAX/1.0;
-        il.idq_ref[1] = 0;
+        il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
+        il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
 
 #ifdef OLD
         timerTS0();
@@ -137,15 +125,12 @@
 #endif
 
 //        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
-        Thread::wait(ms);
 //        debug_p24 = 0;
+        Thread::wait(1);
     }
 //pc2.printf("\r\n^0^ 0\r\n");
-#ifndef SIMULATION
-    encoder.reset();    // set encoder counter zero
-    p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
-#endif
-    c2 = _count;
+    // IPMSMの機器定数等の設定, 制御器の初期化
+    init_parameters();
     f_find_origin=0;
 
 #ifndef OLD
@@ -155,33 +140,36 @@
     RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
 #endif
 
-    for( i=0;i<N;i++ ){
+    while( (t = _count*TS0-TMAX_FIND_ORIGIN) < TMAX ){
 //        debug_p24 = 1;
-        c = _count-c2;
-        // 電流位相（ｄｑ軸電流）変化
-        // if( i>=1500&&i<1900 ){// TS0=0.0001
-        if( c>=1500*0.0001/TS0&&c<1900*0.0001/TS0 ){
-            if( tan_beta_ref>tan_beta_ref2 ){   tan_beta_ref=tan_beta_ref-0.002;}
-        }else{
-            if( tan_beta_ref<tan_beta_ref1){    tan_beta_ref=tan_beta_ref+0.002;}
+
+        // 電流位相（ｄｑ軸電流）を変化させるベクトル制御
+        if( t>=0.15 && t<0.19 ){  // 0.15～0.19秒の間に
+            if( tan_beta_ref>tan_beta_ref2 ){ // βの指令値が0度以上のとき
+                tan_beta_ref=tan_beta_ref-0.002;  // 指令値を小さくする
+            }
+        }else{                   // 0.15～0.19秒の間でないときに
+            if( tan_beta_ref<tan_beta_ref1){ // βの指令値が30度以下のとき
+                tan_beta_ref=tan_beta_ref+0.002;  // 指令値を大きくする
+            }
         }
+        // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
+        vl.tan_beta_ref = tan_beta_ref;
 
         // 速度急変
-        w_ref = w_ref_req[0];
-        if( 2600*0.0001/TS0<=c&&c<3000*0.0001/TS0 ){
-            w_ref=w_ref_req[1];
-//pc2.printf(".\r\n");
+        if( 0.26<=t && t<0.3 ){
+            vl.w_ref=w_ref_req[1];   // 目標速度をメインルーチンから速度制御メインループへ渡す。
+        }else{
+            vl.w_ref=w_ref_req[0];
         }
 #ifdef SIMULATION
         // 負荷トルク急変
-        if( c<4000*0.0001/TS0 ){
+        if( t<0.4 ){
             p.TL = 1;
         }else{
             p.TL = 2;
         }
 #endif
-        vl.w_ref = w_ref;               // 目標速度をメインルーチンから速度制御メインループへ渡す。
-        vl.tan_beta_ref = tan_beta_ref; // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
 
 #ifdef OLD
         if( (++i2)>=(int)(TS1/TS0) ){  i2=0;
@@ -197,8 +185,8 @@
 #endif
 
 //        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
-        Thread::wait(ms);
 //        debug_p24 = 0;
+        Thread::wait(1);         // 1ms待つ
     }
 //pc2.printf("\r\n^0^ 2\r\n");
     // stop timers (OFF)
@@ -225,6 +213,7 @@
 //       fprintf( fp, "%d, ", data[i][1]*10000);
 //       fprintf( fp, "\r\n");
 //
+//        pc2.printf("%f, %f, %f, %f, %f\r\n", 
 //        fprintf( fp, "%f, %f, %f, %f, %f\r\n", 
 //        data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]);  // save data to PC (para, y, time, u)
 //