Diff: main.cpp

Revision:

15:427f5ae8e957

Parent:

14:8e205264baa8

Child:

17:1ac855d69c78

--- a/main.cpp	Thu Jun 13 06:41:16 2013 +0000
+++ b/main.cpp	Tue Sep 03 07:40:50 2013 +0000
@@ -1,5 +1,5 @@
 //  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.
+//      ver. 130903 by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 
@@ -64,7 +64,7 @@
 int main(){
     unsigned short  i, i2=0;
     FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");    // save data to PC
-    char chr[100];
+//    char chr[100];
     RtosTimer RtosTimerTS1(timerTS1);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
     Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
     Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
@@ -80,12 +80,14 @@
 
     // 指令速度
 //    float  w_ref_req[2] = {20* 2*PI, 40* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
-    float  w_ref_req[2] = {0.5* 2*PI, 0.5* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
+    float  w_ref_req[2] = {10* 2*PI, 10* 2*PI};        // [rad/s]（第2要素は指令速度急変後の指令速度）
     // 指令ｄｑ電流位相
-    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
+//    float  beta_ref = 30*PI/180;   // [rad], 電流位相指令βを30度に
+    float  beta_ref = 85*PI/180;   // [rad], 電流位相指令βを30度に
     float  tan_beta_ref1;
     float  tan_beta_ref2,tan_beta_ref;
     float  t;   // current time
+    extern void    velocity_loop();
 
 //    start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
 
@@ -106,7 +108,7 @@
     // start control (ON)
     start_pwm();
     TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
-//    RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
+    RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
     fTimerTS3ON = 1;    // timerTS3 start
     fTimerTS4ON = 1;    // timerTS3 start
 #endif
@@ -114,24 +116,17 @@
     // set th by moving rotor to th=zero.
     f_find_origin=1;    // 回転角原点復帰フラグをセット
     while( (t = _count*TS0) < TMAX_FIND_ORIGIN ){  // TMAX秒経過するまで制御実行
-//        debug_p24 = 1;
-        il.idq_ref[0] = iqMAX/1.0;  // idをプラス、iqをゼロにして、
-        il.idq_ref[1] = 0;          // 無負荷のときにθ=0とさせる。
-
-//pc2.scanf("%f",&debug[2]);
-debug[2]=3*2*PI*t*1;
+        if     (t<1)    p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
+        else if(t<3)    p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
+        else if(t<5)    p.th_const = 15*2*PI*t*1;       // Set theta with constant angular velosity
+        else if(t<TMAX_FIND_ORIGIN-2)
+                        p.th_const =  9*2*PI*t*1;       // Set theta with constant angular velosity
+        else            p.th_const =  0*2*PI*t*1;       // Set theta with constant angular velosity
 #ifdef OLD
         timerTS0();
-        //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
-        //vuvw2pwm();     // vuvw to pwm
-        //sim_motor();    // IPM, ｄｑ座標
 #endif
-
-//        if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}// ms=TS0
-//        debug_p24 = 0;
         Thread::wait(1);
     }
-//pc2.printf("\r\n^0^ 0\r\n");
     // IPMSMの機器定数等の設定, 制御器の初期化
     init_parameters();
     f_find_origin=0;
@@ -159,12 +154,16 @@
         // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
         vl.tan_beta_ref = tan_beta_ref;
 
+#if 0
         // 速度急変
         if( 0.26<=t && t<0.3 ){
             vl.w_ref=w_ref_req[1];   // 目標速度をメインルーチンから速度制御メインループへ渡す。
         }else{
             vl.w_ref=w_ref_req[0];
         }
+#else
+        vl.w_ref=0.999*vl.w_ref + (1-0.999)*w_ref_req[0];
+#endif
 #ifdef SIMULATION
         // 負荷トルク急変
         if( t<0.4 ){