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

Hbridge.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 // Hbridge.cpp: モータ駆動用ドライバのフルブリッジ（Ｈブリッジ）をＰＷＭ駆動する。
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; // チョッピングのオンオフを決定するモードを１増やす
kosaka 13:ba71733c11d7 27 //IN.duty=0.9;IN.fReverse=1;
kosaka 13:ba71733c11d7 28 if( IN.fDeadtime==1 && IN.mode==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上下アームともに０にする
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 ){ // モードが２のとき
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共に０にする
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; // チョッピングのオンオフを決定するモードを１増やす
kosaka 13:ba71733c11d7 85 if( IN.fDeadtime==1 && IN.mode==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ともに０にする
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ともに０にする
kosaka 13:ba71733c11d7 98 }else if( IN.mode==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 ){ // モードが３のとき
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ともに０にする
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; // デューティーを０にする
kosaka 13:ba71733c11d7 119 pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに０にする
kosaka 13:ba71733c11d7 120 IN.mode = 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; // デューティーを０にする
kosaka 13:ba71733c11d7 129 pwm_fwdIN = pwm_rvsIN = 0; // fwdIN, rvsIN上下アームともに０にする
kosaka 13:ba71733c11d7 130 IN.mode = 0; // チョッピングのオンオフを決定するモードを０にする
kosaka 13:ba71733c11d7 131
kosaka 13:ba71733c11d7 132 pwm.detach(); // タイムアウト関数をストップ
kosaka 12:459af534d1ee 133 }