manabu kosaka
Skelton of EMG input method program using timer interrupt and thread.
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Diff: main.cpp
- Revision:
- 9:b8b31e9b60c2
- Parent:
- 7:613febb8f028
--- a/main.cpp Tue Nov 20 09:54:55 2012 +0000
+++ b/main.cpp Fri Nov 23 05:53:26 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. 121118a by Kosaka lab.
+// ver. 121123a by Kosaka lab.
#include "mbed.h"
#include "rtos.h"
#include "QEI.h"
@@ -7,25 +7,37 @@
/*********** 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 4 // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
+ #define PWM_FREQ 10000.0 //[Hz], pwm freq. available if USE_PWM is defined.
+#define USE_CURRENT_CONTROL // Current control on. Comment if current control off.
+#define CONTROL_MODE 0 // 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
float _rmax=100./180.*PI; // [rad], max. of reference signal
-float _Kp=20; // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
-float _Ki=20; // I gain for PID
-float _Kd=5; // 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.
+float _Kp4th=20; // P gain for PID from motor volt. to angle.
+float _Ki4th=20; // I gain for PID from motor volt. to angle.
+float _Kd4th=5; // D gain for PID from motor volt. to angle.
+float _Kp4i=10.0; // P gain for PID from motor volt. to motor current.
+float _Ki4i=10.0; // I gain for PID from motor volt. to motor current.
+float _Kd4i=0.0; // D gain for PID from motor volt. to motor current.
+#define iTS 0.001 // [s], iTS, sampling time[s] of motor current i control PID using timer interrupt
+#define thTS 0.01 // [s], thTS>=0.001[s], sampling time[s] of motor angle th PID using rtos-timer
+#define TS2 0.01 // [s], TS2>=0.001[s], sampling time[s] to save data to PC using thread. 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
+#define IMAX 0.5 // [A], max of motor current i
+#define IMIN -0.5 // [A], max of motor current i
#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
+AnalogIn v_shunt_r(p19); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
+#define R_SHUNT 1.25 // [Ohm], shunt resistanse
+//AnalogIn VCC(p19); // *3.3 [V], Volt of VCC for motor
+//AnalogIn VCC2(p20); // *3.3 [V], Volt of (VCC-R i), R=2.5[Ohm]. R is for preventing too much i when deadtime is failed.
#define N_ENC (24*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);
@@ -51,10 +63,9 @@
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.
+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
@@ -66,18 +77,23 @@
unsigned long _count; // sampling number
float _time; // time[s]
float _r; // reference signal
-float _y; // control output
+float _th=0; // [rad], motor angle, control output of angle controller
+float _i=0; // [A], motor current, control output of current controller
float _e=0; // e=r-y for PID controller
float _eI=0; // integral of e for PID controller
-float _u; // control input[V]
+float _iref; // reference current iref [A], output of angle th_contorller
+float _u; // control input[V], motor input volt.
+float _ei=0; // e=r-y for current PID controller
+float _eiI=0; // integral of e for current PID controller
unsigned char _f_u_plus=1;// sign(u)
unsigned char _f_umax=0;// flag showing u is max or not
+unsigned char _f_imax=0;// flag showing i 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); //
+unsigned int count2=(int)(TS2/iTS); //
#endif
extern "C" void mbed_reset();
@@ -88,7 +104,7 @@
if( u > 0 ){ // forward: rotate to plus
duty = u/3.3; // Vref
- if(_f_u_plus==0){ // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+ if(_f_u_plus==0){ // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
f_deadtime = 1; // deadtime is required
_f_u_plus=1;
}else{
@@ -97,7 +113,7 @@
f_in=1; r_in=0; // set forward direction
}else if( u < 0 ){ // reverse: rotate to minus
duty = -u/3.3;
- if(_f_u_plus==1){ // if plus to/from minus, set FIN=RIN=0/1 for 100[us].
+ if(_f_u_plus==1){ // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
f_deadtime = 1; // deadtime is required
_f_u_plus=0;
}else{
@@ -123,28 +139,19 @@
#endif
}
-void controller(void const *argument) { // if rtos. current controller & velocity controller
-//void controller() { // if ticker. current controller & velocity controller
- void u2Hbridge(float); // input u to TA7291 driver
+void th_controller(void const *argument) { // if rtos. current controller & velocity controller
float e_old, wt;
- float y, u; // to avoid time shift
+ float y, u;
- 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
+// y_old = _th; // y_old=y(t-iTS) is older than y by 1 sampling time iTS[s]. update data
#ifdef SIMULATION
- 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
+ y = _th + iTS/0.1*(0.2*_iref*100-_th); //=(1-iTS/0.1)*_y + 0.2*iTS/0.1*_iref; // G = 0.2/(0.1s+1)
#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
+#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;
@@ -152,29 +159,15 @@
if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
else _r = 0;
#endif
- e_old = _e; // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
+ e_old = _e; // e_old=e(t-iTS) is older than e by 1 sampling time iTS[s]. update data
_e = _r - y; // error e(t)
- if( _f_umax==0 ){
- _eI = _eI + TS*_e; // integral of e(t)
+ if( _f_imax==0 ){ // u is saturated?
+ if( _e>((360.0/N_ENC)/180*PI) || _e<-((360.0/N_ENC)/180*PI) ){ // e is inside minimum precision?
+ _eI = _eI + thTS*_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 = _Kp4th*_e + _Kd4th*(_e-e_old)/iTS + _Ki4th*_eI; // PID output u(t)
- // 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||CONTROL_MODE==2 // 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));
@@ -185,34 +178,87 @@
else u = UMIN;
#endif
#if CONTROL_MODE==3 // u=rand() to identify motor transfer function G(s) from V to angle
- if(count2==(int)(TS2/TS)){
+ if(count2==(int)(TS2/iTS)){
u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
}else{
- u = _u;
+ u = _iref;
}
#endif
#if CONTROL_MODE==4 // FFT identification, u=repetive signal
- if(count2==(int)(TS2/TS)){
+ if(count2==(int)(TS2/iTS)){
u = data[count3][4];
}else{
- u = _u;
+ u = _iref;
}
#endif
-//debug[0]=u;//minus
- u2Hbridge(u); // input u to TA7291 driver
+ // u is saturated? for anti-windup
+ if( u>IMAX ){
+ _eI -= (u-IMAX)/_Ki4th; if(_eI<0){ _eI=0;}
+ u = IMAX;
+// _f_imax = 1;
+ } else if( u<IMIN ){
+ _eI -= (u-IMIN)/_Ki4th; if(_eI>0){ _eI=0;}
+ u = IMIN;
+// _f_imax = 1;
+ }else{
+ _f_imax = 0;
+ }
+ //-------- update data
+ _th = y;
+ _iref = u;
+}
+void i_controller() { // if ticker. current controller & velocity controller
+ void u2Hbridge(float); // input u to H bridge (full bridge) driver
+#ifdef USE_CURRENT_CONTROL
+ float e_old;
+ float y, u;
+
+// _iref=_r*180/PI; // step response from v to i, useful to tune PID gains.
+ 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;
+ // current PID controller
+ y = v_shunt_r/R_SHUNT; // get i [A] from shunt resistance
+ if(_f_u_plus==0){ y=-y;}
+
+ e_old = _ei; // e_old=e(t-iTS) is older than e by 1 sampling time iTS[s]. update data
+ _ei = _iref - y; // error e(t)
+ if( _f_umax==0 ){
+ _eiI = _eiI + iTS*_ei; // integral of e(t)
+ }
+ u = _Kp4i*_e + _Kd4i*(_ei-e_old)/iTS + _Ki4i*_eiI; // PID output u(t)
+
+ // u is saturated? for anti-windup
+ if( u>UMAX ){
+ _eiI -= (u-UMAX)/_Ki4i; if(_eiI<0){ _eiI=0;}
+ u = UMAX;
+// _f_umax = 1;
+ } else if( u<UMIN ){
+ _eiI -= (u-UMIN)/_Ki4i; if(_eiI>0){ _eiI=0;}
+ u = UMIN;
+// _f_umax = 1;
+ }else{
+ _f_umax = 0;
+ }
+ //-------- update data
+ _i = y;
+ _u = u;
+#else
+ _u = _iref; // without current control.
+#endif
+
+ u2Hbridge(_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;
+ _time += iTS; // time
+debug[0]=v_shunt_r; if(_f_u_plus==0){ debug[0]=-debug[0];}
#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;}
+ if(count2==(int)(TS2/iTS)){
+// j=0; if(_count>=j&&_count<j+1000){i=_count-j; data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_th; data[i][3]=_time; data[i][4]=_u;}
if( count3<1000 ){
- data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_y; data[count3][3]=_time; data[count3][4]=_u;
+ data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_th; data[count3][3]=_time; data[count3][4]=_u;
+// data[count3][0]=_iref; data[count3][1]=debug[0]; data[count3][2]=_i; data[count3][3]=_time; data[count3][4]=_u;
count3++;
}
count2 = 0;
@@ -225,7 +271,7 @@
}
void main1() {
- RtosTimer timer_controller(controller);
+ RtosTimer timer_controller(th_controller);
FILE *fp; // save data to PC
#ifdef GOOD_DATA
int i;
@@ -235,10 +281,10 @@
u2Hbridge(0); // initialize H bridge to stop mode
_count=0;
_time = 0; // time
- _e = _eI = 0;
+ _eI = _eiI = 0; // reset integrater
encoder.reset(); // set encoder counter zero
- _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
- _r = _r + _y;
+ _th = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
+ _r = _r + _th;
// if( _r>2*PI ) _r -= _r-2*PI;
pc.printf("Control start!!\r\n");
@@ -246,10 +292,10 @@
#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]
+ controller_ticker.attach(&i_controller, iTS ); // Sampling period[s] of i_controller
+ timer_controller.start((unsigned int)(iTS*1000.)); // Sampling period[ms] of th controller
-// for ( i = 0; i < (unsigned int)(TMAX/TS2); i++ ) {
+// for ( i = 0; i < (unsigned int)(TMAX/iTS2); 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.
@@ -260,8 +306,10 @@
#endif
Thread::wait((unsigned int)(TS2*1000.)); //[ms]
}
+ controller_ticker.detach(); // timer interrupt stop
timer_controller.stop(); // rtos timer stop
u2Hbridge(0); // initialize H bridge to stop mode
+ _eI = _eiI = 0; // reset integrater
#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
@@ -271,7 +319,7 @@
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]/(2*PI)*360.0); // print to tera term
+ pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), debug[0]*3.3/R_SHUNT); // print to tera term
Thread::wait(200);
}
}
@@ -317,19 +365,24 @@
// pc.scanf("%f",&_freq_u);
// 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]. 9)reset mbed ?");
+ pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
+ pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
+ 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].\r\n 6)iKp, 7)iKi, 8)iKd, 9)reset mbed ?");
f=pc.getc()-48; //int = char-48
pc.printf("\r\n Value?... ");
if(f>=1&&f<=5){ 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;}
+ if(f==1){ _Kp4th = val;}
+ if(f==2){ _Ki4th = val;}
+ if(f==3){ _Kd4th = val;}
if(f==4){ _freq_u = val;}
if(f==5){ _rmax = val/180.*PI;}
+ if(f==6){ _Kp4i = val;}
+ if(f==7){ _Ki4i = val;}
+ if(f==8){ _Kd4i = val;}
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);
+ pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
+ pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
#endif
}
}