EC.cpp

00001 #include "mbed.h"
00002 #include "EC.h"
00003 
00004 int Ec::defsolution;
00005 double Ec::deftime;
00006 //ピン変化割り込み関数の定義
00007 void Ec::upA()
00008 {
00009     stateA=1;
00010     if(stateB==0&&S==0) {
00011         S=1;
00012     } else if(stateB==1&&S==3) {
00013         S=2;
00014     }
00015 }
00016 void Ec::downA()
00017 {
00018     stateA=0;
00019     if(stateB==1&&S==2) {
00020         S=3;
00021     } else if(stateB==0&&S==1) {
00022         S=0;
00023         count--;
00024     }
00025 }
00026 void Ec::upB()
00027 {
00028     stateB=1;
00029     if(stateA==1&&S==1) {
00030         S=2;
00031     } else if(stateA==0&&S==0) {
00032         S=3;
00033     }
00034 }
00035 void Ec::downB()
00036 {
00037     stateB=0;
00038     if(stateA==0&&S==3) {
00039         count++;
00040         S=0;
00041     } else if(stateA==1&&S==2) {
00042         S=1;
00043     }
00044 }
00045 
00046 void Ec::upZ()
00047 {
00048     if(first==false) {
00049         static int res_count;
00050         rev++;
00051         first=true;
00052         now_time=timer.read();
00053         RPM=60/(now_time-old_time);
00054         if((RPM_old-RPM)>RPM_th) {
00055             if(res_count >= 5) {
00056                 printf("\r\n CAUTION : speed downed drastically\r\n");
00057                 NVIC_SystemReset();
00058                 res_count=0;
00059             } else {
00060                 res_count++;
00061             }
00062         } else {
00063             res_count=0;
00064         }
00065         RPM_old=RPM;
00066         old_time=timer.read();
00067     }
00068 }
00069 
00070 void Ec::downZ()
00071 {
00072     first=false;
00073 }
00074 
00075 
00076 //コンストラクタの定義
00077 //main関数の前に必ず一度宣言する
00078 //第一・第二引数はエンコーダのA・B相のピン名
00079 //第三院数はエンコーダの分解能
00080 
00081 Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB),signalZ_(signalZ)
00082 {
00083 
00084     if((signalA!=NC)&&(signalB!=NC)) {
00085         S=0;
00086         stateA=0;
00087         stateB=0;
00088         count=0;
00089         pre_count=0.0;
00090         timer.start();
00091         signalA_.rise(callback(this,&Ec::upA));
00092         signalA_.fall(callback(this,&Ec::downA));
00093         signalB_.rise(callback(this,&Ec::upB));
00094         signalB_.fall(callback(this,&Ec::downB));
00095     }
00096     if(signalZ!=NC) {
00097         first=false;
00098         rev=0;
00099         old_time=0;
00100         RPM=0;
00101         RPM_old=0;
00102         signalZ_.rise(callback(this,&Ec::upZ));
00103         signalZ_.fall(callback(this,&Ec::downZ));
00104     }
00105     dt=t;
00106     solution=s;
00107     defsolution=s;
00108     RPM_th=250;
00109     count_to_distance_mm_ = 0;
00110     gear_rate_ = 1;
00111     is_diameter_set = 0;
00112 }
00113 
00114 int Ec::getCount()
00115 {
00116     return count;
00117 }
00118 
00119 void Ec::CalOmega()
00120 {
00121     omega=(count-pre_count)*2*M_pi/(solution*dt);
00122     pre_count=count;
00123 }
00124 
00125 double Ec::getOmega()
00126 {
00127     return omega;
00128 }
00129 
00130 double Ec::getPreCount()
00131 {
00132     precount=count+S/4.0;
00133     return precount;
00134 }
00135 /*reset関数の定義*/
00136 /*エンコーダを初期状態に戻すことができる*/
00137 void Ec::reset()
00138 {
00139     S=0;
00140     stateA=0;
00141     stateB=0;
00142     count=0;
00143     pre_count=0.0,omega=0;
00144     rev=0;
00145     now_time=0;
00146     old_time=0;
00147     RPM=0;
00148     RPM_old=0;
00149 }
00150 /*setTime関数の定義*/
00151 /*自分で好きなように角速度計算の間隔を決めることができる（デフォルトは0.05秒）*/
00152 void Ec::setTime(double t)
00153 {
00154     dt=t;
00155 }
00156 
00157 double Ec::getRPM()
00158 {
00159     return RPM;
00160 }
00161 
00162 int Ec::getRev()
00163 {
00164     return rev;
00165 }
00166 
00167 void Ec::changeRPM_th(int th)
00168 {
00169     RPM_th=th;
00170 }
00171 
00172 double Ec::getDistance_mm()
00173 {
00174     if(is_diameter_set == 0) printf("please set diameter\r\n");
00175     distance_mm_= count * count_to_distance_mm_;
00176     return distance_mm_;
00177 }
00178 void Ec::setDiameter_mm (double diameter_mm)
00179 {
00180     diameter_mm_ = diameter_mm;
00181     setCountToDistance_mm();
00182     is_diameter_set = 1;
00183 }
00184 void Ec::setGearRate (double gear_rate){
00185     gear_rate_ = gear_rate;
00186     setCountToDistance_mm();
00187 }
00188 void Ec::setCountToDistance_mm(){
00189     count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能＊2Pi=角度（rad)。角度*直径/2  距離を整理した式
00190 }