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:
- 12:459af534d1ee
- Parent:
- 11:9747752435d1
- Child:
- 13:ba71733c11d7
--- a/main.cpp Thu Nov 29 09:25:56 2012 +0000 +++ b/main.cpp Fri Jan 04 12:00:48 2013 +0000 @@ -1,406 +1,66 @@ // DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase. -// ver. 121129a by Kosaka lab. +// ver. 121224 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 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 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 _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.0001 // [s], iTS, sampling time[s] of motor current i control PID using timer interrupt -#define thTS 0.001 // [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); -// 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) ***************/ +#include "controller.h" +#include "Hbridge.h" -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. - -#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 - PwmOut RIN(RIN_PORT); // PWM for RIN, FIN=0 when reverse rotation. H bridge driver PWM mode -#else // H bridge Vref=analog mode - DigitalOut FIN(FIN_PORT);// FIN for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation - DigitalOut RIN(RIN_PORT);// RIN for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation -#endif -AnalogOut analog_out(VREF_PORT);// Vref for DC motor H bridge driver. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V] - -unsigned long _count; // sampling number -float _time; // time[s] -float _r; // reference signal -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 _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/iTS); // -#endif +Serial pc2(USBTX, USBRX); // Display on tera term in PC +LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC +Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer. +unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // ON/OFF flag for timerTS2, 3, 4. +//DigitalOut debug_p24(p24); // p17 for debug extern "C" void mbed_reset(); -void u2Hbridge(float u){// input u to H bridge driver - float duty; - unsigned int f_deadtime, f_in, r_in; - - if( u > 0 ){ // forward: rotate to plus - u += DEADZONE_PLUS; // deadzone compensation - duty = u/3.3; // Vref - 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{ - f_deadtime = 0; // deadtime is required +void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) + int ms; + unsigned long c; + while (true) { + c = _count; + if( fTimerTS2ON ){ + timerTS2(); // timerTS2() is called every TS2[s]. } - f_in=1; r_in=0; // set forward direction - }else if( u < 0 ){ // reverse: rotate to minus - u += DEADZONE_MINUS;// deadzone compensation - duty = -u/3.3; - 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{ - f_deadtime = 0; // deadtime is required - } - f_in=0; r_in=1; // set reverse direction - }else{// if( u == 0 ){ // stop mode - duty = 0; - f_deadtime = 0; // deadtime is required - f_in=0; r_in=0; // set FIN & RIN - } - - if( f_deadtime==1 ){// making deadtime - FIN=0; RIN=0; // set upper&lower arm zero - wait(DEADTIME); + if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} + Thread::wait(ms); } -#ifdef USE_PWM // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0 - FIN = duty*(float)f_in; RIN = duty*(float)r_in; // setting pwm FIN & RIN - analog_out = 1; // setting Vref=UMAX, but Vref=Vcc is better. -#else // Analog mode: Vref=analog, FIN, RIN = 1 or 0) - FIN = f_in; RIN = r_in; // setting FIN & RIN - analog_out = duty; // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v -#endif -} - -void th_controller(void const *argument) { // if rtos. current controller & velocity controller - float e_old, wt; - float y, u; - -// y_old = _th; // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data -#ifdef SIMULATION - y = _th + thTS/0.1*(0.2*_iref*100-_th); //=(1-TS/0.1)*_y + 0.2*TS/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 -#define RMIN 0 - 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) - if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){ // e is inside minimum precision? - _e = 0; - } - 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 = _Kp4th*_e + _Kd4th*(_e-e_old)/thTS + _Ki4th*_eI; // PID output u(t) - -#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)); - 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 -#if CONTROL_MODE==3 // u=rand() to identify motor transfer function G(s) from V to angle - 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 = _iref; - } -#endif -#if CONTROL_MODE==4 // FFT identification, u=repetive signal - if(count2==(int)(TS2/thTS)){ - u = data[count3][4]; - }else{ - u = _iref; - } -#endif - // 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-TS) is older than e by 1 sampling time TS[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/IMAX*VMAX; // without current control. -#endif - - u2Hbridge(_u); // input u to TA7291 driver - - //-------- update data - _time += iTS; // time -debug[0]=v_shunt_r; if(_f_u_plus==0){ debug[0]=-debug[0];} -#ifdef GOOD_DATA - 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]=_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++; +void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms) + int ms; + unsigned long c; + while (true) { + c = _count; + if( fTimerTS3ON ){ + timerTS3(); // timerTS3() is called every TS3[s]. } - count2 = 0; - } - count2++; -#endif - //-------- update data - - debug_p17 = 0; // for debug: processing time check -} - -void main1() { - RtosTimer timer_controller(th_controller); - FILE *fp; // save data to PC -#ifdef GOOD_DATA - int i; - - count3=0; -#endif - u2Hbridge(0); // initialize H bridge to stop mode - _count=0; - _time = 0; // time - _eI = _eiI = 0; // reset integrater - encoder.reset(); // set encoder counter zero - _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"); - if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC -#ifdef USE_PWM - FIN.period( 1.0 / PWM_FREQ ); // PWM period [s]. Common to all PWM -#endif - controller_ticker.attach(&i_controller, iTS ); // Sampling period[s] of i_controller - timer_controller.start((unsigned int)(thTS*1000.)); // Sampling period[ms] of th controller - -// 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. -//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], _th, _time, _u); // save data to PC (para, y, time, u) -#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 - 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)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT); // print to tera term - Thread::wait(200); + if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} + Thread::wait(ms); } } -void main2(void const *argument) { -#if CONTROL_MODE==0 // PID control - char f; - float val; -#endif -#if CONTROL_MODE==4 // FFT identification, u=repetive signal - int i, j; - float max_u; -#endif - - while(true){ -#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] - if( data[i][4]>max_u ){ max_u=data[i][4];} +void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms) + int ms; + unsigned long c; + while (true) { + c = _count; + if( fTimerTS4ON ){ + timerTS4(); // timerTS4() is called every TS4[s]. } - 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); - if( data[i][4]>max_u ){ max_u=data[i][4];} - } - } - for( i=0;i<1000;i++ ){ -// data[i][4] *= UMAX/max_u; - data[i][4] = (data[i][4]/max_u+3)/4*UMAX; - } -#endif - main1(); + if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;} + Thread::wait(ms); + } +} -#if CONTROL_MODE>=1 // frequency response, or Step response - pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)..."); - pc.scanf("%f",&_freq_u); - pc.printf("%8.3f[Hz]\r\n", _freq_u); // print to tera term - if(_freq_u==9){ mbed_reset();} -#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("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<=8){ pc.scanf("%f",&val);} - pc.printf("%8.3f\r\n", val); // print to tera term - 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("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 - } -} -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?) +//#define OLD +int main(){ + unsigned short i; + FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC + RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main() + Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal); + Thread ThreadTimerTS4(CallTimerTS4,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 @@ -409,4 +69,70 @@ // osPriorityHigh = +2, ///< priority: high // osPriorityRealtime = +3, ///< priority: realtime (highest) // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority + + // 指令速度 + float w_ref_req[2] = {2* 2*PI, 4* 2*PI}; // [rad/s](第2要素は指令速度急変後の指令速度) + float t; // current time + + // IPMSMの機器定数等の設定, 制御器の初期化 + init_parameters(); + + // シミュレーション開始 + pc2.printf("Simulation start!!\r\n"); +#ifndef OLD + // start control (ON) + start_pwm(); + TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller + RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller + fTimerTS3ON = 1; // timerTS3 start + fTimerTS4ON = 1; // timerTS3 start +#endif + + while( (t = _count*TS0) < TMAX ){ +// debug_p24 = 1; + + // 速度急変 + if( 0.26<=t && t<2.3 ){ + vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。 + }else{ + vl.w_ref=w_ref_req[0]; + } +#ifdef SIMULATION + // 負荷トルク急変 + if( t<3.4 ){ + p.TL = 1; + }else{ + p.TL = 2; + } +#endif + +#ifdef OLD + if( (++i2)>=(int)(TS1/TS0) ){ i2=0; + timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref) + } +#endif + +#ifdef OLD + timerTS0(); +#endif + +// debug_p24 = 0; + Thread::wait(1); + } + // stop timers (OFF) + stop_pwm(); + TickerTimerTS0.detach(); // timer interrupt stop + RtosTimerTS1.stop(); // rtos timer stop + fTimerTS3ON=0;//ThreadTimerTS3.terminate(); // + fTimerTS4ON=0;//ThreadTimerTS4.terminate(); // + + // save data to mbed USB drive + 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) + } + fclose( fp ); // release mbed USB drive + + Thread::wait(100); + pc2.printf("Control completed!!\r\n\r\n"); }