Diff: main.cpp

Revision:

13:4116d4b6c2a5

Parent:

12:9747752435d1

Child:

14:1196c2d455ae

--- a/main.cpp	Thu Nov 29 09:25:56 2012 +0000
+++ b/main.cpp	Sun Dec 02 07:28:39 2012 +0000
@@ -1,5 +1,5 @@
 //  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121129a by Kosaka lab.
+//      ver. 121202a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
@@ -278,61 +278,76 @@
     debug_p17 = 0;  // for debug: processing time check
 }
 
-void main1() {
-    RtosTimer timer_controller(th_controller);
-    FILE *fp;   // save data to PC
-#ifdef  GOOD_DATA
-    int i;
-
-    count3=0;
-#endif
+void init_controller(){ // initialize controller parameters and signals
     u2Hbridge(0);           // initialize H bridge to stop mode
     _count=0;
     _time = 0;  // time
     _eI = _eiI = 0;    // reset integrater
     encoder.reset();    // set encoder counter zero
     _th = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
+#ifdef  USE_PWM
+    FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
+#endif
+}
+void moror_control() {  // motor control ON for TMAX seconds.
+    RtosTimer timer_controller(th_controller);
+    FILE *fp;   // save data to PC
+    float   t=0;
+#ifdef  GOOD_DATA
+    int i;
+
+    count3=0;
+#endif
+    init_controller(); // initialize controller parameters and signals
     _r = _r + _th;
 //    if( _r>2*PI )    _r -= _r-2*PI;
 
-    pc.printf("Control start!!\r\n");
     if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC
-#ifdef  USE_PWM
-    FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
-#endif
+
+    // 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
 
-//    for ( i = 0; i < (unsigned int)(TMAX/iTS2); i++ ) {
     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.
 //BUG   fprintf( fp, "%8.2f, %8.4f,\t%8.1f,\t%8.2f\r\n", disp[3], disp[1], disp[0], tmp);  // save data to PC (para, y, time, u)
-//OK?   fprintf( fp, "%f, %f, %f, %f, %f\r\n", _time, debug[0], debug[3], (_y/(2*PI)*360.0),_u);  // save data to PC (para, y, time, u)
 #ifndef GOOD_DATA
         fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u);  // save data to PC (para, y, time, u)
 #endif
-        Thread::wait((unsigned int)(TS2*1000.));  //[ms]
+        Thread::wait((unsigned int)((TS2-(_time-t))*1000.));  //[ms]
+        t = _time;
     }
+    // stop control (OFF)
     controller_ticker.detach(); // timer interrupt stop
     timer_controller.stop();    // rtos timer stop
-    u2Hbridge(0);           // initialize H bridge to stop mode
-    _eI = _eiI = 0;    // reset integrater
+
+    init_controller(); // initialize controller parameters and signals
 #ifdef  GOOD_DATA
     for(i=0;i<1000;i++){  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)
 #endif
     fclose( fp );               // release mbed USB drive
-    pc.printf("Control completed!!\r\n\r\n");
 }
 
-void thread_print2PC(void const *argument) {
+void thread_print2PC(void const *argument) {    // print to PC (tera term)
     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(200);
+        Thread::wait(1);
     }
 }
 
-void main2(void const *argument) {
+int main() {
+    Thread print2PC(thread_print2PC,NULL,osPriorityBelowNormal);
+//    osStatus set_priority(osPriority osPriorityBelowNormal );
+// Priority of Thread (RtosTimer has no priority?)
+//  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
+//  osPriorityLow           = -2,          ///< priority: low
+//  osPriorityBelowNormal   = -1,          ///< priority: below normal
+//  osPriorityNormal        =  0,          ///< priority: normal (default)
+//  osPriorityAboveNormal   = +1,          ///< priority: above normal
+//  osPriorityHigh          = +2,          ///< priority: high 
+//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
+//  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
 #if CONTROL_MODE==0     // PID control
     char    f;
     float   val;
@@ -360,8 +375,11 @@
             data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
         }
 #endif
-        main1();
+        pc.printf("Control start!!\r\n");
+        moror_control();    // motor control ON for TMAX seconds.
+        pc.printf("Control completed!!\r\n\r\n");
 
+        // Change parameters using tera term
 #if CONTROL_MODE>=1     // frequency response, or Step response
         pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
         pc.scanf("%f",&_freq_u);
@@ -379,7 +397,7 @@
         f=pc.getc()-48; //int = char-48
         pc.printf("\r\n    Value?... ");
         if(f>=1&&f<=8){ pc.scanf("%f",&val);}
-        pc.printf("%8.3f\r\n", val);  // print to tera term
+        pc.printf("%8.3f\r\n", val);
         if(f==1){    _Kp4th = val;}
         if(f==2){    _Ki4th = val;}
         if(f==3){    _Kd4th = val;}
@@ -394,19 +412,3 @@
 #endif
     }    
 }
-int main() {
-//    void main1();
-    Thread save2PC(main2,NULL,osPriorityBelowNormal);
-    Thread print2PC(thread_print2PC,NULL,osPriorityLow);
-
-//    osStatus set_priority(osPriority osPriorityBelowNormal );
-// Priority of Thread (RtosTimer has no priority?)
-//  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
-//  osPriorityLow           = -2,          ///< priority: low
-//  osPriorityBelowNormal   = -1,          ///< priority: below normal
-//  osPriorityNormal        =  0,          ///< priority: normal (default)
-//  osPriorityAboveNormal   = +1,          ///< priority: above normal
-//  osPriorityHigh          = +2,          ///< priority: high 
-//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
-//  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
-}