DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Fork of DCmotor2 by
Hbridge.cpp
00001 // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ(Hブリッジ)をPWM駆動する。 00002 00003 #include "mbed.h" 00004 #include "controller.h" 00005 #include "Hbridge.h" 00006 00007 #define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], デッドタイム deadtime to be set between plus volt. to/from minus 00008 00009 Timeout pwm; // タイムアウト関数の宣言(ある時間経過後に関数コールする) 00010 00011 DigitalOut pwm_fwdIN = fwdIN_PORT; // デジタル信号を出力するポートを設定 00012 DigitalOut pwm_rvsIN = rvsIN_PORT; // デジタル信号を出力するポートを設定 00013 00014 pwm_parameters IN; // フルブリッジのパラメータ宣言 00015 00016 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] 00017 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] 00018 00019 DigitalOut debug_p24(p24); // p17 for debug 00020 //DigitalOut Led3(LED3); 00021 00022 #if PWM_WAVEFORM==0 // PWM変調方式がノコギリ波比較のとき 00023 #if 1 00024 void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] 00025 //debug_p24=1; 00026 IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす 00027 //IN.duty=0.9;IN.fReverse=1; 00028 if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき 00029 pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00030 pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする 00031 IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする 00032 IN.mode = 0; // モードを0にする 00033 }else if( IN.mode==1 ){ // モードが1のとき 00034 if( IN.fReverse==0 ){ // モータの回転が順方向のとき 00035 pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする 00036 }else{ // モータの回転が逆方向のとき 00037 pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする 00038 } 00039 IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 ON time of pwm 00040 if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする 00041 pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00042 IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // rvsINをオンする時間幅を計算 OFF time of pwm 00043 if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする 00044 }else{// if( IN.mode==2 ){ // モードが2のとき 00045 pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00046 #ifndef SIMULATION 00047 // モータ電流の検出 00048 p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; 00049 #endif 00050 pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN共に0にする 00051 IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする 00052 } 00053 //debug_p24=0; 00054 } 00055 #else 00056 void pwm_out() { // 補PWM pwm out using timer 00057 IN.mode += 1; 00058 if( IN.mode==1 ){ 00059 pwm_fwdIN = 1; 00060 pwm_rvsIN = 0; 00061 IN.fwdIN_us = IN.duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of UfwdIN 00062 if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} 00063 pwm.attach_us(&pwm_out, IN.fwdIN_us); // setup pwmU to call pwm_out after t [us] 00064 IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us - 2*DEADTIME_US; // ON time of UrvsIN 00065 if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} 00066 }else if( IN.mode==2 ){ 00067 pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] 00068 pwm_fwdIN = 0; 00069 pwm_rvsIN = 0; 00070 }else if( IN.mode==3 ){ 00071 pwm.attach_us(&pwm_out, IN.rvsIN_us); // setup pwmU to call pwm_out after t [us] 00072 pwm_fwdIN = 0; 00073 pwm_rvsIN = 1; 00074 }else{// if( u.mode==4 ){ 00075 pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] 00076 pwm_fwdIN = 0; 00077 pwm_rvsIN = 0; 00078 IN.mode = 0; 00079 } 00080 } 00081 #endif 00082 #elif PWM_WAVEFORM==1 // PWM変調方式が三角波比較のとき 00083 void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] 00084 IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす 00085 if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき 00086 pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00087 pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする 00088 IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする 00089 IN.mode = 0; // モードを0にする 00090 }else if( IN.mode==1 ){ // モードが1のとき 00091 IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 // ON time of UfwdIN 00092 IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // fwdIN, rvsINをオンする時間幅を計算 // ON time of UrvsIN 00093 IN.rvsIN_us /= 2; // その時間幅を2で割る 00094 if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする 00095 pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINをオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00096 if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする 00097 pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする 00098 }else if( IN.mode==2 ){ // モードが2のとき 00099 pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00100 if( IN.fReverse==0 ){ // モータの回転が順方向のとき 00101 pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする 00102 }else{ // モータの回転が逆方向のとき 00103 pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする 00104 } 00105 }else{// if( IN.mode==3 ){ // モードが3のとき 00106 pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] 00107 #ifndef SIMULATION 00108 // モータ電流の検出 00109 p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; 00110 #endif 00111 pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする 00112 IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする 00113 } 00114 } 00115 #endif 00116 00117 void start_pwm(){ // PWMスタートする関数 00118 IN.duty = 0.0; // デューティーを0にする 00119 pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする 00120 IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする 00121 IN.fDeadtime = 1; // 正負切替時にデッドタイムを要求するフラグをオフにする 00122 IN.fReverse = 0; // モータ逆回転フラグをオフにする:回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 00123 00124 pwm_out(); // タイムアウト関数でPWMを発生する関数をスタート 00125 } 00126 00127 void stop_pwm(){ // PWMストップする関数 00128 IN.duty = 0.0; // デューティーを0にする 00129 pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする 00130 IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする 00131 00132 pwm.detach(); // タイムアウト関数をストップ 00133 }
Generated on Tue Jul 12 2022 18:10:06 by 1.7.2