Diff: main.cpp

Revision:

14:1196c2d455ae

Parent:

13:4116d4b6c2a5

Child:

15:744a81d5b7ac

--- a/main.cpp	Sun Dec 02 07:28:39 2012 +0000
+++ b/main.cpp	Sun Dec 02 10:03:09 2012 +0000
@@ -22,9 +22,11 @@
 float   _Kp4i=10.0;            // P gain for PID from motor volt. to motor current.
 float   _Ki4i=10.0;            // I gain for PID from motor volt. to motor current.
 float   _Kd4i=0.0;             // D gain for PID from motor volt. to motor current.
-#define iTS     0.0001       // [s], iTS, sampling time[s] of motor current i control PID using timer interrupt
-#define thTS    0.001        // [s], thTS>=0.001[s], sampling time[s] of motor angle th PID using rtos-timer
-#define TS2     0.01        // [s], TS2>=0.001[s], sampling time[s] to save data to PC using thread. But, max data length is 1000.
+#define TS0     0.0001      // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
+#define TS1     0.001       // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
+#define TS2     0.01        // [s], sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
+#define TS3     0.05        // [s], sampling time (priority low: precision 4ms)
+#define TS4     0.1         // [s], sampling time (priority lowest: precision 4ms)  to display data to PC tera term
 #define TMAX    10          // [s], experiment starts from 0[s] to TMAX[s]
 #define UMAX    3.3         // [V], max of control input u
 #define UMIN   -3.3         // [V], max of control input u
@@ -95,7 +97,7 @@
 #ifdef  GOOD_DATA
 float data[1000][5];    // memory to save data offline instead of "online fprintf".
 unsigned int    count3; // 
-unsigned int    count2=(int)(TS2/iTS); // 
+unsigned int    count2=(int)(TS2/TS0); // 
 #endif
 
 extern "C" void mbed_reset();
@@ -149,7 +151,7 @@
 
 //    y_old = _th;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
 #ifdef SIMULATION
-    y = _th + thTS/0.1*(0.2*_iref*100-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 0.2/(0.1s+1)
+    y = _th + TS1/0.1*(0.2*_iref*100-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 0.2/(0.1s+1)
 #else
 //    semaphore1.wait();      //
     y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
@@ -171,10 +173,10 @@
     }
     if( _f_imax==0 ){   // u is saturated?
 //        if( _e>((360.0/N_ENC)/180*PI) || _e<-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
-            _eI = _eI + thTS*_e;     // integral of e(t)
+            _eI = _eI + TS1*_e;     // integral of e(t)
 //        }
     }
-    u = _Kp4th*_e + _Kd4th*(_e-e_old)/thTS + _Ki4th*_eI;   // PID output u(t)
+    u = _Kp4th*_e + _Kd4th*(_e-e_old)/TS1 + _Ki4th*_eI;   // PID output u(t)
  
 #if CONTROL_MODE==1||CONTROL_MODE==2   // frequency response, or Step response
     wt = _freq_u *2.0*PI*_time;
@@ -186,14 +188,14 @@
     else        u = UMIN;
 #endif
 #if CONTROL_MODE==3                 // u=rand() to identify motor transfer function G(s) from V to angle
-    if(count2==(int)(TS2/iTS)){
+    if(count2==(int)(TS2/TS0)){
         u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
     }else{
         u = _iref;
     }
 #endif
 #if CONTROL_MODE==4                 // FFT identification, u=repetive signal
-    if(count2==(int)(TS2/thTS)){
+    if(count2==(int)(TS2/TS1)){
         u = data[count3][4];
     }else{
         u = _iref;
@@ -233,9 +235,9 @@
     e_old = _ei;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
     _ei = _iref - y;    // error e(t)
     if( _f_umax==0 ){
-        _eiI = _eiI + iTS*_ei;     // integral of e(t)
+        _eiI = _eiI + TS0*_ei;     // integral of e(t)
     }
-    u = _Kp4i*_e + _Kd4i*(_ei-e_old)/iTS + _Ki4i*_eiI;   // PID output u(t)
+    u = _Kp4i*_e + _Kd4i*(_ei-e_old)/TS0 + _Ki4i*_eiI;   // PID output u(t)
 
      // u is saturated? for anti-windup
     if( u>UMAX ){
@@ -259,10 +261,10 @@
     u2Hbridge(_u);  // input u to TA7291 driver
 
     //-------- update data
-    _time += iTS;    // time
+    _time += TS0;    // time
 debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
 #ifdef  GOOD_DATA
-    if(count2==(int)(TS2/iTS)){
+    if(count2==(int)(TS2/TS0)){
 //        j=0; if(_count>=j&&_count<j+1000){i=_count-j;  data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_th; data[i][3]=_time; data[i][4]=_u;}
         if( count3<1000 ){
             data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_th; data[count3][3]=_time; data[count3][4]=_u;
@@ -290,7 +292,7 @@
 #endif
 }
 void moror_control() {  // motor control ON for TMAX seconds.
-    RtosTimer timer_controller(th_controller);
+    RtosTimer timer_controller(th_controller);  // RtosTimer priority is osPriorityAboveNormal, just one above main()
     FILE *fp;   // save data to PC
     float   t=0;
 #ifdef  GOOD_DATA
@@ -305,9 +307,10 @@
     if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
 
     // start control (ON)
-    controller_ticker.attach(&i_controller, iTS ); // Sampling period[s] of i_controller
-    timer_controller.start((unsigned int)(thTS*1000.));   // Sampling period[ms] of th controller
+    controller_ticker.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
+    timer_controller.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller
 
+    t = _time;
     while ( _time <= TMAX ) {
         // BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
         //     fprintf returns before process completed.
@@ -329,16 +332,34 @@
     fclose( fp );               // release mbed USB drive
 }
 
-void thread_print2PC(void const *argument) {    // print to PC (tera term)
+void display2PC(){  // display to tera term on PC
+    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT);  // print to tera term
+}
+void TS3timer(void const *argument) {   // make sampling time TS4 timer (priority low: precision 4ms)
+    int ms;
+    unsigned long c;
     while (true) {
-        pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT);  // print to tera term
-        Thread::wait(1);
+        c = _count;
+        //dummy();  // dummy() is called every TS3[s].
+        if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
+    }
+}
+
+void TS4timer(void const *argument) {   // make sampling time TS4 timer (priority lowest: precision 4ms)
+    int ms;
+    unsigned long c;
+    while (true) {
+        c = _count;
+        display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
+        if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
+        Thread::wait(ms);
     }
 }
 
 int main() {
-    Thread print2PC(thread_print2PC,NULL,osPriorityBelowNormal);
-//    osStatus set_priority(osPriority osPriorityBelowNormal );
+    Thread startTS3timer(TS3timer,NULL,osPriorityBelowNormal);
+    Thread startTS4timer(TS4timer,NULL,osPriorityLow);
 // Priority of Thread (RtosTimer has no priority?)
 //  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
 //  osPriorityLow           = -2,          ///< priority: low