DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Fork of DCmotor2 by
controller.cpp
- Committer:
- kosakaLab
- Date:
- 2016-06-10
- Revision:
- 22:4fd506b1c68d
- Parent:
- 15:29797f995594
File content as of revision 22:4fd506b1c68d:
// controller.cpp: モータ制御器(位置制御、電流制御) #include "mbed.h" // mbedマイコンではstdio.hに相当 #include "QEI.h" // エンコーダ用ライブラリを使用 #include "controller.h" // controller.cpp: モータ制御器(位置制御、電流制御) #include "Hbridge.h" // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ(Hブリッジ)をPWM駆動する。 //Serial pc(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言 motor_parameters p; // モータの定数、信号など current_loop_parameters il; // 電流制御マイナーループの定数、変数 velocity_loop_parameters vl; // 速度制御メインループの定数、変数 QEI encoder (CH_A, CH_B, 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) ***************/ AnalogOut analog_out(DA_PORT); // デバッグ用DA(アナログ信号をDA_PORTに出力) unsigned long _countTS0; // TS0[s]ごとのカウント数 float _time; // [s], プログラム開始時からの経過時間 float debug[20]; // デバッグ用変数 DigitalOut led1(LED1); // mbedマイコンのLED1を点灯 DigitalOut led2(LED2); // mbedマイコンのLED2を点灯 DigitalOut led3(LED3); // mbedマイコンのLED3を点灯 DigitalOut led4(LED4); // mbedマイコンのLED4を点灯 float data[1000][5]; // PC上のmbed USB ディスクにセーブするデータ memory to save data offline instead of "online fprintf". //unsigned int count3; // //unsigned int count2=(int)(TS2/TS0); // unsigned short _countTS3=0; DigitalOut debug_p26(p26); // p17 for debug DigitalOut debug_p25(p25); // p17 for debug //DigitalOut debug_p24(p24); // p17 for debug //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. void init_parameters(){ // モータ機器定数等、モータ・制御器の初期値の設定 // 親関数: main() // 子関数: なし #ifdef SIMULATION // モータ機器定数の設定 p.L = 0.0063; // [H], インダクタンス p.R = 0.143; // [Ω], モータ巻線抵抗 p.phi = 0.176; // [V s], 永久磁石の鎖交磁束 p.Jm = 0.00018; // [Nms^2], イナーシャ p.p = 2; // 極対数 #endif // モータの初期値の設定 #ifdef SIMULATION p.th[0] = p.th[1] = 0; // [rad], ロータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値 #else encoder.reset(); // 現在の回転角を原点に設定(エンコーダのカウント数をゼロに設定) set encoder counter zero p.th[0] = p.th[1] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // [rad], ロータの回転角をエンコーダ検出値に設定, th[0]は現在の値, th[1]はTS0[s]だけ過去の値 get angle [rad] from encoder #endif p.w = 0; // [rad/s], モータの回転速度 p.i = 0; // [A], モータ電流 p.v =0; // [V], モータ電圧 // 制御器の初期値の設定 vl.i_ref=0; // 電流指令[A] vl.w_lpf = 0; // 検出した速度[rad/s] vl.eI = 0; // 速度制御用偏差の積分値(積分項) vl.e_old = 0; // 速度制御用偏差の1サンプル過去の値 il.eI = 0; // 電流制御用偏差の積分値(積分項) il.e_old = 0; // 電流制御用偏差の1サンプル過去の値 } void i_control(){ // 電流PID制御器(電流マイナーループのフィードバック制御) // 親関数: current_loop() // 子関数: なし // 入力:指令電流 il.i_ref [A], 実電流 p.i [A], PID制御積分項 il.eI, サンプル時間 TS0 [s] // 出力:電圧指令 il.v_ref [A] float e, ed; e = il.i_ref - p.i; // 電流偏差の計算 il.eI = il.eI + TS0*e; // 電流偏差の積分項の計算 ed = (e-il.e_old)/TS0; // 電流偏差の微分値の計算 il.e_old = e; // 電流偏差の1サンプル過去の値を更新 il.v_ref = iKP*e + iKI*il.eI + iKD*ed; // PID制御器の出力を計算 } void current_loop(){ // 電流制御マイナーループ、サンプル時間TS0秒 // 親関数: timerTS0() // 子関数: i_control() // 入力:指令電流 il.i_ref [A], 実電流 p.i [A] // 出力:電圧指令 p.v [V] #ifdef USE_CURRENT_CONTROL i_control(); // 電流制御(電流フィードバック制御) #else il.v_ref = il.i_ref/iMAX*vMAX; // 速度制御の出力をそのまま電圧指令にする #endif // 電圧指令の大きさをMAX制限 // アンチワインゴアップ:制御入力v_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI. if( il.v_ref > vMAX ){ // 電圧指令がプラス側に大きすぎるとき il.eI -= (il.v_ref - vMAX)/iKI; // 電圧指令がvMAXと等しくなる積分項を計算 if( il.eI<0 ){ il.eI=0;} // 積分項が負になるとゼロにする il.v_ref = vMAX; // 電圧指令をvMAXにする }else if( il.v_ref < -vMAX ){ // 電圧指令がマイナス側に大きすぎるとき il.eI -= (il.v_ref + vMAX)/iKI; // 電圧指令が-vMAXと等しくなる積分項を計算 if( il.eI>0 ){ il.eI=0;} // 積分項が正になるとゼロにする il.v_ref = -vMAX; // 電圧指令を-vMAXにする } p.v = il.v_ref; // 指令電圧をp.vに格納してv2Hbridge()に渡す } void vel_control(){ // 速度制御器:速度偏差が入力され、q軸電流指令を出力。 // 入力:指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s], PI制御積分項 vl.eI, サンプル時間 TS1 [s] // 出力:電流指令 vl.i_ref [A] float e, ed; e = vl.w_ref - vl.w_lpf; // 速度偏差の計算 debug[1]=vl.w_ref/2/PI;//[Hz], for debug vl.eI = vl.eI + TS1*e; // 速度偏差の積分値の計算 ed = (e-vl.e_old)/TS1; // 速度偏差の微分値の計算 vl.e_old = e; // 速度偏差の1サンプル過去の値を更新 vl.i_ref = wKp*e + wKi*vl.eI + wKd*ed; // PID制御器の出力を計算 } void velocity_loop(){ // 速度制御メインループ、サンプル時間TS1秒 // 親関数: timerTS1() // 子関数: vel_control() // 入力:指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s] // 出力:電圧指令 il.i_ref [A] float tmp; #if 1 // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。 tmp = p.th[0]-p.th[1]; // 回転角の差分を取る while( tmp> PI ){ tmp -= 2*PI;} // 差分の値域を-π~+πに設定 while( tmp<-PI ){ tmp += 2*PI;} // 〃 vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF); // オイラー微分近似+1次LPFで現在速度を計算 #else vl.w_lpf = p.th[0]; #endif vel_control(); // 速度制御:速度偏差が入力され、電流指令を出力。 // 電流指令の大きさをMAX制限 // アンチワインドアップ:制御入力i_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI. if( vl.i_ref > iMAX ){ // 電流指令がプラス側に大きすぎるとき vl.eI -= (vl.i_ref - iMAX)/wKi; // 電流指令がvMAXと等しくなる積分項を計算 if( vl.eI<0 ){ vl.eI=0;} // 積分項が負になるとゼロにする vl.i_ref = iMAX; // 電流指令をvMAXにする }else if( vl.i_ref < -iMAX ){ // 電流指令がマイナス側に大きすぎるとき vl.eI -= (vl.i_ref + iMAX)/wKi; // 電流指令が-vMAXと等しくなる積分項を計算 if( vl.eI>0 ){ vl.eI=0;} // 積分項が正になるとゼロにする vl.i_ref = -iMAX; // 電流指令を-vMAXにする } il.i_ref = vl.i_ref; // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。 } void v2Hbridge(){ // 指令電圧vより、PWM関数pwm_out()のパラメータ(dutyとフラグ)をセット。 // 親関数: timerTS0() // 子関数: なし // 入力:電圧指令 p.v [V] // 出力:フルブリッジのfwdIN, rvsIN用duty, // デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse float duty; duty = p.v/vMAX; // 指令電圧p.vの値を最大電圧vMAXで割って-1~1にしてdutyに代入 if( duty>=0 ){ // dutyがプラスでモータが順回転のとき IN.duty = duty; // dutyをPWM関数pwm_out()に渡す if( IN.fReverse==1 ){ // モータが逆回転から順回転に切り替ったとき IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする } IN.fReverse = 0; // 逆回転フラグをオフにする }else{ // dutyがマイナスでモータが逆回転のとき IN.duty = -duty; // dutyの絶対値をPWM関数pwm_out()に渡す if( IN.fReverse==0 ){ // モータが順回転から逆回転に切り替ったとき IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする } IN.fReverse = 1; // 逆回転フラグをオンにする } } #ifdef SIMULATION void sim_motor(){ // ブラシ付きDCモータシミュレータ // 入力信号:電圧p.v [V]、負荷トルクp.TL [Nm] // 出力信号:モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A] float i_dot, Tall, TL; analog_out=p.v/100.+0.5;//debug //debug[0]=p.v; // 電圧方程式より、指令電圧から電流を計算 i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) ); // 電圧方程式より電流の微分値を計算 p.i = p.i + TS0*i_dot; // オイラー近似微分で電流の微分値を積分して電流を求める // トルク方程式より、電流からモータトルクを計算 p.Tm = p.p * p.phi * p.i; // モータ速度ωの計算 if( abs(p.w) > 5*2*PI ){ // 速度が5rps以上のとき、減速するように負荷トルクTLをセット if( p.w>=0 ){ TL= p.TL;} // 速度が正なら負荷トルクを+TLにセット else{ TL=-p.TL;} // 速度が負なら負荷トルクを-TLにセット }else{ // 速度が5rps以下のとき、速度に比例した負荷トルクをセット TL = p.w/(5*2*PI)*p.TL; } Tall = p.Tm - TL; // モータのシャフトにかかるトルクを計算 p.w = p.w + TS0 * (1.0 / p.Jm) * Tall; // モータの機械的特性をシミュレートしてトルクから速度を計算 // モータ角度θの計算 p.th[0] = p.th[0] + TS0 * p.w; // オイラー近似微分で速度を積分して回転角を計算 // 回転角の値域を0~2πにする if( p.th[0]>2*PI) // 回転角が2π以上のとき p.th[0] = p.th[0] - 2*PI; // 2π引く if( p.th[0]<0 ) // 回転角が負のとき p.th[0] = p.th[0] + 2*PI; // 2π足す } #endif void data2mbedUSB(){ // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 save data to mbed USB drive if( _countTS3<1000 ){ // データ数が1,000の5種類のデータをメモリーに貯める data[_countTS3][0]=debug[0]; data[_countTS3][1]=debug[1]; data[_countTS3][2]=vl.w_lpf; data[_countTS3][3]=_countTS0*TS0; data[_countTS3][4]=il.v_ref; _countTS3++; } #if 0 if( _countTS3<500 ){ debug[0]=p.vab[0]; debug[1]=p.vab[1]; debug[2]=il.vdq_ref[0]; debug[3]=il.vdq_ref[1]; debug[4]=p.iab[0]; debug[5]=p.iab[1]; debug[6]=p.vuvw[0]; debug[7]=p.vuvw[1]; debug[8]=p.vuvw[2]; debug[9]=p.iuvw[0]; debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL; debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_countTS0*TS0;//_time; //BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]); for(j=0;j<19;j++){ printf("%f, ",debug[j]);} printf("%f\n",debug[19]); // for(j=0;j<19;j++){ pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]); } #endif } void timerTS0(){ // タイマーtimerTS0()はTS0[s]ごとにコールされる timer called every TS0[s]. // debug_p26 = 1; _countTS0++; // カウンターに1足す _time += TS0; // 現在の時間にTS0[s]足す current_loop(); // 電流制御マイナーループ(指令電流i_refからPID制御で指令電圧を計算) v2Hbridge(); // 指令電圧を見てPWM関数pwm_out()のパラメータを決める volt. to Hbridge // モータ回転角の検出 p.th[1] = p.th[0]; // // 1サンプル前の回転角p.th[1]を更新 #ifdef SIMULATION sim_motor(); // モータシミュレータ #else p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // エンコーダで回転角を検出 get angle [rad] from encoder #endif // debug_p26 = 0; } void timerTS1(void const *argument){ // タイマーtimerTS1()はTS1[s]ごとにコールされる // debug_p25 = 1; velocity_loop(); // 速度制御メインループ(指令速度w_refから指令電流i_refを求める) // debug_p25 = 0; } void display2PC(){ // PCのモニタ上のtera termに諸量を表示 display to tera term on PC printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n", _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]); // 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 timerTS2(){ // タイマーtimerTS2()はTS2[s]ごとにコールされる } void timerTS3(){ // タイマーtimerTS3()はTS3[s]ごとにコールされる data2mbedUSB(); // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 data2mbedUSB() is called every TS3[s]. } void timerTS4(){ // タイマーtimerTS4()はTS4[s]ごとにコールされる display2PC(); // PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s]. }