改良版(仮)
Dependents: harurobo1006 harurobo_1026
Fork of EC by
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 }
Generated on Thu Aug 4 2022 11:33:19 by 1.7.2