DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.
Fork of DCmotor2 by
Hbridge.cpp@13:ba71733c11d7, 2013-03-01 (annotated)
- Committer:
- kosaka
- Date:
- Fri Mar 01 02:10:59 2013 +0000
- Revision:
- 13:ba71733c11d7
- Parent:
- 12:459af534d1ee
130214
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosaka | 13:ba71733c11d7 | 1 | // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ(Hブリッジ)をPWM駆動する。 |
kosaka | 13:ba71733c11d7 | 2 | |
kosaka | 12:459af534d1ee | 3 | #include "mbed.h" |
kosaka | 12:459af534d1ee | 4 | #include "controller.h" |
kosaka | 12:459af534d1ee | 5 | #include "Hbridge.h" |
kosaka | 12:459af534d1ee | 6 | |
kosaka | 13:ba71733c11d7 | 7 | #define DEADTIME_US (unsigned long)(DEADTIME*1000000) // [us], デッドタイム deadtime to be set between plus volt. to/from minus |
kosaka | 12:459af534d1ee | 8 | |
kosaka | 13:ba71733c11d7 | 9 | Timeout pwm; // タイムアウト関数の宣言(ある時間経過後に関数コールする) |
kosaka | 12:459af534d1ee | 10 | |
kosaka | 13:ba71733c11d7 | 11 | DigitalOut pwm_fwdIN = fwdIN_PORT; // デジタル信号を出力するポートを設定 |
kosaka | 13:ba71733c11d7 | 12 | DigitalOut pwm_rvsIN = rvsIN_PORT; // デジタル信号を出力するポートを設定 |
kosaka | 12:459af534d1ee | 13 | |
kosaka | 13:ba71733c11d7 | 14 | pwm_parameters IN; // フルブリッジのパラメータ宣言 |
kosaka | 13:ba71733c11d7 | 15 | |
kosaka | 13:ba71733c11d7 | 16 | 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] |
kosaka | 13:ba71733c11d7 | 17 | 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] |
kosaka | 12:459af534d1ee | 18 | |
kosaka | 12:459af534d1ee | 19 | DigitalOut debug_p24(p24); // p17 for debug |
kosaka | 12:459af534d1ee | 20 | //DigitalOut Led3(LED3); |
kosaka | 12:459af534d1ee | 21 | |
kosaka | 13:ba71733c11d7 | 22 | #if PWM_WAVEFORM==0 // PWM変調方式がノコギリ波比較のとき |
kosaka | 12:459af534d1ee | 23 | #if 1 |
kosaka | 13:ba71733c11d7 | 24 | void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] |
kosaka | 12:459af534d1ee | 25 | //debug_p24=1; |
kosaka | 13:ba71733c11d7 | 26 | IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす |
kosaka | 13:ba71733c11d7 | 27 | //IN.duty=0.9;IN.fReverse=1; |
kosaka | 13:ba71733c11d7 | 28 | if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき |
kosaka | 13:ba71733c11d7 | 29 | pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 30 | pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする |
kosaka | 13:ba71733c11d7 | 31 | IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする |
kosaka | 13:ba71733c11d7 | 32 | IN.mode = 0; // モードを0にする |
kosaka | 13:ba71733c11d7 | 33 | }else if( IN.mode==1 ){ // モードが1のとき |
kosaka | 13:ba71733c11d7 | 34 | if( IN.fReverse==0 ){ // モータの回転が順方向のとき |
kosaka | 13:ba71733c11d7 | 35 | pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする |
kosaka | 13:ba71733c11d7 | 36 | }else{ // モータの回転が逆方向のとき |
kosaka | 13:ba71733c11d7 | 37 | pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする |
kosaka | 12:459af534d1ee | 38 | } |
kosaka | 13:ba71733c11d7 | 39 | IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 ON time of pwm |
kosaka | 13:ba71733c11d7 | 40 | if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする |
kosaka | 13:ba71733c11d7 | 41 | pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 42 | IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // rvsINをオンする時間幅を計算 OFF time of pwm |
kosaka | 13:ba71733c11d7 | 43 | if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする |
kosaka | 13:ba71733c11d7 | 44 | }else{// if( IN.mode==2 ){ // モードが2のとき |
kosaka | 13:ba71733c11d7 | 45 | pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 46 | #ifndef SIMULATION |
kosaka | 13:ba71733c11d7 | 47 | // モータ電流の検出 |
kosaka | 13:ba71733c11d7 | 48 | p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; |
kosaka | 13:ba71733c11d7 | 49 | #endif |
kosaka | 13:ba71733c11d7 | 50 | pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsIN共に0にする |
kosaka | 13:ba71733c11d7 | 51 | IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする |
kosaka | 12:459af534d1ee | 52 | } |
kosaka | 12:459af534d1ee | 53 | //debug_p24=0; |
kosaka | 12:459af534d1ee | 54 | } |
kosaka | 12:459af534d1ee | 55 | #else |
kosaka | 13:ba71733c11d7 | 56 | void pwm_out() { // 補PWM pwm out using timer |
kosaka | 12:459af534d1ee | 57 | IN.mode += 1; |
kosaka | 12:459af534d1ee | 58 | if( IN.mode==1 ){ |
kosaka | 13:ba71733c11d7 | 59 | pwm_fwdIN = 1; |
kosaka | 13:ba71733c11d7 | 60 | pwm_rvsIN = 0; |
kosaka | 13:ba71733c11d7 | 61 | IN.fwdIN_us = IN.duty*1000000/PWM_FREQ - DEADTIME_US; // ON time of UfwdIN |
kosaka | 13:ba71733c11d7 | 62 | if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} |
kosaka | 13:ba71733c11d7 | 63 | pwm.attach_us(&pwm_out, IN.fwdIN_us); // setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 64 | IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us - 2*DEADTIME_US; // ON time of UrvsIN |
kosaka | 13:ba71733c11d7 | 65 | if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} |
kosaka | 12:459af534d1ee | 66 | }else if( IN.mode==2 ){ |
kosaka | 12:459af534d1ee | 67 | pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 68 | pwm_fwdIN = 0; |
kosaka | 13:ba71733c11d7 | 69 | pwm_rvsIN = 0; |
kosaka | 12:459af534d1ee | 70 | }else if( IN.mode==3 ){ |
kosaka | 13:ba71733c11d7 | 71 | pwm.attach_us(&pwm_out, IN.rvsIN_us); // setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 72 | pwm_fwdIN = 0; |
kosaka | 13:ba71733c11d7 | 73 | pwm_rvsIN = 1; |
kosaka | 12:459af534d1ee | 74 | }else{// if( u.mode==4 ){ |
kosaka | 12:459af534d1ee | 75 | pwm.attach_us(&pwm_out, DEADTIME_US); // setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 76 | pwm_fwdIN = 0; |
kosaka | 13:ba71733c11d7 | 77 | pwm_rvsIN = 0; |
kosaka | 12:459af534d1ee | 78 | IN.mode = 0; |
kosaka | 12:459af534d1ee | 79 | } |
kosaka | 12:459af534d1ee | 80 | } |
kosaka | 12:459af534d1ee | 81 | #endif |
kosaka | 13:ba71733c11d7 | 82 | #elif PWM_WAVEFORM==1 // PWM変調方式が三角波比較のとき |
kosaka | 13:ba71733c11d7 | 83 | void pwm_out() { // タイムアウト関数でPWMを発生する関数: [fwdIN,rvsIN]=[PWM,0] or [0,PWM] or [0,0] |
kosaka | 13:ba71733c11d7 | 84 | IN.mode += 1; // チョッピングのオンオフを決定するモードを1増やす |
kosaka | 13:ba71733c11d7 | 85 | if( IN.fDeadtime==1 && IN.mode==1){ // デッドタイムフラグがオンで、モードが1のとき |
kosaka | 13:ba71733c11d7 | 86 | pwm.attach_us(&pwm_out, DEADTIME_US); // fwdIN, rvsIN上下アームをデッドタイム時間オフしてからタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 87 | pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする |
kosaka | 13:ba71733c11d7 | 88 | IN.fDeadtime = 0; // デッドタイムフラグをゼロクリアする |
kosaka | 13:ba71733c11d7 | 89 | IN.mode = 0; // モードを0にする |
kosaka | 13:ba71733c11d7 | 90 | }else if( IN.mode==1 ){ // モードが1のとき |
kosaka | 13:ba71733c11d7 | 91 | IN.fwdIN_us = IN.duty*1000000/PWM_FREQ; // fwdINをオンする時間幅を計算 // ON time of UfwdIN |
kosaka | 13:ba71733c11d7 | 92 | IN.rvsIN_us = 1000000/PWM_FREQ -IN.fwdIN_us; // fwdIN, rvsINをオンする時間幅を計算 // ON time of UrvsIN |
kosaka | 13:ba71733c11d7 | 93 | IN.rvsIN_us /= 2; // その時間幅を2で割る |
kosaka | 13:ba71733c11d7 | 94 | if( IN.rvsIN_us < TMIN ){ IN.rvsIN_us=TMIN;} // 時間幅が負のときはTMINにする |
kosaka | 13:ba71733c11d7 | 95 | pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINをオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 96 | if( IN.fwdIN_us < TMIN ){ IN.fwdIN_us=TMIN;} // 時間幅が負のときはTMINにする |
kosaka | 13:ba71733c11d7 | 97 | pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする |
kosaka | 13:ba71733c11d7 | 98 | }else if( IN.mode==2 ){ // モードが2のとき |
kosaka | 13:ba71733c11d7 | 99 | pwm.attach_us(&pwm_out, IN.fwdIN_us); // fwdINをオンする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 100 | if( IN.fReverse==0 ){ // モータの回転が順方向のとき |
kosaka | 13:ba71733c11d7 | 101 | pwm_fwdIN = 1; pwm_rvsIN = 0; // fwdINのみオンにする |
kosaka | 13:ba71733c11d7 | 102 | }else{ // モータの回転が逆方向のとき |
kosaka | 13:ba71733c11d7 | 103 | pwm_fwdIN = 0; pwm_rvsIN = 1; // rvsINのみオンにする |
kosaka | 12:459af534d1ee | 104 | } |
kosaka | 13:ba71733c11d7 | 105 | }else{// if( IN.mode==3 ){ // モードが3のとき |
kosaka | 13:ba71733c11d7 | 106 | pwm.attach_us(&pwm_out, IN.rvsIN_us); // fwdIN, rvsINを共にオフする時間幅経過後にタイムアウトでこの関数自身をコール setup pwmU to call pwm_out after t [us] |
kosaka | 13:ba71733c11d7 | 107 | #ifndef SIMULATION |
kosaka | 13:ba71733c11d7 | 108 | // モータ電流の検出 |
kosaka | 13:ba71733c11d7 | 109 | p.i = (VshuntR_plus - VshuntR_minus) /R_SHUNT; // シャント抵抗の両端の電圧を見てモータ電流を検出 get i [A] from shunt resistance; |
kosaka | 13:ba71733c11d7 | 110 | #endif |
kosaka | 13:ba71733c11d7 | 111 | pwm_fwdIN = 0; pwm_rvsIN = 0; // fwdIN, rvsINともに0にする |
kosaka | 13:ba71733c11d7 | 112 | IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする |
kosaka | 12:459af534d1ee | 113 | } |
kosaka | 12:459af534d1ee | 114 | } |
kosaka | 12:459af534d1ee | 115 | #endif |
kosaka | 12:459af534d1ee | 116 | |
kosaka | 13:ba71733c11d7 | 117 | void start_pwm(){ // PWMスタートする関数 |
kosaka | 13:ba71733c11d7 | 118 | IN.duty = 0.0; // デューティーを0にする |
kosaka | 13:ba71733c11d7 | 119 | pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする |
kosaka | 13:ba71733c11d7 | 120 | IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする |
kosaka | 13:ba71733c11d7 | 121 | IN.fDeadtime = 1; // 正負切替時にデッドタイムを要求するフラグをオフにする |
kosaka | 13:ba71733c11d7 | 122 | IN.fReverse = 0; // モータ逆回転フラグをオフにする:回転方向が順方向のとき0、逆方向のとき1。[0]が現在の値、[1]はその前の値 |
kosaka | 12:459af534d1ee | 123 | |
kosaka | 13:ba71733c11d7 | 124 | pwm_out(); // タイムアウト関数でPWMを発生する関数をスタート |
kosaka | 12:459af534d1ee | 125 | } |
kosaka | 12:459af534d1ee | 126 | |
kosaka | 13:ba71733c11d7 | 127 | void stop_pwm(){ // PWMストップする関数 |
kosaka | 13:ba71733c11d7 | 128 | IN.duty = 0.0; // デューティーを0にする |
kosaka | 13:ba71733c11d7 | 129 | pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに0にする |
kosaka | 13:ba71733c11d7 | 130 | IN.mode = 0; // チョッピングのオンオフを決定するモードを0にする |
kosaka | 13:ba71733c11d7 | 131 | |
kosaka | 13:ba71733c11d7 | 132 | pwm.detach(); // タイムアウト関数をストップ |
kosaka | 12:459af534d1ee | 133 | } |