SpeedController.h

00001 #ifndef _INCLUDE_SPEEDCONTROLLER_H_
00002 #define _INCLUDE_SPEEDCONTROLLER_H_
00003 
00004 /**
00005 *   @brief モーターの速度制御用プログラム
00006 *   @details モーターのdutyと角速度がほぼ比例することを利用して出力を決定し、PID制御で補正する
00007 **/
00008 class SpeedControl
00009 {
00010 private:
00011     double Kp_,Kd_;
00012     double pre_diff;
00013     double ptsc_;
00014     double Cf_;
00015     double Cb_;
00016     double Df_;
00017     double Db_;
00018     double initial_Df_;
00019     double initial_Db_;
00020     double duty_limit_;
00021     Ec *ec_;
00022 
00023 public:
00024     PwmOut pwm_F_;
00025     PwmOut pwm_B_;
00026 
00027     /** constractorの定義
00028     *   @param pwm_F モーターを正転させるpwmピン名
00029     *   @param pwm_B モーターを後転させるpwmピン名
00030     *   @param us モーターのpwm周期[us]
00031     *   @param ec Encoder classのポインタ
00032     *   モーターを正転させるとエンコーダのcountが正のほうに加算されるようにエンコーダとモーターを設置する
00033     */
00034     SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec);
00035 
00036 
00037 
00038     /**モーターのpwm周期の設定用関数
00039     *   @param s モーターのpwm周期[s]
00040     */
00041     void period(double s);
00042     /**モーターのpwm周期の設定用関数
00043     *   @param ms モーターのpwm周期[ms]
00044     */
00045     void period_ms(int ms);
00046     /**モーターのpwm周期の設定用関数
00047     *   @param us モーターのpwm周期[us]
00048     *
00049     *   @senction SAMPLE
00050     *   @code
00051     *   Ec4multi EC(p11,p12,500);
00052     *   SpeedControl motor(p21,p22,50,EC);
00053     *   int main(){
00054     *       motor.period_us(50);
00055     *       while(1){
00056     *   @endcode
00057     */
00058     void period_us(int us);
00059     /** PIDパラメータ設定関数
00060     *
00061     *  引数はそれぞれp,dパラメータ
00062     *
00063     *  パラメーター値は実験的に頑張って求めるしかない
00064     *  @param kp p制御パラメータ
00065     *  @param kd d制御パラメータ
00066     *
00067     *  @senction SAMPLE
00068     *  @code
00069     *  Ec4multi EC(p11,p12,500);
00070     *  SpeedControl motor(p21,p22,50,EC);
00071     *  int main(){
00072     *      motor.setPDparam(0.1,0.001);
00073     *      while(1){
00074     *  @endcode
00075     */
00076     void setPDparam(double kp,double kd);
00077 
00078     /** 角速度・duty比の関係式の設定関数
00079     *  横軸を角速度[rad/s], 縦軸をduty比としたときの傾きCと切片Dを正転と逆転について設定する
00080     *  @param cf 角速度[rad/s]とduty比の関係式の傾き(正転時)
00081     *  @param df 角速度[rad/s]とduty比の関係式の切片(正転時)
00082     *  @param cb 角速度[rad/s]とduty比の関係式の傾き(逆転時)
00083     *  @param db 角速度[rad/s]とduty比の関係式の切片(逆転時)
00084     *
00085     *  @senction SAMPLE
00086     *  @code
00087     *  Ec4multi EC(p11,p12,500);
00088     *  SpeedControl motor(p21,p22,50,EC);
00089     *  int main(){
00090     *      motor.setEquation(0.02,0.0001,-0.02,0.0001);
00091     *      while(1){
00092     *  @endcode
00093     */
00094     void setEquation(double cf,double df,double cb,double db);
00095     /** 出力duty比の上限を決める関数
00096     *   0 ~ 0.95の範囲内で設定する
00097     *   @param duty_limit 出力duty比の上限
00098     *
00099     *  @senction SAMPLE
00100     *  @code
00101     *  Ec4multi EC(p11,p12,500);
00102     *  SpeedControl motor(p21,p22,50,EC);
00103     *  int main(){
00104     *      motor.setDutyLimit(0.5);    //duty比の上限を0.5に設定
00105     *      while(1){
00106     *  @endcode
00107     */
00108     void setDutyLimit(double duty_limit);
00109     /** 速度制御関数、引数は目標速度
00110     *
00111     *   この目標角加速度になるようにモーターを回転させることができる
00112     *   負の数を代入すれば逆回転することができる
00113     *
00114     *   @param target_omega 目標角速度
00115     *
00116     *   @section SAMPLE
00117     *   @code
00118     *   //プログラム例
00119     *   #include "mbed.h"
00120     *   #include "EC.h" //Encoderライブラリをインクルード
00121     *   #include "SpeedController.h"    //SpeedControlライブラリをインクルード
00122     *   #define RESOLUTION 500
00123     *   Ec4multi EC(p11,p12,RESOLUTION);
00124     *   SpeedControl motor(p21,p22,50,EC);
00125     *   int main(){
00126     *       motor.setEquation(0.0062,0.0045,-0.0061,0.0091);    //求めたC,Dの値を設定
00127     *       motor.setPDparam(0.1,0.0);  //PIDの係数を設定
00128     *       int kai=0;
00129     *       for(int i=0;i<100000000;i++){
00130     *           motor.Sc(10);   //10[rad/s]でモーターを回転
00131     *           kai++;
00132     *           if(kai>1000){
00133     *               printf("omega=%f\r\n",EC.getOmega());
00134     *               kai=0;
00135     *           }
00136     *       }
00137     *       motor.stop()
00138     *       while(1);
00139     *   }
00140     *   @endcode
00141     *
00142     *   @section CAUTION（printfの負荷について）
00143     *   printfはプログラム的に大きな負担がかかり、かなり時間がかかってしまうものである。
00144     *
00145     *   なのでループごとにいちいちprintfをさせると、速度の収束にかなり時間がかかってしまう。
00146     *
00147     *   （どんなプログラムにも言えるが、）そのためこのようなprintfの頻度を少なくさせるような工夫が必要になる。
00148     */
00149     void Sc(double target_omega);
00150     /** モーター停止用関数
00151     *   dutyを0にする
00152     */
00153     void stop();
00154     /** duty(-1 ~ 1)比でモーターを回転させる関数
00155     *   負の値を入れたら逆回転する
00156     *   @param duty 出力duty比
00157     *   @section SAMPLE
00158     *   @code
00159     *   #include "mbed.h"
00160     *   #include "EC.h" //Encoderライブラリをインクルード
00161     *   #include "SpeedController.h"    //SpeedControlライブラリをインクルード
00162     *   #define RESOLUTION 500
00163     *   Ec4multi EC(p11,p12,RESOLUTION);
00164     *   SpeedControl motor(p21,p22,50,EC);
00165     *   int main(){
00166     *       while(1){
00167     *           motor.turn(0.5);    //duty0.5で正転
00168     *           wait(1);
00169     *           motor.turn(0);      //停止
00170     *           wait(1);
00171     *           motor.turn(-0.5);   //duty0.5で逆転
00172     *           wait(1);
00173     *           motor.turn(0);      //停止
00174     *           wait(1);
00175     *       }
00176     *   }
00177     *   @endcode
00178     */
00179     void turn(double duty);
00180     ///Encoderや速度制御の値をresetする関数
00181     void reset();
00182 };
00183 
00184 #endif