manabu kosaka
Skelton of EMG input method program using timer interrupt and thread.
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Revision 0:fe068497f773, committed 2012-11-15
- Comitter:
- kosaka
- Date:
- Thu Nov 15 06:18:51 2012 +0000
- Child:
- 1:b91aeb5673f3
- Commit message:
- 121115;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Nov 15 06:18:51 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /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
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Thu Nov 15 06:18:51 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#9654a71f5a90
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 15 06:18:51 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ed12d17f06 \ No newline at end of file