Diff: main.cpp

Revision:

3:b6b9b8c7dce6

Parent:

2:e056793d6fc5

Child:

4:6ccbf4d3cb6d

--- a/main.cpp	Thu Nov 15 08:25:43 2012 +0000
+++ b/main.cpp	Fri Nov 16 01:42:30 2012 +0000
@@ -1,28 +1,30 @@
 //  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-//      ver. 121115a by Kosaka lab.
+//      ver. 121116a by Kosaka lab.
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
 #define PI 3.14159265358979 // def. of PI
 /*********** 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    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
 float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
 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.
+float   _Kp=1;              // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
+float   _Ki=0;              // I gain for PID
+float   _Kd=0;              // 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. But, max data length is 1000.
 #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
-
-AnalogOut   analog_out(p18);// Vref for DC motor driver TA7291P. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]
-DigitalOut  IN1(p19);       // IN1  for DC motor driver TA7291P
-DigitalOut  IN2(p20);       // IN2  for DC motor driver TA7291P
+#define DEADTIME    0.0001  // [s], deadtime to be set between plus volt. to/from minus
+                        // H bridge port setting
+#define FIN_PORT    p21     // FIN (IN1) port of mbed
+#define RIN_PORT    p22     // RIN (IN2) port of mbed
+#define VREF_PORT   p18     // Vref      port of mbed (available if USE_PWM is not defined)
 DigitalOut  debug_p17(p17); // p17 for debug
 
 #define N_ENC   (360*4)     // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
@@ -51,6 +53,15 @@
 //Mutex stdio_mutex;            // wait and release to protect memories and so on
 //Ticker controller_ticker;     // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
 
+#ifdef  USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0.
+  #define PWM_FREQ 10000.0  //[Hz], pwm freq.
+  PwmOut FIN(FIN_PORT);     // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
+  PwmOut RIN(RIN_PORT);     // PWM for RIN, FIN=0 when reverse rotation. H bridge driver PWM mode
+#else                       // H bridge Vref=analog mode
+  AnalogOut   analog_out(VREF_PORT);// Vref for DC motor H bridge driver. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]
+  DigitalOut  FIN(FIN_PORT);// FIN  for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation
+  DigitalOut  RIN(RIN_PORT);// RIN  for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation
+#endif
 unsigned long _count;   // sampling number
 float   _time;          // time[s]
 float   _r;             // reference signal
@@ -62,37 +73,56 @@
 unsigned char _f_umax=0;// flag showing u is max or not
 float   debug[10];      // for debug
 float   disp[10];       // for printf to avoid interrupted by quicker process
-
 #ifdef  GOOD_DATA
 float data[1000][5];    // memory to save data offline instead of "online fprintf".
 unsigned int    count3; // 
 unsigned int    count2=(int)(TS2/TS); // 
 #endif
 
-void u2TA7291P(float u){// input u to TA7291 driver
-    float   abs_u;
+
+void u2Hbridge(float u){// input u to H bridge driver
+    float           duty;
+    unsigned int    f_deadtime, f_in, r_in;
 
     if( u > 0 ){        // forward: rotate to plus
-        abs_u = u;          // Vref
-        if(_f_u_plus==0){   _f_u_plus=1;  IN1=0;  IN2=0;  analog_out=0;  wait(0.0001);}  // if plus to/from minus, set IN1=IN2=0/1 for 100[us].
-        IN1 = 1;
-        IN2 = 0;
+        duty = u/3.3;       // Vref
+        if(_f_u_plus==0){   // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+            f_deadtime = 1; // deadtime is required
+            _f_u_plus=1;
+        }else{
+            f_deadtime = 0; // deadtime is required
+        }
+        f_in=1;  r_in=0;    // set forward direction
     }else if( u < 0 ){  // reverse: rotate to minus
-        abs_u = -u;
-        if(_f_u_plus==1){   _f_u_plus=0;  IN1=0;  IN2=0;  analog_out=0;  wait(0.0001);}  // if plus to/from minus, set IN1=IN2=0/1 for 100[us].
-        IN1 = 0;
-        IN2 = 1;
+        duty = -u/3.3;
+        if(_f_u_plus==1){   // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+            f_deadtime = 1; // deadtime is required
+            _f_u_plus=0;
+        }else{
+            f_deadtime = 0; // deadtime is required
+        }
+        f_in=0;  r_in=1;    // set reverse direction
     }else{// if( u == 0 ){  // stop mode
-        abs_u = 0;
-        IN1 = 0;
-        IN2 = 0;
+        duty = 0;
+        f_deadtime = 0; // deadtime is required
+        f_in=0;  r_in=0;    // set FIN & RIN
     }
-    analog_out = abs_u/3.3; // PID write DA, range is 0-1. Output voltage 0-3.3v
+
+    if( f_deadtime==1 ){// making deadtime
+        FIN=0;  RIN=0;      // set upper&lower arm zero
+        wait(DEADTIME);
+    }
+#ifdef USE_PWM  // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0
+    FIN = duty*(float)f_in;    RIN = duty*(float)r_in;  // setting pwm FIN & RIN
+#else           //       Analog mode: Vref=analog, FIN, RIN = 1 or 0)
+    FIN = f_in;    RIN = r_in;  // setting FIN & RIN
+    analog_out = duty;          // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v
+#endif
 }
 
 void controller(void const *argument) {    // if rtos. current controller & velocity controller
 //void controller() {    // if ticker. current controller & velocity controller
-    void    u2TA7291P(float);    // input u to TA7291 driver
+    void    u2Hbridge(float);    // input u to TA7291 driver
     float   e_old, wt;
     float   y, u;     // to avoid time shift
 
@@ -102,7 +132,7 @@
     _count+=1;
 //    y_old = _y;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
 #ifdef SIMULATION
-    y = _y + TS/0.1*(0.02*_u*100-_y);   //=(1-TS/0.1)*_y + 0.02*TS/0.1*_u; // G = 0.02/(0.1s+1)
+    y = _y + TS/0.1*(0.2*_u*100-_y);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_u; // G = 0.2/(0.1s+1)
 //debug[0]=_u;//plus
 #else
 //    semaphore1.wait();      //
@@ -152,7 +182,7 @@
     else        u = UMIN;
 #endif
 //debug[0]=u;//minus
-    u2TA7291P(u);    // input u to TA7291 driver
+    u2Hbridge(u);    // input u to TA7291 driver
 
     //-------- update data
     _time += TS;    // time
@@ -185,6 +215,7 @@
 
     count3=0;
 #endif
+    u2Hbridge(0);           // initialize H bridge to stop mode
     _count=0;
     _time = 0;  // time
     _e = _eI = 0;
@@ -195,7 +226,9 @@
 
     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
 //    controller_ticker.attach(&controller, TS ); // period [s]
     timer_controller.start((unsigned int)(TS*1000.));   // Sampling period[ms]
 
@@ -211,7 +244,7 @@
         Thread::wait((unsigned int)(TS2*1000.));  //[ms]
     }
     timer_controller.stop();    // rtos timer stop
-    analog_out = 0;              // stop motor
+    u2Hbridge(0);           // initialize H bridge to stop mode
 #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
@@ -245,7 +278,7 @@
 //        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)?");
+        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] ?");
         f=pc.getc()-48; //int = char-48
         pc.printf("\r\n    Value?... ");
         if(f>=1&&f<=5){ pc.scanf("%f",&val);}