manabu kosaka / Mbed 2 deprecated EMGbasic

Skelton of EMG input method program using timer interrupt and thread.

Dependencies:   QEI mbed-rtos mbed

Fork of DCmotor by manabu kosaka

main.cpp@1:b91aeb5673f3, 2012-11-15 (annotated)

Committer:
kosaka
Date:
Thu Nov 15 07:51:42 2012 +0000
Revision:
1:b91aeb5673f3
Parent:
0:fe068497f773
Child:
2:e056793d6fc5
max. of r(t) can now be changed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 1:b91aeb5673f3 1 // DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
kosaka 1:b91aeb5673f3 2 // ver. 121115a by Kosaka lab.
kosaka 0:fe068497f773 3 #include "mbed.h"
kosaka 0:fe068497f773 4 #include "rtos.h"
kosaka 0:fe068497f773 5 #include "QEI.h"
kosaka 0:fe068497f773 6 #define PI 3.14159265358979 // def. of PI
kosaka 0:fe068497f773 7 /*********** User setting for control parameters (begin) ***************/
kosaka 0:fe068497f773 8 #define SIMULATION // Comment this line if not simulation
kosaka 0:fe068497f773 9 #define CONTROL_MODE 0 // 0:PID control, 1:Frequency response, 2:Step response
kosaka 0:fe068497f773 10 #define GOOD_DATA // Comment this line if the length of data TMAX/TS2 > 1000
kosaka 1:b91aeb5673f3 11 //#define R_SIN // Comment this line if not r = sin
kosaka 0:fe068497f773 12 float _freq_u = 0.3; // [Hz], freq. of Frequency response, or Step response
kosaka 1:b91aeb5673f3 13 float _rmax=100./180.*PI; // [rad], max. of reference signal
kosaka 0:fe068497f773 14 float _Kp=70; // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
kosaka 0:fe068497f773 15 float _Ki=10; // I gain for PID
kosaka 0:fe068497f773 16 float _Kd=0.01; // D gain for PID
kosaka 1:b91aeb5673f3 17 #define TS 0.001 // [s], TS>0.001[s], sampling time[s] of PID controller
kosaka 1:b91aeb5673f3 18 #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.
kosaka 0:fe068497f773 19 #define TMAX 10 // [s], experiment starts from 0[s] to TMAX[s]
kosaka 0:fe068497f773 20 #define UMAX 3.3 // [V], max of control input u
kosaka 0:fe068497f773 21 #define UMIN -3.3 // [V], max of control input u
kosaka 0:fe068497f773 22
kosaka 0:fe068497f773 23 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]
kosaka 0:fe068497f773 24 DigitalOut IN1(p19); // IN1 for DC motor driver TA7291P
kosaka 0:fe068497f773 25 DigitalOut IN2(p20); // IN2 for DC motor driver TA7291P
kosaka 0:fe068497f773 26 DigitalOut debug_p17(p17); // p17 for debug
kosaka 0:fe068497f773 27
kosaka 0:fe068497f773 28 #define N_ENC (360*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
kosaka 0:fe068497f773 29 QEI encoder (p29, p30, NC, N_ENC, QEI::X4_ENCODING);
kosaka 0:fe068497f773 30 // QEI(PinName channelA, mbed pin for channel A input.
kosaka 0:fe068497f773 31 // PinName channelB, mbed pin for channel B input.
kosaka 0:fe068497f773 32 // PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
kosaka 0:fe068497f773 33 // int pulsesPerRev, Number of pulses in one revolution(=360 deg).
kosaka 0:fe068497f773 34 // Encoding encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as
kosaka 0:fe068497f773 35 // X4 uses them on both channels.
kosaka 0:fe068497f773 36 // )
kosaka 0:fe068497f773 37 // void reset (void)
kosaka 0:fe068497f773 38 // Reset the encoder.
kosaka 0:fe068497f773 39 // int getCurrentState (void)
kosaka 0:fe068497f773 40 // Read the state of the encoder.
kosaka 0:fe068497f773 41 // int getPulses (void)
kosaka 0:fe068497f773 42 // Read the number of pulses recorded by the encoder.
kosaka 0:fe068497f773 43 // int getRevolutions (void)
kosaka 0:fe068497f773 44 // Read the number of revolutions recorded by the encoder on the index channel.
kosaka 0:fe068497f773 45 /*********** User setting for control parameters (end) ***************/
kosaka 0:fe068497f773 46
kosaka 0:fe068497f773 47
kosaka 0:fe068497f773 48 Serial pc(USBTX, USBRX); // Display on tera term in PC
kosaka 0:fe068497f773 49 LocalFileSystem local("local"); // save data to mbed USB disk drive in PC
kosaka 0:fe068497f773 50 //Semaphore semaphore1(1); // wait and release to protect memories and so on
kosaka 0:fe068497f773 51 //Mutex stdio_mutex; // wait and release to protect memories and so on
kosaka 0:fe068497f773 52 //Ticker controller_ticker; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
kosaka 0:fe068497f773 53
kosaka 0:fe068497f773 54 unsigned long _count; // sampling number
kosaka 0:fe068497f773 55 float _time; // time[s]
kosaka 1:b91aeb5673f3 56 float _r; // reference signal
kosaka 0:fe068497f773 57 float _y; // control output
kosaka 0:fe068497f773 58 float _e=0; // e=r-y for PID controller
kosaka 0:fe068497f773 59 float _eI=0; // integral of e for PID controller
kosaka 0:fe068497f773 60 float _u; // control input[V]
kosaka 0:fe068497f773 61 unsigned char _f_u_plus=1;// sign(u)
kosaka 0:fe068497f773 62 unsigned char _f_umax=0;// flag showing u is max or not
kosaka 0:fe068497f773 63 float debug[10]; // for debug
kosaka 0:fe068497f773 64 float disp[10]; // for printf to avoid interrupted by quicker process
kosaka 0:fe068497f773 65
kosaka 0:fe068497f773 66 #ifdef GOOD_DATA
kosaka 0:fe068497f773 67 float data[1000][5]; // memory to save data offline instead of "online fprintf".
kosaka 0:fe068497f773 68 unsigned int count3; //
kosaka 0:fe068497f773 69 unsigned int count2=(int)(TS2/TS); //
kosaka 0:fe068497f773 70 #endif
kosaka 0:fe068497f773 71
kosaka 0:fe068497f773 72 void u2TA7291P(float u){// input u to TA7291 driver
kosaka 0:fe068497f773 73 float abs_u;
kosaka 0:fe068497f773 74
kosaka 0:fe068497f773 75 if( u > 0 ){ // forward: rotate to plus
kosaka 0:fe068497f773 76 abs_u = u; // Vref
kosaka 0:fe068497f773 77 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].
kosaka 0:fe068497f773 78 IN1 = 1;
kosaka 0:fe068497f773 79 IN2 = 0;
kosaka 0:fe068497f773 80 }else if( u < 0 ){ // reverse: rotate to minus
kosaka 0:fe068497f773 81 abs_u = -u;
kosaka 0:fe068497f773 82 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].
kosaka 0:fe068497f773 83 IN1 = 0;
kosaka 0:fe068497f773 84 IN2 = 1;
kosaka 0:fe068497f773 85 }else{// if( u == 0 ){ // stop mode
kosaka 0:fe068497f773 86 abs_u = 0;
kosaka 0:fe068497f773 87 IN1 = 0;
kosaka 0:fe068497f773 88 IN2 = 0;
kosaka 0:fe068497f773 89 }
kosaka 0:fe068497f773 90 analog_out = abs_u/3.3; // PID write DA, range is 0-1. Output voltage 0-3.3v
kosaka 0:fe068497f773 91 }
kosaka 0:fe068497f773 92
kosaka 0:fe068497f773 93 void controller(void const *argument) { // if rtos. current controller & velocity controller
kosaka 0:fe068497f773 94 //void controller() { // if ticker. current controller & velocity controller
kosaka 0:fe068497f773 95 void u2TA7291P(float); // input u to TA7291 driver
kosaka 0:fe068497f773 96 float e_old, wt;
kosaka 0:fe068497f773 97 float y, u; // to avoid time shift
kosaka 0:fe068497f773 98
kosaka 0:fe068497f773 99 debug_p17 = 1; // for debug: processing time check
kosaka 0:fe068497f773 100 // if(debug_p17 == 1) debug_p17=0;else debug_p17=1; // for debug: sampling time check
kosaka 0:fe068497f773 101
kosaka 0:fe068497f773 102 _count+=1;
kosaka 0:fe068497f773 103 // y_old = _y; // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
kosaka 0:fe068497f773 104 #ifdef SIMULATION
kosaka 0:fe068497f773 105 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)
kosaka 0:fe068497f773 106 //debug[0]=_u;//plus
kosaka 0:fe068497f773 107 #else
kosaka 0:fe068497f773 108 // semaphore1.wait(); //
kosaka 0:fe068497f773 109 y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
kosaka 0:fe068497f773 110 // semaphore1.release(); //
kosaka 0:fe068497f773 111 #endif
kosaka 1:b91aeb5673f3 112 //#ifdef R_SIN
kosaka 1:b91aeb5673f3 113 // #define RMAX (100./180.*PI)
kosaka 0:fe068497f773 114 #define RMIN 0
kosaka 0:fe068497f773 115 wt = _freq_u *2.0*PI*_time;
kosaka 0:fe068497f773 116 if(wt>2*PI){ wt -= 2*PI*(float)((int)(wt/(2.0*PI)));}
kosaka 1:b91aeb5673f3 117 _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
kosaka 1:b91aeb5673f3 118 #ifndef R_SIN
kosaka 1:b91aeb5673f3 119 if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
kosaka 1:b91aeb5673f3 120 else _r = 0;
kosaka 0:fe068497f773 121 #endif
kosaka 0:fe068497f773 122 e_old = _e; // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
kosaka 0:fe068497f773 123 _e = _r - y; // error e(t)
kosaka 0:fe068497f773 124 if( _f_umax==0 ){
kosaka 0:fe068497f773 125 _eI = _eI + TS*_e; // integral of e(t)
kosaka 0:fe068497f773 126 }
kosaka 0:fe068497f773 127
kosaka 0:fe068497f773 128 u = _Kp*_e + _Kd*(_e-e_old)/TS + _Ki*_eI; // PID output u(t)
kosaka 0:fe068497f773 129 //debug[0]=_e;//minus
kosaka 0:fe068497f773 130 //debug[0]=u;//minus
kosaka 0:fe068497f773 131
kosaka 0:fe068497f773 132 // u is saturated? for anti-windup
kosaka 0:fe068497f773 133 if( u>UMAX ){
kosaka 0:fe068497f773 134 _eI -= (u-UMAX)/_Ki; if(_eI<0){ _eI=0;}
kosaka 0:fe068497f773 135 u = UMAX;
kosaka 0:fe068497f773 136 // _f_umax = 1;
kosaka 0:fe068497f773 137 } else if( u<UMIN ){
kosaka 0:fe068497f773 138 _eI -= (u-UMIN)/_Ki; if(_eI>0){ _eI=0;}
kosaka 0:fe068497f773 139 u = UMIN;
kosaka 0:fe068497f773 140 // _f_umax = 1;
kosaka 0:fe068497f773 141 }else{
kosaka 0:fe068497f773 142 _f_umax = 0;
kosaka 0:fe068497f773 143 }
kosaka 0:fe068497f773 144 //#define CONTROL_MODE 2 // 0:PID control, 1:Frequency response, 2:Step response
kosaka 0:fe068497f773 145 #if CONTROL_MODE>=1 // frequency response, or Step response
kosaka 0:fe068497f773 146 wt = _freq_u *2.0*PI*_time;
kosaka 0:fe068497f773 147 if(wt>2*PI) wt -= 2*PI*(float)((int)(wt/2.0*PI));
kosaka 0:fe068497f773 148 u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0;
kosaka 0:fe068497f773 149 #endif
kosaka 0:fe068497f773 150 #if CONTROL_MODE==2 // Step response
kosaka 0:fe068497f773 151 if( u>=0 ) u = UMAX;
kosaka 0:fe068497f773 152 else u = UMIN;
kosaka 0:fe068497f773 153 #endif
kosaka 0:fe068497f773 154 //debug[0]=u;//minus
kosaka 0:fe068497f773 155 u2TA7291P(u); // input u to TA7291 driver
kosaka 0:fe068497f773 156
kosaka 0:fe068497f773 157 //-------- update data
kosaka 0:fe068497f773 158 _time += TS; // time
kosaka 0:fe068497f773 159 _y = y;
kosaka 0:fe068497f773 160 _u = u;
kosaka 0:fe068497f773 161 //debug[0]=_u;//minus
kosaka 0:fe068497f773 162 //debug[0]=_eI;
kosaka 0:fe068497f773 163 debug[0]=_r;
kosaka 0:fe068497f773 164 #ifdef GOOD_DATA
kosaka 0:fe068497f773 165 if(count2==(int)(TS2/TS)){
kosaka 0:fe068497f773 166 // 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;}
kosaka 0:fe068497f773 167 data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_y; data[count3][3]=_time; data[count3][4]=_u;
kosaka 0:fe068497f773 168 count3++;
kosaka 0:fe068497f773 169 count2 = 0;
kosaka 0:fe068497f773 170 }
kosaka 0:fe068497f773 171 count2++;
kosaka 0:fe068497f773 172 #endif
kosaka 0:fe068497f773 173 //-------- update data
kosaka 0:fe068497f773 174
kosaka 0:fe068497f773 175 debug_p17 = 0; // for debug: processing time check
kosaka 0:fe068497f773 176 }
kosaka 0:fe068497f773 177
kosaka 0:fe068497f773 178 void main1() {
kosaka 0:fe068497f773 179 RtosTimer timer_controller(controller);
kosaka 0:fe068497f773 180 FILE *fp; // save data to PC
kosaka 0:fe068497f773 181 #ifdef GOOD_DATA
kosaka 0:fe068497f773 182 int i;
kosaka 0:fe068497f773 183
kosaka 0:fe068497f773 184 count3=0;
kosaka 0:fe068497f773 185 #endif
kosaka 0:fe068497f773 186 _count=0;
kosaka 0:fe068497f773 187 _time = 0; // time
kosaka 0:fe068497f773 188 _e = _eI = 0;
kosaka 1:b91aeb5673f3 189 encoder.reset(); // set encoder counter zero
kosaka 0:fe068497f773 190 _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
kosaka 0:fe068497f773 191 _r = _r + _y;
kosaka 1:b91aeb5673f3 192 // if( _r>2*PI ) _r -= _r-2*PI;
kosaka 0:fe068497f773 193
kosaka 0:fe068497f773 194 pc.printf("Control start!!\r\n");
kosaka 0:fe068497f773 195 if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC
kosaka 0:fe068497f773 196
kosaka 0:fe068497f773 197 // controller_ticker.attach(&controller, TS ); // period [s]
kosaka 0:fe068497f773 198 timer_controller.start((unsigned int)(TS*1000.)); // Sampling period[ms]
kosaka 0:fe068497f773 199
kosaka 0:fe068497f773 200 // for ( i = 0; i < (unsigned int)(TMAX/TS2); i++ ) {
kosaka 0:fe068497f773 201 while ( _time <= TMAX ) {
kosaka 0:fe068497f773 202 // BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
kosaka 0:fe068497f773 203 // fprintf returns before process completed.
kosaka 0:fe068497f773 204 //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)
kosaka 0:fe068497f773 205 //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)
kosaka 0:fe068497f773 206 #ifndef GOOD_DATA
kosaka 0:fe068497f773 207 fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _y, _time, _u); // save data to PC (para, y, time, u)
kosaka 0:fe068497f773 208 #endif
kosaka 0:fe068497f773 209 Thread::wait((unsigned int)(TS2*1000.)); //[ms]
kosaka 0:fe068497f773 210 }
kosaka 0:fe068497f773 211 timer_controller.stop(); // rtos timer stop
kosaka 0:fe068497f773 212 analog_out = 0; // stop motor
kosaka 0:fe068497f773 213 #ifdef GOOD_DATA
kosaka 0:fe068497f773 214 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)
kosaka 0:fe068497f773 215 #endif
kosaka 0:fe068497f773 216 fclose( fp ); // release mbed USB drive
kosaka 0:fe068497f773 217 pc.printf("Control completed!!\r\n\r\n");
kosaka 0:fe068497f773 218 }
kosaka 0:fe068497f773 219
kosaka 0:fe068497f773 220 void thread_print2PC(void const *argument) {
kosaka 0:fe068497f773 221 while (true) {
kosaka 1:b91aeb5673f3 222 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]/(2*PI)*360.0); // print to tera term
kosaka 0:fe068497f773 223 Thread::wait(200);
kosaka 0:fe068497f773 224 }
kosaka 0:fe068497f773 225 }
kosaka 0:fe068497f773 226
kosaka 0:fe068497f773 227 void main2(void const *argument) {
kosaka 0:fe068497f773 228 #if CONTROL_MODE==0 // PID control
kosaka 0:fe068497f773 229 char f;
kosaka 0:fe068497f773 230 float val;
kosaka 0:fe068497f773 231 #endif
kosaka 0:fe068497f773 232
kosaka 0:fe068497f773 233 while(true){
kosaka 0:fe068497f773 234 main1();
kosaka 0:fe068497f773 235
kosaka 0:fe068497f773 236 #if CONTROL_MODE>=1 // frequency response, or Step response
kosaka 0:fe068497f773 237 pc.printf("Input u(t) Frequency[Hz]?...");
kosaka 0:fe068497f773 238 pc.scanf("%f",&_freq_u);
kosaka 0:fe068497f773 239 pc.printf("%8.3f[Hz]\r\n", _freq_u); // print to tera term
kosaka 0:fe068497f773 240 #else // PID control
kosaka 1:b91aeb5673f3 241 // #ifdef R_SIN
kosaka 1:b91aeb5673f3 242 // pc.printf("Reference signal r(t) Frequency[Hz]?...");
kosaka 1:b91aeb5673f3 243 // pc.scanf("%f",&_freq_u);
kosaka 1:b91aeb5673f3 244 // pc.printf("%8.3f[Hz]\r\n", _freq_u); // print to tera term
kosaka 1:b91aeb5673f3 245 // #endif
kosaka 1:b91aeb5673f3 246 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)?");
kosaka 0:fe068497f773 247 f=pc.getc()-48; //int = char-48
kosaka 0:fe068497f773 248 pc.printf("\r\n Value?... ");
kosaka 1:b91aeb5673f3 249 if(f>=1&&f<=5){ pc.scanf("%f",&val);}
kosaka 0:fe068497f773 250 pc.printf("%8.3f\r\n", val); // print to tera term
kosaka 0:fe068497f773 251 if(f==1){ _Kp = val;}
kosaka 0:fe068497f773 252 if(f==2){ _Ki = val;}
kosaka 0:fe068497f773 253 if(f==3){ _Kd = val;}
kosaka 1:b91aeb5673f3 254 if(f==4){ _freq_u = val;}
kosaka 1:b91aeb5673f3 255 if(f==5){ _rmax = val/180.*PI;}
kosaka 1:b91aeb5673f3 256 pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
kosaka 0:fe068497f773 257 #endif
kosaka 0:fe068497f773 258 }
kosaka 0:fe068497f773 259 }
kosaka 0:fe068497f773 260 int main() {
kosaka 0:fe068497f773 261 // void main1();
kosaka 0:fe068497f773 262 Thread save2PC(main2,NULL,osPriorityBelowNormal);
kosaka 0:fe068497f773 263 Thread print2PC(thread_print2PC,NULL,osPriorityLow);
kosaka 0:fe068497f773 264
kosaka 0:fe068497f773 265 // osStatus set_priority(osPriority osPriorityBelowNormal );
kosaka 0:fe068497f773 266 // Priority of Thread (RtosTimer has no priority?)
kosaka 0:fe068497f773 267 // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
kosaka 0:fe068497f773 268 // osPriorityLow = -2, ///< priority: low
kosaka 0:fe068497f773 269 // osPriorityBelowNormal = -1, ///< priority: below normal
kosaka 0:fe068497f773 270 // osPriorityNormal = 0, ///< priority: normal (default)
kosaka 0:fe068497f773 271 // osPriorityAboveNormal = +1, ///< priority: above normal
kosaka 0:fe068497f773 272 // osPriorityHigh = +2, ///< priority: high
kosaka 0:fe068497f773 273 // osPriorityRealtime = +3, ///< priority: realtime (highest)
kosaka 0:fe068497f773 274 // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
kosaka 0:fe068497f773 275 }