DC motor control program using TA7291P type driver and rotary encoder with A, B phase.
Dependencies: QEI mbed-rtos mbed
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);}