manabu kosaka
DC motor control program using TA7291P type driver and rotary encoder with A, B phase.
Dependencies: QEI mbed-rtos mbed
Diff: main.cpp
- Revision:
- 15:744a81d5b7ac
- Parent:
- 14:1196c2d455ae
- Child:
- 16:759d6f647c83
--- a/main.cpp Sun Dec 02 10:03:09 2012 +0000
+++ b/main.cpp Sat Dec 08 05:01:53 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. 121202a by Kosaka lab.
+// ver. 121208a by Kosaka lab.
#include "mbed.h"
#include "rtos.h"
#include "QEI.h"
@@ -8,25 +8,25 @@
//#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 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 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 DEADZONE_PLUS 1. // deadzone of plus side
#define DEADZONE_MINUS -1.5 // deadzone of minus side
#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 _rmax=360./180.*PI; // [rad], max. of reference signal
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 TS0 0.0001 // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
-#define TS1 0.001 // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
-#define TS2 0.01 // [s], sampling time (priority =main(): precision 4ms) to save data to PC using thread. But, max data length is 1000.
-#define TS3 0.05 // [s], sampling time (priority low: precision 4ms)
-#define TS4 0.1 // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
+#define TS0 0.001//08//8 // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
+#define TS1 0.002//2//0.01 // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
+#define TS2 0.05 // [s], sampling time (priority =main(): precision 4ms)
+#define TS3 0.02 // [s], sampling time (priority low: precision 4ms) to save data to PC using thread. But, max data length is 1000.
+#define TS4 0.2 // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
#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
@@ -38,6 +38,8 @@
#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
+DigitalOut debug_p23(p23); // p17 for debug
+DigitalOut debug_p24(p24); // 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
@@ -67,7 +69,7 @@
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 TickerTimerTS0; // 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.
PwmOut FIN(FIN_PORT); // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
@@ -92,12 +94,13 @@
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 debug[20]; // 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/TS0); //
+unsigned int count2=(int)(TS3/TS0); //
+unsigned int _count_data=0; // data2mbedUSB()
#endif
extern "C" void mbed_reset();
@@ -151,7 +154,9 @@
// y_old = _th; // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
#ifdef SIMULATION
- y = _th + TS1/0.1*(0.2*_iref*100-_th); //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 0.2/(0.1s+1)
+ if( (u=_iref)>IMAX ){ u-=IMAX;}else if(u<IMIN){ u+=IMIN;}
+ y = _th + TS1/10*(20*u-_th); //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 20/(10s+1)
+debug[0] =_iref;
#else
// semaphore1.wait(); //
y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
@@ -161,13 +166,13 @@
wt = _freq_u *2.0*PI*_time;
if(wt>2.0*PI){ wt -= 2.0*PI*(float)((int)(wt/(2.0*PI)));}
_r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
-//debug[0] =1;
#ifndef R_SIN
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 = _r - y; // error e(t)
+//debug[0]=_e;
if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){ // e is inside minimum precision?
_e = 0;
}
@@ -184,18 +189,18 @@
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;
+ if( u>=0 ) u = IMAX/2.;
+ else u = IMIN/2.;
#endif
#if CONTROL_MODE==3 // u=rand() to identify motor transfer function G(s) from V to angle
- if(count2==(int)(TS2/TS0)){
+ if(count2==(int)(TS3/TS0)){
u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
}else{
u = _iref;
}
#endif
#if CONTROL_MODE==4 // FFT identification, u=repetive signal
- if(count2==(int)(TS2/TS1)){
+ if(count2==(int)(TS3/TS1)){
u = data[count3][4];
}else{
u = _iref;
@@ -216,6 +221,7 @@
//-------- update data
_th = y;
_iref = u;
+//debug[0] =_iref;
}
void i_controller() { // if ticker. current controller & velocity controller
void u2Hbridge(float); // input u to H bridge (full bridge) driver
@@ -229,7 +235,11 @@
_count+=1;
// current PID controller
+ #ifdef SIMULATION
y = v_shunt_r/R_SHUNT; // get i [A] from shunt resistance
+ #else
+ y = _iref;
+ #endif
if(_f_u_plus==0){ y=-y;}
e_old = _ei; // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
@@ -255,26 +265,14 @@
_i = y;
_u = u;
#else
- _u = _iref/IMAX*VMAX; // without current control.
+ _u = _iref/IMAX*UMAX; // without current control.
#endif
u2Hbridge(_u); // input u to TA7291 driver
//-------- update data
_time += TS0; // time
-debug[0]=v_shunt_r; if(_f_u_plus==0){ debug[0]=-debug[0];}
-#ifdef GOOD_DATA
- if(count2==(int)(TS2/TS0)){
-// 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]=_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;
- }
- count2++;
-#endif
+//debug[0]=v_shunt_r; if(_f_u_plus==0){ debug[0]=-debug[0];}
//-------- update data
debug_p17 = 0; // for debug: processing time check
@@ -291,62 +289,40 @@
FIN.period( 1.0 / PWM_FREQ ); // PWM period [s]. Common to all PWM
#endif
}
-void moror_control() { // motor control ON for TMAX seconds.
- RtosTimer timer_controller(th_controller); // RtosTimer priority is osPriorityAboveNormal, just one above main()
- FILE *fp; // save data to PC
- float t=0;
-#ifdef GOOD_DATA
- int i;
- count3=0;
-#endif
- init_controller(); // initialize controller parameters and signals
- _r = _r + _th;
-// if( _r>2*PI ) _r -= _r-2*PI;
-
- if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC
-
- // start control (ON)
- controller_ticker.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
- timer_controller.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
-
- t = _time;
- 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)
-#ifndef GOOD_DATA
- fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u); // save data to PC (para, y, time, u)
-#endif
- Thread::wait((unsigned int)((TS2-(_time-t))*1000.)); //[ms]
- t = _time;
+void data2mbedUSB(){ // store data to save to mbedUSB after experiment is over
+ if( _count_data<1000 ){
+ data[_count_data][0]=_r; data[_count_data][1]=debug[0];
+ data[_count_data][2]=_th; data[_count_data][3]=_time; data[_count_data][4]=_u;
+ _count_data++;
}
- // stop control (OFF)
- controller_ticker.detach(); // timer interrupt stop
- timer_controller.stop(); // rtos timer stop
-
- init_controller(); // initialize controller parameters and signals
-#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
+//BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
}
-
void display2PC(){ // display to tera term on PC
+// pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5)); // 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), _r);//debug[0]*3.3/R_SHUNT); // print to tera term
}
-void TS3timer(void const *argument) { // make sampling time TS4 timer (priority low: precision 4ms)
+void timerTS2(void const *argument) { // make sampling time TS2 timer (priority 2: precision 4ms)
int ms;
unsigned long c;
while (true) {
c = _count;
- //dummy(); // dummy() is called every TS3[s].
+ //dummy(); // dummy() is called every TS2[s].
+ if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
+ Thread::wait(ms);
+ }
+}
+void timerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
+ int ms;
+ unsigned long c;
+ while (true) {
+ c = _count;
+ data2mbedUSB(); // dummy() is called every TS3[s].
if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
Thread::wait(ms);
}
}
-
-void TS4timer(void const *argument) { // make sampling time TS4 timer (priority lowest: precision 4ms)
+void timerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms)
int ms;
unsigned long c;
while (true) {
@@ -357,10 +333,16 @@
}
}
-int main() {
- Thread startTS3timer(TS3timer,NULL,osPriorityBelowNormal);
- Thread startTS4timer(TS4timer,NULL,osPriorityLow);
-// Priority of Thread (RtosTimer has no priority?)
+void motor_control() { // motor control ON for TMAX seconds.
+ FILE *fp; // save data to PC
+ float t=0;
+#ifdef GOOD_DATA
+ int i;
+ RtosTimer RtosTimerTS1(th_controller); // RtosTimer priority is osPriorityAboveNormal, just one above main()
+//BUG(unstable!!) Thread startTimerTS2(timerTS2,NULL,osPriorityNormal);
+ Thread ThreadTimerTS3(timerTS3,NULL,osPriorityBelowNormal);
+ Thread ThreadTimerTS4(timerTS4,NULL,osPriorityLow);
+// Priority of Thread (RtosTimer is osPriorityAboveNormal)
// osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
// osPriorityLow = -2, ///< priority: low
// osPriorityBelowNormal = -1, ///< priority: below normal
@@ -369,6 +351,45 @@
// osPriorityHigh = +2, ///< priority: high
// osPriorityRealtime = +3, ///< priority: realtime (highest)
// osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
+
+ count3=0;
+ _count_data=0;
+#endif
+ init_controller(); // initialize controller parameters and signals
+ _r = _r + _th;
+// if( _r>2*PI ) _r -= _r-2*PI;
+
+ if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC
+
+ // start control (ON)
+ TickerTimerTS0.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
+ RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
+
+ t = _time;
+ while ( _time <= TMAX ) {
+ // BUG!! Dangerous if TS3<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)
+#ifndef GOOD_DATA // fprintf is dangerous because priority is higher than Ticker!
+ fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u); // save data to PC (para, y, time, u)
+#endif
+ Thread::wait((unsigned int)((TS3-(_time-t))*1000.)); //[ms]
+ t = _time;
+ }
+ // stop control (OFF)
+ TickerTimerTS0.detach(); // timer interrupt stop
+ RtosTimerTS1.stop(); // rtos timer stop
+// ThreadTimerTS3.terminate(); // if remove comment, mbed hangs up! why?
+// ThreadTimerTS4.terminate(); // if remove comment, mbed hangs up! why?
+
+ init_controller(); // initialize controller parameters and signals
+#ifdef GOOD_DATA
+ for(i=0;i<_count_data;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
+}
+
+int main() {
#if CONTROL_MODE==0 // PID control
char f;
float val;
@@ -382,12 +403,12 @@
#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]
+ data[i][4] = sin(_freq_u*2*PI * i*TS1); // _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);
+ data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS1);
if( data[i][4]>max_u ){ max_u=data[i][4];}
}
}
@@ -397,7 +418,7 @@
}
#endif
pc.printf("Control start!!\r\n");
- moror_control(); // motor control ON for TMAX seconds.
+ motor_control(); // motor control ON for TMAX seconds.
pc.printf("Control completed!!\r\n\r\n");
// Change parameters using tera term