DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Fork of DCmotor2 by
Hbridge.cpp
- Committer:
- kosakaLab
- Date:
- 2016-06-10
- Revision:
- 22:4fd506b1c68d
- Parent:
- 13:ba71733c11d7
File content as of revision 22:4fd506b1c68d:
// Hbridge.cpp: モータ駆動用ドライバのフルブリッジ(Hブリッジ)をPWM駆動する。 #include "mbed.h" #include "controller.h" #include "Hbridge.h" #define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], デッドタイム deadtime to be set between plus volt. to/from minus Timeout pwm; // タイムアウト関数の宣言(ある時間経過後に関数コールする) DigitalOut pwm_fwdIN = fwdIN_PORT; // デジタル信号を出力するポートを設定 DigitalOut pwm_rvsIN = rvsIN_PORT; // デジタル信号を出力するポートを設定 pwm_parameters IN; // フルブリッジのパラメータ宣言 AnalogIn VshuntR_plus(R_SHUNT_P_PORT); // *3.3 [V], シャント抵抗のプラス側アナログ入力, Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] AnalogIn VshuntR_minus(R_SHUNT_M_PORT); // *3.3 [V], シャント抵抗のマイナス側アナログ入力, Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A] DigitalOut debug_p24(p24); // p17 for debug //DigitalOut Led3(LED3); #if PWM_WAVEFORM==0 // PWM変調方式がノコギリ波比較のとき #if 1 void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] //debug_p24=1; IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす //IN.duty=0.9;IN.fReverse=1; if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする IN.mode = 0; // モードを0にする }else if( IN.mode==1 ){ // モードが1のとき if( IN.fReverse==0 ){ // モータの回転が順方向のとき pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする }else{ // モータの回転が逆方向のとき pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする } IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 ON time of pwm if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // rvsINをオンする時間幅を計算 OFF time of pwm if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする }else{// if( IN.mode==2 ){ // モードが2のとき pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] #ifndef SIMULATION // モータ電流の検出 p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; #endif pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN共に0にする IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする } //debug_p24=0; } #else void pwm_out() { // 補PWM pwm out using timer IN.mode += 1; if( IN.mode==1 ){ pwm_fwdIN = 1; pwm_rvsIN = 0; IN.fwdIN_us = IN.duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of UfwdIN if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} pwm.attach_us(&pwm_out, IN.fwdIN_us); // setup pwmU to call pwm_out after t [us] IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us - 2*DEADTIME_US; // ON time of UrvsIN if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} }else if( IN.mode==2 ){ pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] pwm_fwdIN = 0; pwm_rvsIN = 0; }else if( IN.mode==3 ){ pwm.attach_us(&pwm_out, IN.rvsIN_us); // setup pwmU to call pwm_out after t [us] pwm_fwdIN = 0; pwm_rvsIN = 1; }else{// if( u.mode==4 ){ pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] pwm_fwdIN = 0; pwm_rvsIN = 0; IN.mode = 0; } } #endif #elif PWM_WAVEFORM==1 // PWM変調方式が三角波比較のとき void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする IN.mode = 0; // モードを0にする }else if( IN.mode==1 ){ // モードが1のとき IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 // ON time of UfwdIN IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // fwdIN, rvsINをオンする時間幅を計算 // ON time of UrvsIN IN.rvsIN_us /= 2; // その時間幅を2で割る if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINをオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする }else if( IN.mode==2 ){ // モードが2のとき pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] if( IN.fReverse==0 ){ // モータの回転が順方向のとき pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする }else{ // モータの回転が逆方向のとき pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする } }else{// if( IN.mode==3 ){ // モードが3のとき pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] #ifndef SIMULATION // モータ電流の検出 p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; #endif pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする } } #endif void start_pwm(){ // PWMスタートする関数 IN.duty = 0.0; // デューティーを0にする pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする IN.fDeadtime = 1; // 正負切替時にデッドタイムを要求するフラグをオフにする IN.fReverse = 0; // モータ逆回転フラグをオフにする:回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 pwm_out(); // タイムアウト関数でPWMを発生する関数をスタート } void stop_pwm(){ // PWMストップする関数 IN.duty = 0.0; // デューティーを0にする pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする pwm.detach(); // タイムアウト関数をストップ }