Diff: main.cpp

Revision:

1:b91aeb5673f3

Parent:

0:fe068497f773

Child:

2:e056793d6fc5

--- a/main.cpp	Thu Nov 15 06:18:51 2012 +0000
+++ b/main.cpp	Thu Nov 15 07:51:42 2012 +0000
@@ -1,5 +1,5 @@
-//  DC motor control program using TA7291P driver and 360 resolution rotary encoder with A, B phase.
-//      ver. 121115 by Kosaka lab.
+//  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
+//      ver. 121115a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
@@ -8,14 +8,14 @@
 #define SIMULATION          // Comment this line if not simulation
 #define CONTROL_MODE    0   // 0:PID control, 1:Frequency response, 2:Step response
 #define GOOD_DATA           // Comment this line if the length of data TMAX/TS2 > 1000
-#define R_SIN               // Comment this line if not r = sin
+//#define R_SIN               // Comment this line if not r = sin
 float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
-float   _r=100./180.*PI;    // [rad], reference signal
+float   _rmax=100./180.*PI; // [rad], max. of reference signal
 float   _Kp=70;             // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
 float   _Ki=10;             // I gain for PID
 float   _Kd=0.01;           // D gain for PID
-#define TS  0.001           // [s], TS>0.001[s], sampling time[s] of PID controller
-#define TS2 0.01            // [s], TS2>0.001[s], sampling time[s] of data save to PC. BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
+#define TS      0.001       // [s], TS>0.001[s], sampling time[s] of PID controller
+#define TS2     0.01        // [s], TS2>0.001[s], sampling time[s] of data save to PC. BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
 #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
@@ -53,6 +53,7 @@
 
 unsigned long _count;   // sampling number
 float   _time;          // time[s]
+float   _r;             // reference signal
 float   _y;             // control output
 float   _e=0;           // e=r-y for PID controller
 float   _eI=0;          // integral of e for PID controller
@@ -108,12 +109,15 @@
     y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
 //    semaphore1.release();   //
 #endif
-#ifdef R_SIN
-  #define RMAX  (100./180.*PI)
+//#ifdef R_SIN
+//  #define RMAX  (100./180.*PI)
   #define RMIN  0
     wt = _freq_u *2.0*PI*_time;
     if(wt>2*PI){    wt -= 2*PI*(float)((int)(wt/(2.0*PI)));}
-    _r = sin(wt ) * (RMAX-RMIN)/2.0 + (RMAX+RMIN)/2.0;
+    _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
+#ifndef R_SIN
+    if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
+    else        _r = 0;
 #endif
     e_old = _e;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
     _e = _r - y;    // error e(t)
@@ -182,9 +186,10 @@
     _count=0;
     _time = 0;  // time
     _e = _eI = 0;
+    encoder.reset();    // set encoder counter zero
     _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
     _r = _r + _y;
-    if( _r>2*PI )    _r -= _r-2*PI;
+//    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
@@ -214,7 +219,7 @@
 
 void thread_print2PC(void const *argument) {
     while (true) {
-        pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_y/(2*PI)*360.0), debug[0]);  // print to tera term
+        pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_y/(2*PI)*360.0), debug[0]/(2*PI)*360.0);  // print to tera term
         Thread::wait(200);
     }
 }
@@ -233,20 +238,22 @@
         pc.scanf("%f",&_freq_u);
         pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
 #else                   // PID control
-  #ifdef R_SIN
-        pc.printf("Reference signal r(t) Frequency[Hz]?...");
-        pc.scanf("%f",&_freq_u);
-        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
-  #endif
-        pc.printf("What number do you like to change?... 0) no change, 1) Kp, 2) Ki, 3)Kd");
+//  #ifdef R_SIN
+//        pc.printf("Reference signal r(t) Frequency[Hz]?...");
+//        pc.scanf("%f",&_freq_u);
+//        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
+//  #endif
+        pc.printf("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)freq.[Hz] of r(t), 5)amp.[deg] of r(t)?");
         f=pc.getc()-48; //int = char-48
         pc.printf("\r\n    Value?... ");
-        if(f>=1&&f<=3){ pc.scanf("%f",&val);}
+        if(f>=1&&f<=5){ pc.scanf("%f",&val);}
         pc.printf("%8.3f\r\n", val);  // print to tera term
         if(f==1){    _Kp = val;}
         if(f==2){    _Ki = val;}
         if(f==3){    _Kd = val;}
-        pc.printf("Kp=%f, Ki=%f, Kd=%f\r\n",_Kp, _Ki, _Kd);
+        if(f==4){    _freq_u = val;}
+        if(f==5){    _rmax = val/180.*PI;}
+        pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
 #endif
     }    
 }