manabu kosaka
UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of
DCmotor
by 
manabu kosaka
Diff: main.cpp
- Revision:
- 12:a4b17bb682eb
- Parent:
- 11:9747752435d1
- Child:
- 13:791e20f1af43
--- a/main.cpp Thu Nov 29 09:25:56 2012 +0000
+++ b/main.cpp Fri Dec 21 22:06:56 2012 +0000
@@ -1,406 +1,81 @@
-// DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
-// ver. 121129a by Kosaka lab.
+// UVW three phases Blushless DC motor control program using 3 H-bridge driver (ex. BD6211F) and 360 resolution rotary encoder with A, B phase.
+// ver. 121206 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 "fast_math.h"
+#include "controller.h"
+#include "UVWpwm.h"
-Serial pc(USBTX, USBRX); // Display on tera term in PC
-LocalFileSystem local("local"); // save data to mbed USB disk drive in PC
+Serial pc2(USBTX, USBRX); // Display on tera term in PC
+LocalFileSystem local("mbedUSBdrive"); // 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
+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.
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;
+FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); //save data to PC
+Timeout emergencyStop; // kill fprintf process when bug
- 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
- }
- 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);
- }
-#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)
-// }
+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].
+ }
+ if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
+ Thread::wait(ms);
}
- 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);
+//void stop_fprintf(){ // emergencyStop if fprintf keep busy for 3 secons
+// fclose(fp);
+// pc2.printf("error: fprintf was in hung-up!");
+//}
-// osStatus set_priority(osPriority osPriorityBelowNormal );
-// Priority of Thread (RtosTimer has no priority?)
+//#define OLD
+int main(){
+ int ms=1;
+ unsigned long c, c2;
+ unsigned short i, i2=0;
+// FILE *fp; // save data to PC
+// FILE *fp = fopen("/mbedUSBdrive/data.csv", "w");
+ char chr[100];
+ 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 +84,156 @@
// osPriorityHigh = +2, ///< priority: high
// osPriorityRealtime = +3, ///< priority: realtime (highest)
// osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
+
+ // シミュレーション総サンプル数
+ int N;// = 5000;
+ // 指令速度
+ float w_ref_req[2] = {20* 2*PI, 40* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
+ float w_ref;
+ // 指令ｄｑ電流位相
+ float beta_ref = 30*PI/180; // rad
+ float tan_beta_ref1;
+ float tan_beta_ref2,tan_beta_ref;
+
+// start_timers(1); // start multi-timers, sampling times are TS0, TS1, TS2, TS3, TS4.
+
+ N = (int)(TMAX/TS0);
+pc2.printf("N=%d\r\n",N);
+ // IPMSMの機器定数等の設定, 制御器の初期化
+ init_parameters();
+ p.th[0] = 2*PI/3; //θの初期値
+
+
+// p.Lq0 = p.Ld; // SPMSMの場合
+// p.phi = 0; // SynRMの場合
+
+// w_ref=p.w; // 速度指令[rad/s]
+ tan_beta_ref1 = tan(beta_ref);
+ tan_beta_ref2 = tan(beta_ref-30*PI/180);
+ tan_beta_ref = tan_beta_ref1;
+ // シミュレーション開始
+ 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
+
+ // set th by moving rotor to th=zero.
+ f_find_origin=1;
+ while( _count*TS0<0.1 ){ // while( time<1 ){
+// debug_p24 = 1;
+ il.idq_ref[0] = iqMAX/1.0;
+ il.idq_ref[1] = 0;
+
+#ifdef OLD
+ timerTS0();
+ //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+ //vuvw2pwm(); // vuvw to pwm
+ //sim_motor(); // IPM, ｄｑ座標
+#endif
+
+// if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0
+ Thread::wait(ms);
+// debug_p24 = 0;
+ }
+//pc2.printf("\r\n^0^ 0\r\n");
+#ifndef SIMULATION
+ encoder.reset(); // set encoder counter zero
+ p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
+#endif
+ c2 = _count;
+ f_find_origin=0;
+
+#ifndef OLD
+ TickerTimerTS0.detach(); // timer interrupt stop
+ // start control (ON)
+ TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
+ RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
+#endif
+
+ for( i=0;i<N;i++ ){
+// debug_p24 = 1;
+ c = _count-c2;
+ // 電流位相（ｄｑ軸電流）変化
+ // if( i>=1500&&i<1900 ){// TS0=0.0001
+ if( c>=1500*0.0001/TS0&&c<1900*0.0001/TS0 ){
+ if( tan_beta_ref>tan_beta_ref2 ){ tan_beta_ref=tan_beta_ref-0.002;}
+ }else{
+ if( tan_beta_ref<tan_beta_ref1){ tan_beta_ref=tan_beta_ref+0.002;}
+ }
+
+ // 速度急変
+ w_ref = w_ref_req[0];
+ if( 2600*0.0001/TS0<=c&&c<3000*0.0001/TS0 ){
+ w_ref=w_ref_req[1];
+//pc2.printf(".\r\n");
+ }
+#ifdef SIMULATION
+ // 負荷トルク急変
+ if( c<4000*0.0001/TS0 ){
+ p.TL = 1;
+ }else{
+ p.TL = 2;
+ }
+#endif
+ vl.w_ref = w_ref; // 目標速度をメインルーチンから速度制御メインループへ渡す。
+ vl.tan_beta_ref = tan_beta_ref; // 目標電流位相をメインルーチンから速度制御メインループへ渡す。
+
+#ifdef OLD
+ if( (++i2)>=(int)(TS1/TS0) ){ i2=0;
+ timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
+ }
+#endif
+
+#ifdef OLD
+ timerTS0();
+ //current_loop(); // 電流制御マイナーループ(idq_ref to vuvw)
+ //vuvw2pwm(); // vuvw to pwm
+ //sim_motor(); // IPM, ｄｑ座標
+#endif
+
+// if( (ms=(int)(TS1*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}// ms=TS0
+ Thread::wait(ms);
+// debug_p24 = 0;
+ }
+//pc2.printf("\r\n^0^ 2\r\n");
+ // stop timers (OFF)
+ stop_pwm();
+ TickerTimerTS0.detach(); // timer interrupt stop
+ RtosTimerTS1.stop(); // rtos timer stop
+// Thread::wait(1000); // wait till timerTS3 completed
+ fTimerTS3ON=0;//ThreadTimerTS3.terminate(); //
+ fTimerTS4ON=0;//ThreadTimerTS4.terminate(); //
+//pc2.printf("\r\n^0^ 0\r\n\r\n");
+
+ // save data to mbed USB drive
+// if ( NULL == (fp = fopen( "/mbedUSBdrive/data.csv", "w" )) ){ error( "" );} // save data to PC
+//pc2.printf("\r\n^0^ %d\r\n\r\n",_count_data);
+// emergencyStop.attach(&stop_fprintf, 0.001); // emergencyStop if fprintf keep busy for 3 secons
+ for(i=0;i<_count_data;i++){
+//pc2.printf("%d: ",i);
+//pc2.printf("%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)
+// sprintf(&chr[0],"Temperature: f ºC\r\n");//,data[i][0]);
+// sprintf(&chr[0],"%d, ", data[i][1]);
+// fprintf(fp,&chr[0]);
+// fprintf( fp, ", ");
+// fprintf( fp, "%d, ", data[i][1]*10000);
+// fprintf( fp, "\r\n");
+//
+// 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)
+//
+// wait(0.1);
+// Thread::wait(100);
+ }
+//pc2.printf("\r\n^0^ 2\r\n\r\n");
+ fclose( fp ); // release mbed USB drive
+
+ Thread::wait(100);
+ pc2.printf("Control completed!!\r\n\r\n");
}