Hbridge.cpp

00001 //  Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
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;   // チョッピングのオンオフを決定するモードを１増やす
00027 //IN.duty=0.9;IN.fReverse=1;
00028     if( IN.fDeadtime==1 && IN.mode==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上下アームともに０にする
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 ){  // モードが２のとき
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共に０にする
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;   // チョッピングのオンオフを決定するモードを１増やす
00085     if( IN.fDeadtime==1 && IN.mode==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ともに０にする
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ともに０にする
00098     }else if( IN.mode==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 ){  // モードが３のとき
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ともに０にする
00112         IN.mode = 0;    // チョッピングのオンオフを決定するモードを0にする
00113     }
00114 }
00115 #endif
00116 
00117 void start_pwm(){   // PWMスタートする関数
00118     IN.duty = 0.0;              // デューティーを０にする
00119     pwm_fwdIN = pwm_rvsIN = 0;    // fwdIN, rvsIN上下アームともに０にする
00120     IN.mode = 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;              // デューティーを０にする
00129     pwm_fwdIN = pwm_rvsIN = 0;    // fwdIN, rvsIN上下アームともに０にする
00130     IN.mode = 0;                // チョッピングのオンオフを決定するモードを０にする
00131 
00132     pwm.detach();               // タイムアウト関数をストップ
00133 }