DC motor control program using TA7291P type driver and rotary encoder with A, B phase.
Dependencies: QEI mbed-rtos mbed
Diff: main.cpp
- Revision:
- 7:613febb8f028
- Parent:
- 6:16bee943a9fa
- Child:
- 8:b8b31e9b60c2
- Child:
- 9:0540582a220e
--- 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 }