DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Fork of DCmotor2 by
Diff: main.cpp
- Revision:
- 0:fe068497f773
- Child:
- 1:b91aeb5673f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 15 06:18:51 2012 +0000 @@ -0,0 +1,268 @@ +// DC motor control program using TA7291P driver and 360 resolution rotary encoder with A, B phase. +// ver. 121115 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 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 _r=100./180.*PI; // [rad], 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 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 +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. +QEI encoder (p29, p30, NC, N_ENC, QEI::X4_ENCODING); +// QEI(PinName channelA, mbed pin for channel A input. +// PinName channelB, mbed pin for channel B input. +// PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed). +// int pulsesPerRev, Number of pulses in one revolution(=360 deg). +// Encoding encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as +// X4 uses them on both channels. +// ) +// void reset (void) +// Reset the encoder. +// int getCurrentState (void) +// Read the state of the encoder. +// int getPulses (void) +// Read the number of pulses recorded by the encoder. +// int getRevolutions (void) +// Read the number of revolutions recorded by the encoder on the index channel. +/*********** User setting for control parameters (end) ***************/ + + +Serial pc(USBTX, USBRX); // Display on tera term in PC +LocalFileSystem local("local"); // save data to mbed USB disk drive in PC +//Semaphore semaphore1(1); // wait and release to protect memories and so on +//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. + +unsigned long _count; // sampling number +float _time; // time[s] +float _y; // control output +float _e=0; // e=r-y for PID controller +float _eI=0; // integral of e for PID controller +float _u; // control input[V] +unsigned char _f_u_plus=1;// sign(u) +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; + + 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; + }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; + }else{// if( u == 0 ){ // stop mode + abs_u = 0; + IN1 = 0; + IN2 = 0; + } + analog_out = abs_u/3.3; // PID write DA, range is 0-1. Output voltage 0-3.3v +} + +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 + float e_old, wt; + float y, u; // to avoid time shift + + debug_p17 = 1; // for debug: processing time check +// if(debug_p17 == 1) debug_p17=0;else debug_p17=1; // for debug: sampling time check + + _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) +//debug[0]=_u;//plus +#else +// semaphore1.wait(); // + 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) + #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; +#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) + if( _f_umax==0 ){ + _eI = _eI + TS*_e; // integral of e(t) + } + + u = _Kp*_e + _Kd*(_e-e_old)/TS + _Ki*_eI; // PID output u(t) +//debug[0]=_e;//minus +//debug[0]=u;//minus + + // u is saturated? for anti-windup + if( u>UMAX ){ + _eI -= (u-UMAX)/_Ki; if(_eI<0){ _eI=0;} + u = UMAX; +// _f_umax = 1; + } else if( u<UMIN ){ + _eI -= (u-UMIN)/_Ki; if(_eI>0){ _eI=0;} + u = UMIN; +// _f_umax = 1; + }else{ + _f_umax = 0; + } +//#define CONTROL_MODE 2 // 0:PID control, 1:Frequency response, 2:Step response +#if CONTROL_MODE>=1 // frequency response, or Step response + wt = _freq_u *2.0*PI*_time; + if(wt>2*PI) wt -= 2*PI*(float)((int)(wt/2.0*PI)); + u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0; +#endif +#if CONTROL_MODE==2 // Step response + if( u>=0 ) u = UMAX; + else u = UMIN; +#endif +//debug[0]=u;//minus + u2TA7291P(u); // input u to TA7291 driver + + //-------- update data + _time += TS; // time + _y = y; + _u = u; +//debug[0]=_u;//minus +//debug[0]=_eI; +debug[0]=_r; +#ifdef GOOD_DATA + if(count2==(int)(TS2/TS)){ +// j=0; if(_count>=j&&_count<j+1000){i=_count-j; data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_y; data[i][3]=_time; data[i][4]=_u;} + data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_y; data[count3][3]=_time; data[count3][4]=_u; + count3++; + count2 = 0; + } + count2++; +#endif + //-------- update data + + debug_p17 = 0; // for debug: processing time check +} + +void main1() { + RtosTimer timer_controller(controller); + FILE *fp; // save data to PC +#ifdef GOOD_DATA + int i; + + count3=0; +#endif + _count=0; + _time = 0; // time + _e = _eI = 0; + _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; + + pc.printf("Control start!!\r\n"); + if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC + +// controller_ticker.attach(&controller, TS ); // period [s] + timer_controller.start((unsigned int)(TS*1000.)); // Sampling period[ms] + +// for ( i = 0; i < (unsigned int)(TMAX/TS2); i++ ) { + while ( _time <= TMAX ) { + // BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed. + // fprintf returns before process completed. +//BUG fprintf( fp, "%8.2f, %8.4f,\t%8.1f,\t%8.2f\r\n", disp[3], disp[1], disp[0], tmp); // save data to PC (para, y, time, u) +//OK? fprintf( fp, "%f, %f, %f, %f, %f\r\n", _time, debug[0], debug[3], (_y/(2*PI)*360.0),_u); // save data to PC (para, y, time, u) +#ifndef GOOD_DATA + fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _y, _time, _u); // save data to PC (para, y, time, u) +#endif + Thread::wait((unsigned int)(TS2*1000.)); //[ms] + } + timer_controller.stop(); // rtos timer stop + analog_out = 0; // stop motor +#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 + fclose( fp ); // release mbed USB drive + pc.printf("Control completed!!\r\n\r\n"); +} + +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 + Thread::wait(200); + } +} + +void main2(void const *argument) { +#if CONTROL_MODE==0 // PID control + char f; + float val; +#endif + + while(true){ + main1(); + +#if CONTROL_MODE>=1 // frequency response, or Step response + pc.printf("Input u(t) Frequency[Hz]?..."); + 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"); + f=pc.getc()-48; //int = char-48 + pc.printf("\r\n Value?... "); + if(f>=1&&f<=3){ 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); +#endif + } +} +int main() { +// void main1(); + Thread save2PC(main2,NULL,osPriorityBelowNormal); + Thread print2PC(thread_print2PC,NULL,osPriorityLow); + +// osStatus set_priority(osPriority osPriorityBelowNormal ); +// Priority of Thread (RtosTimer has no priority?) +// osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!! +// osPriorityLow = -2, ///< priority: low +// osPriorityBelowNormal = -1, ///< priority: below normal +// osPriorityNormal = 0, ///< priority: normal (default) +// osPriorityAboveNormal = +1, ///< priority: above normal +// osPriorityHigh = +2, ///< priority: high +// osPriorityRealtime = +3, ///< priority: realtime (highest) +// osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority +}