Diff: main.cpp

Revision:

7:613febb8f028

Parent:

6:16bee943a9fa

Child:

8:0540582a220e

Child:

9:b8b31e9b60c2

--- a/main.cpp	Sat Nov 17 03:14:59 2012 +0000
+++ b/main.cpp	Tue Nov 20 09:54:55 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. 121117a by Kosaka lab.
+//      ver. 121118a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
@@ -7,7 +7,7 @@
 /*********** User setting for control parameters (begin) ***************/
 //#define SIMULATION          // Comment this line if not simulation
 #define USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0. Comment if use Vref=analog mode
-#define CONTROL_MODE    3   // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s)
+#define CONTROL_MODE    4   // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
 #define GOOD_DATA           // Comment this line if the length of data TMAX/TS2 > 1000
 //#define R_SIN               // Comment this line if r=step, not r = sin
 float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
@@ -80,6 +80,7 @@
 unsigned int    count2=(int)(TS2/TS); // 
 #endif
 
+extern "C" void mbed_reset();
 
 void u2Hbridge(float u){// input u to H bridge driver
     float           duty;
@@ -190,6 +191,13 @@
         u = _u;
     }
 #endif
+#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
+    if(count2==(int)(TS2/TS)){
+        u = data[count3][4];
+    }else{
+        u = _u;
+    }
+#endif
 //debug[0]=u;//minus
     u2Hbridge(u);    // input u to TA7291 driver
 
@@ -273,14 +281,36 @@
     char    f;
     float   val;
 #endif
-    
+#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
+    int i, j;
+    float   max_u;
+#endif
+
     while(true){
+#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
+        max_u = 0;
+        for( i=0;i<1000;i++ ){  // u=data[i][4]: memory for FFT identification input signal.
+            data[i][4] = sin(_freq_u*2*PI * i*TS2);   // _u_freq = 10/2 * i [Hz]
+            if( data[i][4]>max_u ){   max_u=data[i][4];}
+        }
+        for( j=1;j<50;j++ ){
+            for( i=0;i<1000;i++ ){
+                data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS2);
+                if( data[i][4]>max_u ){   max_u=data[i][4];}
+            }
+        }
+        for( i=0;i<1000;i++ ){
+//            data[i][4] *= UMAX/max_u;
+            data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
+        }
+#endif
         main1();
 
 #if CONTROL_MODE>=1     // frequency response, or Step response
-        pc.printf("Input u(t) Frequency[Hz]?...");
+        pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
         pc.scanf("%f",&_freq_u);
         pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
+        if(_freq_u==9){    mbed_reset();}
 #else                   // PID control
 //  #ifdef R_SIN
 //        pc.printf("Reference signal r(t) Frequency[Hz]?...");
@@ -288,7 +318,7 @@
 //        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
 //  #endif
         pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
-        pc.printf("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)r(t) freq.[Hz], 5)r(t) amp.[deg] ?");
+        pc.printf("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)r(t) freq.[Hz], 5)r(t) amp.[deg]. 9)reset mbed ?");
         f=pc.getc()-48; //int = char-48
         pc.printf("\r\n    Value?... ");
         if(f>=1&&f<=5){ pc.scanf("%f",&val);}
@@ -298,6 +328,7 @@
         if(f==3){    _Kd = val;}
         if(f==4){    _freq_u = val;}
         if(f==5){    _rmax = val/180.*PI;}
+        if(f==9){    mbed_reset();}
         pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
 #endif
     }