manabu kosaka / Mbed 2 deprecated DCmotor2

DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.

Dependencies:   QEI mbed-rtos mbed

Fork of DCmotor by manabu kosaka

controller.cpp@14:02411880ffb9, 2013-03-12 (annotated)

Committer:
kosaka
Date:
Tue Mar 12 04:32:22 2013 +0000
Revision:
14:02411880ffb9
Parent:
13:ba71733c11d7 
130303;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 13:ba71733c11d7 1 // controller.cpp: モータ制御器（位置制御、電流制御）
kosaka 13:ba71733c11d7 2 #include "mbed.h" // mbedマイコンではstdio.hに相当
kosaka 13:ba71733c11d7 3 #include "QEI.h" // エンコーダ用ライブラリを使用
kosaka 12:459af534d1ee 4
kosaka 13:ba71733c11d7 5 #include "controller.h" // controller.cpp: モータ制御器（位置制御、電流制御）
kosaka 13:ba71733c11d7 6 #include "Hbridge.h" // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
kosaka 13:ba71733c11d7 7 Serial pc(USBTX, USBRX); // PCのモニタ上のtera termに文字を表示する宣言
kosaka 12:459af534d1ee 8
kosaka 12:459af534d1ee 9 motor_parameters p; // モータの定数、信号など
kosaka 12:459af534d1ee 10 current_loop_parameters il; // 電流制御マイナーループの定数、変数
kosaka 12:459af534d1ee 11 velocity_loop_parameters vl; // 速度制御メインループの定数、変数
kosaka 12:459af534d1ee 12
kosaka 13:ba71733c11d7 13 QEI encoder (CH_A, CH_B, NC, N_ENC, QEI::X4_ENCODING); // エンコーダ用ライブラリを使用
kosaka 12:459af534d1ee 14 // QEI(PinName channelA, mbed pin for channel A input.
kosaka 12:459af534d1ee 15 // PinName channelB, mbed pin for channel B input.
kosaka 12:459af534d1ee 16 // PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
kosaka 12:459af534d1ee 17 // int pulsesPerRev, Number of pulses in one revolution(=360 deg).
kosaka 12:459af534d1ee 18 // Encoding encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as
kosaka 12:459af534d1ee 19 // X4 uses them on both channels.
kosaka 12:459af534d1ee 20 // )
kosaka 12:459af534d1ee 21 // void reset (void)
kosaka 12:459af534d1ee 22 // Reset the encoder.
kosaka 12:459af534d1ee 23 // int getCurrentState (void)
kosaka 12:459af534d1ee 24 // Read the state of the encoder.
kosaka 12:459af534d1ee 25 // int getPulses (void)
kosaka 12:459af534d1ee 26 // Read the number of pulses recorded by the encoder.
kosaka 12:459af534d1ee 27 // int getRevolutions (void)
kosaka 12:459af534d1ee 28 // Read the number of revolutions recorded by the encoder on the index channel.
kosaka 12:459af534d1ee 29 /*********** User setting for control parameters (end) ***************/
kosaka 12:459af534d1ee 30
kosaka 13:ba71733c11d7 31 AnalogOut analog_out(DA_PORT); // デバッグ用DA(アナログ信号をDA_PORTに出力)
kosaka 12:459af534d1ee 32
kosaka 13:ba71733c11d7 33 unsigned long _countTS0; // TS0[s]ごとのカウント数
kosaka 13:ba71733c11d7 34 float _time; // [s], プログラム開始時からの経過時間
kosaka 13:ba71733c11d7 35 float debug[20]; // デバッグ用変数
kosaka 13:ba71733c11d7 36 DigitalOut led1(LED1); // mbedマイコンのLED1を点灯
kosaka 13:ba71733c11d7 37 DigitalOut led2(LED2); // mbedマイコンのLED2を点灯
kosaka 13:ba71733c11d7 38 DigitalOut led3(LED3); // mbedマイコンのLED3を点灯
kosaka 13:ba71733c11d7 39 DigitalOut led4(LED4); // mbedマイコンのLED4を点灯
kosaka 12:459af534d1ee 40
kosaka 13:ba71733c11d7 41
kosaka 13:ba71733c11d7 42 float data[1000][5]; // PC上のmbed USB ディスクにセーブするデータ memory to save data offline instead of "online fprintf".
kosaka 13:ba71733c11d7 43 //unsigned int count3; //
kosaka 13:ba71733c11d7 44 //unsigned int count2=(int)(TS2/TS0); //
kosaka 13:ba71733c11d7 45 unsigned short _countTS3=0;
kosaka 13:ba71733c11d7 46
kosaka 12:459af534d1ee 47 DigitalOut debug_p26(p26); // p17 for debug
kosaka 12:459af534d1ee 48 DigitalOut debug_p25(p25); // p17 for debug
kosaka 12:459af534d1ee 49 //DigitalOut debug_p24(p24); // p17 for debug
kosaka 12:459af534d1ee 50 //AnalogIn VCC(p19); // *3.3 [V], Volt of VCC for motor
kosaka 12:459af534d1ee 51 //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.
kosaka 12:459af534d1ee 52
kosaka 12:459af534d1ee 53
kosaka 13:ba71733c11d7 54 void init_parameters(){
kosaka 13:ba71733c11d7 55 // モータ機器定数等、モータ・制御器の初期値の設定
kosaka 13:ba71733c11d7 56 // 親関数： main()
kosaka 13:ba71733c11d7 57 // 子関数： なし
kosaka 12:459af534d1ee 58 #ifdef SIMULATION
kosaka 13:ba71733c11d7 59 // モータ機器定数の設定
kosaka 13:ba71733c11d7 60 p.L = 0.0063; // [H], インダクタンス
kosaka 13:ba71733c11d7 61 p.R = 0.143; // [Ω], モータ巻線抵抗
kosaka 13:ba71733c11d7 62 p.phi = 0.176; // [V s], 永久磁石の鎖交磁束
kosaka 13:ba71733c11d7 63 p.Jm = 0.00018; // [Nms^2], イナーシャ
kosaka 12:459af534d1ee 64 p.p = 2; // 極対数
kosaka 12:459af534d1ee 65 #endif
kosaka 13:ba71733c11d7 66 // モータの初期値の設定
kosaka 13:ba71733c11d7 67 #ifdef SIMULATION
kosaka 13:ba71733c11d7 68 p.th[0] = p.th[1] = 0; // [rad], ロータの回転角, th[0]は現在の値, th[1]はTS0[s]だけ過去の値
kosaka 13:ba71733c11d7 69 #else
kosaka 13:ba71733c11d7 70 encoder.reset(); // 現在の回転角を原点に設定（エンコーダのカウント数をゼロに設定） set encoder counter zero
kosaka 13:ba71733c11d7 71 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
kosaka 13:ba71733c11d7 72 #endif
kosaka 13:ba71733c11d7 73 p.w = 0; // [rad/s], モータの回転速度
kosaka 13:ba71733c11d7 74 p.i = 0; // [A], モータ電流
kosaka 13:ba71733c11d7 75 p.v =0; // [V], モータ電圧
kosaka 12:459af534d1ee 76
kosaka 13:ba71733c11d7 77 // 制御器の初期値の設定
kosaka 12:459af534d1ee 78 vl.i_ref=0; // 電流指令[A]
kosaka 12:459af534d1ee 79 vl.w_lpf = 0; // 検出した速度[rad/s]
kosaka 13:ba71733c11d7 80 vl.eI = 0; // 速度制御用偏差の積分値（積分項）
kosaka 13:ba71733c11d7 81 vl.e_old = 0; // 速度制御用偏差の1サンプル過去の値
kosaka 13:ba71733c11d7 82 il.eI = 0; // 電流制御用偏差の積分値（積分項）
kosaka 13:ba71733c11d7 83 il.e_old = 0; // 電流制御用偏差の1サンプル過去の値
kosaka 12:459af534d1ee 84 }
kosaka 12:459af534d1ee 85
kosaka 13:ba71733c11d7 86 void i_control(){
kosaka 13:ba71733c11d7 87 // 電流PID制御器（電流マイナーループのフィードバック制御）
kosaka 13:ba71733c11d7 88 // 親関数： current_loop()
kosaka 13:ba71733c11d7 89 // 子関数： なし
kosaka 13:ba71733c11d7 90 // 入力：指令電流 il.i_ref [A], 実電流 p.i [A], PID制御積分項 il.eI, サンプル時間 TS0 [s]
kosaka 13:ba71733c11d7 91 // 出力：電圧指令 il.v_ref [A]
kosaka 13:ba71733c11d7 92 float e, ed;
kosaka 12:459af534d1ee 93
kosaka 13:ba71733c11d7 94 e = il.i_ref - p.i; // 電流偏差の計算
kosaka 13:ba71733c11d7 95
kosaka 13:ba71733c11d7 96 il.eI = il.eI + TS0*e; // 電流偏差の積分項の計算
kosaka 12:459af534d1ee 97
kosaka 13:ba71733c11d7 98 ed = (e-il.e_old)/TS0; // 電流偏差の微分値の計算
kosaka 13:ba71733c11d7 99 il.e_old = e; // 電流偏差の1サンプル過去の値を更新
kosaka 12:459af534d1ee 100
kosaka 13:ba71733c11d7 101 il.v_ref = iKP*e + iKI*il.eI + iKD*ed; // ＰＩＤ制御器の出力を計算
kosaka 12:459af534d1ee 102 }
kosaka 12:459af534d1ee 103
kosaka 13:ba71733c11d7 104 void current_loop(){
kosaka 13:ba71733c11d7 105 // 電流制御マイナーループ、サンプル時間TS0秒
kosaka 13:ba71733c11d7 106 // 親関数： timerTS0()
kosaka 13:ba71733c11d7 107 // 子関数： i_control()
kosaka 13:ba71733c11d7 108 // 入力：指令電流 il.i_ref [A], 実電流 p.i [A]
kosaka 13:ba71733c11d7 109 // 出力：電圧指令 p.v [V]
kosaka 13:ba71733c11d7 110
kosaka 12:459af534d1ee 111 #ifdef USE_CURRENT_CONTROL
kosaka 13:ba71733c11d7 112 i_control(); // 電流制御（電流フィードバック制御）
kosaka 12:459af534d1ee 113 #else
kosaka 13:ba71733c11d7 114 il.v_ref = il.i_ref/iMAX*vMAX; // 速度制御の出力をそのまま電圧指令にする
kosaka 12:459af534d1ee 115 #endif
kosaka 12:459af534d1ee 116 // 電圧指令の大きさをMAX制限
kosaka 13:ba71733c11d7 117 // アンチワインゴアップ：制御入力v_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI.
kosaka 13:ba71733c11d7 118 if( il.v_ref > vMAX ){ // 電圧指令がプラス側に大きすぎるとき
kosaka 13:ba71733c11d7 119 il.eI -= (il.v_ref - vMAX)/iKI; // 電圧指令がvMAXと等しくなる積分項を計算
kosaka 13:ba71733c11d7 120 if( il.eI<0 ){ il.eI=0;} // 積分項が負になるとゼロにする
kosaka 13:ba71733c11d7 121 il.v_ref = vMAX; // 電圧指令をvMAXにする
kosaka 13:ba71733c11d7 122 }else if( il.v_ref < -vMAX ){ // 電圧指令がマイナス側に大きすぎるとき
kosaka 13:ba71733c11d7 123 il.eI -= (il.v_ref + vMAX)/iKI; // 電圧指令が-vMAXと等しくなる積分項を計算
kosaka 13:ba71733c11d7 124 if( il.eI>0 ){ il.eI=0;} // 積分項が正になるとゼロにする
kosaka 13:ba71733c11d7 125 il.v_ref = -vMAX; // 電圧指令を-vMAXにする
kosaka 12:459af534d1ee 126 }
kosaka 13:ba71733c11d7 127 p.v = il.v_ref; // 指令電圧をp.vに格納してv2Hbridge()に渡す
kosaka 12:459af534d1ee 128 }
kosaka 12:459af534d1ee 129
kosaka 12:459af534d1ee 130
kosaka 12:459af534d1ee 131 void vel_control(){
kosaka 12:459af534d1ee 132 // 速度制御器：速度偏差が入力され、q軸電流指令を出力。
kosaka 13:ba71733c11d7 133 // 入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s], PI制御積分項 vl.eI, サンプル時間 TS1 [s]
kosaka 13:ba71733c11d7 134 // 出力：電流指令 vl.i_ref [A]
kosaka 13:ba71733c11d7 135 float e, ed;
kosaka 13:ba71733c11d7 136
kosaka 13:ba71733c11d7 137 e = vl.w_ref - vl.w_lpf; // 速度偏差の計算
kosaka 14:02411880ffb9 138 debug[1]=vl.w_ref/2/PI;//[Hz], for debug
kosaka 12:459af534d1ee 139
kosaka 13:ba71733c11d7 140 vl.eI = vl.eI + TS1*e; // 速度偏差の積分値の計算
kosaka 12:459af534d1ee 141
kosaka 13:ba71733c11d7 142 ed = (e-vl.e_old)/TS1; // 速度偏差の微分値の計算
kosaka 13:ba71733c11d7 143 vl.e_old = e; // 速度偏差の1サンプル過去の値を更新
kosaka 12:459af534d1ee 144
kosaka 13:ba71733c11d7 145 vl.i_ref = wKp*e + wKi*vl.eI + wKd*ed; // ＰＩＤ制御器の出力を計算
kosaka 12:459af534d1ee 146 }
kosaka 12:459af534d1ee 147
kosaka 13:ba71733c11d7 148 void velocity_loop(){
kosaka 13:ba71733c11d7 149 // 速度制御メインループ、サンプル時間TS1秒
kosaka 13:ba71733c11d7 150 // 親関数： timerTS1()
kosaka 13:ba71733c11d7 151 // 子関数： vel_control()
kosaka 13:ba71733c11d7 152 // 入力：指令速度 vl.w_ref [rad/s], 実速度 vl.w_lpf [rad/s]
kosaka 13:ba71733c11d7 153 // 出力：電圧指令 il.i_ref [A]
kosaka 12:459af534d1ee 154 float tmp;
kosaka 12:459af534d1ee 155
kosaka 13:ba71733c11d7 156 #if 1
kosaka 12:459af534d1ee 157 // 速度ωを求めるために、位置θをオイラー微分して、一次ローパスフィルタに通す。
kosaka 13:ba71733c11d7 158 tmp = p.th[0]-p.th[1]; // 回転角の差分を取る
kosaka 13:ba71733c11d7 159 while( tmp> PI ){ tmp -= 2*PI;} // 差分の値域を-π～+πに設定
kosaka 13:ba71733c11d7 160 while( tmp<-PI ){ tmp += 2*PI;} // 〃
kosaka 13:ba71733c11d7 161 vl.w_lpf = iLPF*vl.w_lpf + tmp/TS0 *(1-iLPF); // オイラー微分近似＋１次ＬＰＦで現在速度を計算
kosaka 12:459af534d1ee 162 #else
kosaka 12:459af534d1ee 163 vl.w_lpf = p.th[0];
kosaka 12:459af534d1ee 164 #endif
kosaka 13:ba71733c11d7 165
kosaka 13:ba71733c11d7 166 vel_control(); // 速度制御：速度偏差が入力され、電流指令を出力。
kosaka 12:459af534d1ee 167
kosaka 13:ba71733c11d7 168 // 電流指令の大きさをMAX制限
kosaka 13:ba71733c11d7 169 // アンチワインドアップ：制御入力i_refが飽和したとき積分項eIを減衰させる anti-windup: if u=v_ref is saturated, then reduce eI.
kosaka 13:ba71733c11d7 170 if( vl.i_ref > iMAX ){ // 電流指令がプラス側に大きすぎるとき
kosaka 13:ba71733c11d7 171 vl.eI -= (vl.i_ref - iMAX)/wKi; // 電流指令がvMAXと等しくなる積分項を計算
kosaka 13:ba71733c11d7 172 if( vl.eI<0 ){ vl.eI=0;} // 積分項が負になるとゼロにする
kosaka 13:ba71733c11d7 173 vl.i_ref = iMAX; // 電流指令をvMAXにする
kosaka 13:ba71733c11d7 174 }else if( vl.i_ref < -iMAX ){ // 電流指令がマイナス側に大きすぎるとき
kosaka 13:ba71733c11d7 175 vl.eI -= (vl.i_ref + iMAX)/wKi; // 電流指令が-vMAXと等しくなる積分項を計算
kosaka 13:ba71733c11d7 176 if( vl.eI>0 ){ vl.eI=0;} // 積分項が正になるとゼロにする
kosaka 13:ba71733c11d7 177 vl.i_ref = -iMAX; // 電流指令を-vMAXにする
kosaka 12:459af534d1ee 178 }
kosaka 12:459af534d1ee 179
kosaka 13:ba71733c11d7 180 il.i_ref = vl.i_ref; // 電流の目標値を速度制御メインループから電流制御マイナーループへ渡す。
kosaka 12:459af534d1ee 181 }
kosaka 12:459af534d1ee 182
kosaka 13:ba71733c11d7 183 void v2Hbridge(){
kosaka 13:ba71733c11d7 184 // 指令電圧vより、PWM関数pwm_out()のパラメータ（dutyとフラグ）をセット。
kosaka 13:ba71733c11d7 185 // 親関数： timerTS0()
kosaka 13:ba71733c11d7 186 // 子関数： なし
kosaka 13:ba71733c11d7 187 // 入力：電圧指令 p.v [V]
kosaka 13:ba71733c11d7 188 // 出力：フルブリッジのfwdIN, rvsIN用duty,
kosaka 13:ba71733c11d7 189 // デッドタイムフラグfDeadtime, モータ逆回転フラグfReverse
kosaka 12:459af534d1ee 190 float duty;
kosaka 12:459af534d1ee 191
kosaka 13:ba71733c11d7 192 duty = p.v/vMAX; // 指令電圧p.vの値を最大電圧vMAXで割って-1～1にしてdutyに代入
kosaka 13:ba71733c11d7 193 if( duty>=0 ){ // dutyがプラスでモータが順回転のとき
kosaka 13:ba71733c11d7 194 IN.duty = duty; // dutyをPWM関数pwm_out()に渡す
kosaka 13:ba71733c11d7 195 if( IN.fReverse==1 ){ // モータが逆回転から順回転に切り替ったとき
kosaka 13:ba71733c11d7 196 IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする
kosaka 12:459af534d1ee 197 }
kosaka 13:ba71733c11d7 198 IN.fReverse = 0; // 逆回転フラグをオフにする
kosaka 13:ba71733c11d7 199 }else{ // dutyがマイナスでモータが逆回転のとき
kosaka 13:ba71733c11d7 200 IN.duty = -duty; // dutyの絶対値をPWM関数pwm_out()に渡す
kosaka 13:ba71733c11d7 201 if( IN.fReverse==0 ){ // モータが順回転から逆回転に切り替ったとき
kosaka 13:ba71733c11d7 202 IN.fDeadtime = 1; // デッドタイム要求フラグをオンにする
kosaka 12:459af534d1ee 203 }
kosaka 13:ba71733c11d7 204 IN.fReverse = 1; // 逆回転フラグをオンにする
kosaka 12:459af534d1ee 205 }
kosaka 12:459af534d1ee 206 }
kosaka 12:459af534d1ee 207
kosaka 12:459af534d1ee 208 #ifdef SIMULATION
kosaka 12:459af534d1ee 209 void sim_motor(){
kosaka 13:ba71733c11d7 210 // ブラシ付きDCモータシミュレータ
kosaka 12:459af534d1ee 211 // 入力信号：電圧p.v [V]、負荷トルクp.TL [Nm]
kosaka 12:459af534d1ee 212 // 出力信号：モータ角度p.th[0] [rad], モータ速度p.w [rad/s], モータ電流p.i [A]
kosaka 12:459af534d1ee 213 float i_dot, Tall, TL;
kosaka 12:459af534d1ee 214 analog_out=p.v/100.+0.5;//debug
kosaka 12:459af534d1ee 215 //debug[0]=p.v;
kosaka 13:ba71733c11d7 216 // 電圧方程式より、指令電圧から電流を計算
kosaka 13:ba71733c11d7 217 i_dot = (1.0/p.L) * ( p.v - (p.R*p.i + p.w*p.phi) ); // 電圧方程式より電流の微分値を計算
kosaka 13:ba71733c11d7 218 p.i = p.i + TS0*i_dot; // オイラー近似微分で電流の微分値を積分して電流を求める
kosaka 12:459af534d1ee 219
kosaka 13:ba71733c11d7 220 // トルク方程式より、電流からモータトルクを計算
kosaka 12:459af534d1ee 221 p.Tm = p.p * p.phi * p.i;
kosaka 12:459af534d1ee 222
kosaka 12:459af534d1ee 223 // モータ速度ωの計算
kosaka 13:ba71733c11d7 224 if( abs(p.w) > 5*2*PI ){ // 速度が5rps以上のとき、減速するように負荷トルクTLをセット
kosaka 13:ba71733c11d7 225 if( p.w>=0 ){ TL= p.TL;} // 速度が正なら負荷トルクを+TLにセット
kosaka 13:ba71733c11d7 226 else{ TL=-p.TL;} // 速度が負なら負荷トルクを-TLにセット
kosaka 13:ba71733c11d7 227 }else{ // 速度が5rps以下のとき、速度に比例した負荷トルクをセット
kosaka 12:459af534d1ee 228 TL = p.w/(5*2*PI)*p.TL;
kosaka 13:ba71733c11d7 229 }
kosaka 12:459af534d1ee 230
kosaka 13:ba71733c11d7 231 Tall = p.Tm - TL; // モータのシャフトにかかるトルクを計算
kosaka 13:ba71733c11d7 232 p.w = p.w + TS0 * (1.0 / p.Jm) * Tall; // モータの機械的特性をシミュレートしてトルクから速度を計算
kosaka 12:459af534d1ee 233
kosaka 12:459af534d1ee 234 // モータ角度θの計算
kosaka 13:ba71733c11d7 235 p.th[0] = p.th[0] + TS0 * p.w; // オイラー近似微分で速度を積分して回転角を計算
kosaka 12:459af534d1ee 236
kosaka 13:ba71733c11d7 237 // 回転角の値域を０～２πにする
kosaka 13:ba71733c11d7 238 if( p.th[0]>2*PI) // 回転角が２π以上のとき
kosaka 13:ba71733c11d7 239 p.th[0] = p.th[0] - 2*PI; // ２π引く
kosaka 13:ba71733c11d7 240 if( p.th[0]<0 ) // 回転角が負のとき
kosaka 13:ba71733c11d7 241 p.th[0] = p.th[0] + 2*PI; // ２π足す
kosaka 12:459af534d1ee 242 }
kosaka 12:459af534d1ee 243 #endif
kosaka 12:459af534d1ee 244
kosaka 13:ba71733c11d7 245 void data2mbedUSB(){ // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 save data to mbed USB drive
kosaka 13:ba71733c11d7 246 if( _countTS3<1000 ){ // データ数が1,000の5種類のデータをメモリーに貯める
kosaka 14:02411880ffb9 247 data[_countTS3][0]=debug[0]; data[_countTS3][1]=debug[1];
kosaka 13:ba71733c11d7 248 data[_countTS3][2]=vl.w_lpf; data[_countTS3][3]=_countTS0*TS0; data[_countTS3][4]=il.v_ref;
kosaka 13:ba71733c11d7 249 _countTS3++;
kosaka 12:459af534d1ee 250 }
kosaka 12:459af534d1ee 251 #if 0
kosaka 13:ba71733c11d7 252 if( _countTS3<500 ){
kosaka 12:459af534d1ee 253 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];
kosaka 12:459af534d1ee 254 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];
kosaka 12:459af534d1ee 255 debug[10]=p.iuvw[1]; debug[11]=p.iuvw[2]; debug[12]=p.idq[0]; debug[13]=p.idq[1]; debug[14]=p.TL;
kosaka 13:ba71733c11d7 256 debug[15]=p.Tm; debug[16]=p.w; debug[17]=vl.w_lpf; debug[18]=p.th[0]; debug[19]=_countTS0*TS0;//_time;
kosaka 12:459af534d1ee 257 //BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
kosaka 12:459af534d1ee 258 for(j=0;j<19;j++){ printf("%f, ",debug[j]);} printf("%f\n",debug[19]);
kosaka 12:459af534d1ee 259 // for(j=0;j<19;j++){ pc.printf("%f, ",debug[j]);} pc.printf("%f\n",debug[19]);
kosaka 12:459af534d1ee 260 }
kosaka 12:459af534d1ee 261 #endif
kosaka 12:459af534d1ee 262 }
kosaka 13:ba71733c11d7 263 void timerTS0(){ // タイマーtimerTS0()はTS0[s]ごとにコールされる timer called every TS0[s].
kosaka 13:ba71733c11d7 264 // debug_p26 = 1;
kosaka 13:ba71733c11d7 265 _countTS0++; // カウンターに1足す
kosaka 13:ba71733c11d7 266 _time += TS0; // 現在の時間にTS0[s]足す
kosaka 12:459af534d1ee 267
kosaka 13:ba71733c11d7 268 current_loop(); // 電流制御マイナーループ(指令電流i_refからPID制御で指令電圧を計算)
kosaka 13:ba71733c11d7 269 v2Hbridge(); // 指令電圧を見てPWM関数pwm_out()のパラメータを決める volt. to Hbridge
kosaka 13:ba71733c11d7 270
kosaka 13:ba71733c11d7 271 // モータ回転角の検出
kosaka 13:ba71733c11d7 272 p.th[1] = p.th[0]; // // 1サンプル前の回転角p.th[1]を更新
kosaka 13:ba71733c11d7 273 #ifdef SIMULATION
kosaka 13:ba71733c11d7 274 sim_motor(); // モータシミュレータ
kosaka 13:ba71733c11d7 275 #else
kosaka 13:ba71733c11d7 276 p.th[0] = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // エンコーダで回転角を検出 get angle [rad] from encoder
kosaka 13:ba71733c11d7 277 #endif
kosaka 13:ba71733c11d7 278 // debug_p26 = 0;
kosaka 12:459af534d1ee 279 }
kosaka 12:459af534d1ee 280
kosaka 13:ba71733c11d7 281 void timerTS1(void const *argument){ // タイマーtimerTS1()はTS1[s]ごとにコールされる
kosaka 13:ba71733c11d7 282 // debug_p25 = 1;
kosaka 13:ba71733c11d7 283 velocity_loop(); // 速度制御メインループ(指令速度w_refから指令電流i_refを求める)
kosaka 13:ba71733c11d7 284 // debug_p25 = 0;
kosaka 13:ba71733c11d7 285 }
kosaka 13:ba71733c11d7 286
kosaka 13:ba71733c11d7 287 void display2PC(){ // PCのモニタ上のtera termに諸量を表示 display to tera term on PC
kosaka 13:ba71733c11d7 288 pc.printf("%8.1f[s]\t%8.2f[V]\t%8.2f [Hz]\t%8.2f\t%8.2f\r\n",
kosaka 13:ba71733c11d7 289 _time, il.v_ref, vl.w_lpf/(2*PI), vl.w_ref/(2*PI), debug[0]);
kosaka 12:459af534d1ee 290 // 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
kosaka 12:459af534d1ee 291 }
kosaka 13:ba71733c11d7 292 void timerTS2(){ // タイマーtimerTS2()はTS2[s]ごとにコールされる
kosaka 12:459af534d1ee 293 }
kosaka 13:ba71733c11d7 294 void timerTS3(){ // タイマーtimerTS3()はTS3[s]ごとにコールされる
kosaka 13:ba71733c11d7 295 data2mbedUSB(); // PC上のmbed USB ディスクにセーブするためのデータをTS3[s]ごとに代入 data2mbedUSB() is called every TS3[s].
kosaka 12:459af534d1ee 296 }
kosaka 13:ba71733c11d7 297 void timerTS4(){ // タイマーtimerTS4()はTS4[s]ごとにコールされる
kosaka 13:ba71733c11d7 298 display2PC(); // PCのモニタ上のtera termに文字を表示 display to tera term on PC. display2PC() is called every TS4[s].
kosaka 12:459af534d1ee 299 }