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