yuto kawamura
/
MR2_2n_1_saku
sls
ec_temp/EC.cpp@4:96f38805f055, 2019-04-29 (annotated)
- Committer:
- yuto17320508
- Date:
- Mon Apr 29 07:02:33 2019 +0000
- Revision:
- 4:96f38805f055
- Parent:
- 0:c1476d342c13
ss
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eri | 0:c1476d342c13 | 1 | #include "mbed.h" |
eri | 0:c1476d342c13 | 2 | #include "EC.h" |
eri | 0:c1476d342c13 | 3 | |
eri | 0:c1476d342c13 | 4 | int Ec::defsolution; |
eri | 0:c1476d342c13 | 5 | double Ec::deftime; |
eri | 0:c1476d342c13 | 6 | //ピン変化割り込み関数の定義 |
eri | 0:c1476d342c13 | 7 | void Ec::upA() |
eri | 0:c1476d342c13 | 8 | { |
eri | 0:c1476d342c13 | 9 | /*stateA=1; |
eri | 0:c1476d342c13 | 10 | if(stateB==0&&S==0) { |
eri | 0:c1476d342c13 | 11 | S=1; |
eri | 0:c1476d342c13 | 12 | } else if(stateB==1&&S==3) { |
eri | 0:c1476d342c13 | 13 | S=2; |
eri | 0:c1476d342c13 | 14 | }*/ |
eri | 0:c1476d342c13 | 15 | if(signalB_.read()){ |
eri | 0:c1476d342c13 | 16 | count--; |
eri | 0:c1476d342c13 | 17 | }else{ |
eri | 0:c1476d342c13 | 18 | count++; |
eri | 0:c1476d342c13 | 19 | } |
eri | 0:c1476d342c13 | 20 | } |
eri | 0:c1476d342c13 | 21 | void Ec::downA() |
eri | 0:c1476d342c13 | 22 | { |
eri | 0:c1476d342c13 | 23 | /*stateA=0; |
eri | 0:c1476d342c13 | 24 | if(stateB==1&&S==2) { |
eri | 0:c1476d342c13 | 25 | S=3; |
eri | 0:c1476d342c13 | 26 | } else if(stateB==0&&S==1) { |
eri | 0:c1476d342c13 | 27 | S=0; |
eri | 0:c1476d342c13 | 28 | count--; |
eri | 0:c1476d342c13 | 29 | }*/ |
eri | 0:c1476d342c13 | 30 | if(signalB_.read()){ |
eri | 0:c1476d342c13 | 31 | count++; |
eri | 0:c1476d342c13 | 32 | }else{ |
eri | 0:c1476d342c13 | 33 | count--; |
eri | 0:c1476d342c13 | 34 | } |
eri | 0:c1476d342c13 | 35 | } |
eri | 0:c1476d342c13 | 36 | void Ec::upB() |
eri | 0:c1476d342c13 | 37 | { |
eri | 0:c1476d342c13 | 38 | stateB=1; |
eri | 0:c1476d342c13 | 39 | if(stateA==1&&S==1) { |
eri | 0:c1476d342c13 | 40 | S=2; |
eri | 0:c1476d342c13 | 41 | } else if(stateA==0&&S==0) { |
eri | 0:c1476d342c13 | 42 | S=3; |
eri | 0:c1476d342c13 | 43 | } |
eri | 0:c1476d342c13 | 44 | } |
eri | 0:c1476d342c13 | 45 | void Ec::downB() |
eri | 0:c1476d342c13 | 46 | { |
eri | 0:c1476d342c13 | 47 | stateB=0; |
eri | 0:c1476d342c13 | 48 | if(stateA==0&&S==3) { |
eri | 0:c1476d342c13 | 49 | count++; |
eri | 0:c1476d342c13 | 50 | S=0; |
eri | 0:c1476d342c13 | 51 | } else if(stateA==1&&S==2) { |
eri | 0:c1476d342c13 | 52 | S=1; |
eri | 0:c1476d342c13 | 53 | } |
eri | 0:c1476d342c13 | 54 | } |
eri | 0:c1476d342c13 | 55 | |
eri | 0:c1476d342c13 | 56 | /*void Ec::upZ() |
eri | 0:c1476d342c13 | 57 | { |
eri | 0:c1476d342c13 | 58 | if(first==false) { |
eri | 0:c1476d342c13 | 59 | static int res_count; |
eri | 0:c1476d342c13 | 60 | rev++; |
eri | 0:c1476d342c13 | 61 | first=true; |
eri | 0:c1476d342c13 | 62 | //now_time=timer.read(); |
eri | 0:c1476d342c13 | 63 | RPM=60/(now_time-old_time); |
eri | 0:c1476d342c13 | 64 | if((RPM_old-RPM)>RPM_th) { |
eri | 0:c1476d342c13 | 65 | if(res_count >= 5) { |
eri | 0:c1476d342c13 | 66 | printf("\r\n CAUTION : speed downed drastically\r\n"); |
eri | 0:c1476d342c13 | 67 | NVIC_SystemReset(); |
eri | 0:c1476d342c13 | 68 | res_count=0; |
eri | 0:c1476d342c13 | 69 | } else { |
eri | 0:c1476d342c13 | 70 | res_count++; |
eri | 0:c1476d342c13 | 71 | } |
eri | 0:c1476d342c13 | 72 | } else { |
eri | 0:c1476d342c13 | 73 | res_count=0; |
eri | 0:c1476d342c13 | 74 | } |
eri | 0:c1476d342c13 | 75 | RPM_old=RPM; |
eri | 0:c1476d342c13 | 76 | //old_time=timer.read(); |
eri | 0:c1476d342c13 | 77 | } |
eri | 0:c1476d342c13 | 78 | } |
eri | 0:c1476d342c13 | 79 | |
eri | 0:c1476d342c13 | 80 | void Ec::downZ() |
eri | 0:c1476d342c13 | 81 | { |
eri | 0:c1476d342c13 | 82 | first=false; |
eri | 0:c1476d342c13 | 83 | }*/ |
eri | 0:c1476d342c13 | 84 | |
eri | 0:c1476d342c13 | 85 | |
eri | 0:c1476d342c13 | 86 | //コンストラクタの定義 |
eri | 0:c1476d342c13 | 87 | //main関数の前に必ず一度宣言する |
eri | 0:c1476d342c13 | 88 | //第一・第二引数はエンコーダのA・B相のピン名 |
eri | 0:c1476d342c13 | 89 | //第三院数はエンコーダの分解能 |
eri | 0:c1476d342c13 | 90 | |
eri | 0:c1476d342c13 | 91 | Ec::Ec(PinName signalA , PinName signalB , PinName signalZ , int s=defsolution , double t=deftime) : signalA_(signalA),signalB_(signalB)//,signalZ_(signalZ) |
eri | 0:c1476d342c13 | 92 | { |
eri | 0:c1476d342c13 | 93 | |
eri | 0:c1476d342c13 | 94 | if((signalA!=NC)&&(signalB!=NC)) { |
eri | 0:c1476d342c13 | 95 | S=0; |
eri | 0:c1476d342c13 | 96 | stateA=0; |
eri | 0:c1476d342c13 | 97 | stateB=0; |
eri | 0:c1476d342c13 | 98 | count=0; |
eri | 0:c1476d342c13 | 99 | pre_count=0.0; |
eri | 0:c1476d342c13 | 100 | //timer.start(); |
eri | 0:c1476d342c13 | 101 | signalA_.rise(callback(this,&Ec::upA)); |
eri | 0:c1476d342c13 | 102 | signalA_.fall(callback(this,&Ec::downA)); |
eri | 0:c1476d342c13 | 103 | //signalB_.rise(callback(this,&Ec::upB)); |
eri | 0:c1476d342c13 | 104 | //signalB_.fall(callback(this,&Ec::downB)); |
eri | 0:c1476d342c13 | 105 | } |
eri | 0:c1476d342c13 | 106 | /*if(signalZ!=NC) { |
eri | 0:c1476d342c13 | 107 | first=false; |
eri | 0:c1476d342c13 | 108 | rev=0; |
eri | 0:c1476d342c13 | 109 | old_time=0; |
eri | 0:c1476d342c13 | 110 | RPM=0; |
eri | 0:c1476d342c13 | 111 | RPM_old=0; |
eri | 0:c1476d342c13 | 112 | signalZ_.rise(callback(this,&Ec::upZ)); |
eri | 0:c1476d342c13 | 113 | signalZ_.fall(callback(this,&Ec::downZ)); |
eri | 0:c1476d342c13 | 114 | }*/ |
eri | 0:c1476d342c13 | 115 | dt=t; |
eri | 0:c1476d342c13 | 116 | solution=s; |
eri | 0:c1476d342c13 | 117 | defsolution=s; |
eri | 0:c1476d342c13 | 118 | RPM_th=250; |
eri | 0:c1476d342c13 | 119 | count_to_distance_mm_ = 0; |
eri | 0:c1476d342c13 | 120 | gear_rate_ = 1; |
eri | 0:c1476d342c13 | 121 | is_diameter_set = 0; |
eri | 0:c1476d342c13 | 122 | } |
eri | 0:c1476d342c13 | 123 | |
eri | 0:c1476d342c13 | 124 | int Ec::getCount() |
eri | 0:c1476d342c13 | 125 | { |
eri | 0:c1476d342c13 | 126 | return count; |
eri | 0:c1476d342c13 | 127 | } |
eri | 0:c1476d342c13 | 128 | |
eri | 0:c1476d342c13 | 129 | void Ec::CalOmega() |
eri | 0:c1476d342c13 | 130 | { |
eri | 0:c1476d342c13 | 131 | omega=(count-pre_count)*2*M_pi/(solution*dt); |
eri | 0:c1476d342c13 | 132 | pre_count=count; |
eri | 0:c1476d342c13 | 133 | } |
eri | 0:c1476d342c13 | 134 | |
eri | 0:c1476d342c13 | 135 | double Ec::getOmega() |
eri | 0:c1476d342c13 | 136 | { |
eri | 0:c1476d342c13 | 137 | return omega; |
eri | 0:c1476d342c13 | 138 | } |
eri | 0:c1476d342c13 | 139 | |
eri | 0:c1476d342c13 | 140 | double Ec::getPreCount() |
eri | 0:c1476d342c13 | 141 | { |
eri | 0:c1476d342c13 | 142 | precount=count+S/4.0; |
eri | 0:c1476d342c13 | 143 | return precount; |
eri | 0:c1476d342c13 | 144 | } |
eri | 0:c1476d342c13 | 145 | /*reset関数の定義*/ |
eri | 0:c1476d342c13 | 146 | /*エンコーダを初期状態に戻すことができる*/ |
eri | 0:c1476d342c13 | 147 | void Ec::reset() |
eri | 0:c1476d342c13 | 148 | { |
eri | 0:c1476d342c13 | 149 | S=0; |
eri | 0:c1476d342c13 | 150 | stateA=0; |
eri | 0:c1476d342c13 | 151 | stateB=0; |
eri | 0:c1476d342c13 | 152 | count=0; |
eri | 0:c1476d342c13 | 153 | pre_count=0.0,omega=0; |
eri | 0:c1476d342c13 | 154 | rev=0; |
eri | 0:c1476d342c13 | 155 | now_time=0; |
eri | 0:c1476d342c13 | 156 | old_time=0; |
eri | 0:c1476d342c13 | 157 | RPM=0; |
eri | 0:c1476d342c13 | 158 | RPM_old=0; |
eri | 0:c1476d342c13 | 159 | } |
eri | 0:c1476d342c13 | 160 | /*setTime関数の定義*/ |
eri | 0:c1476d342c13 | 161 | /*自分で好きなように角速度計算の間隔を決めることができる(デフォルトは0.05秒)*/ |
eri | 0:c1476d342c13 | 162 | void Ec::setTime(double t) |
eri | 0:c1476d342c13 | 163 | { |
eri | 0:c1476d342c13 | 164 | dt=t; |
eri | 0:c1476d342c13 | 165 | } |
eri | 0:c1476d342c13 | 166 | |
eri | 0:c1476d342c13 | 167 | double Ec::getRPM() |
eri | 0:c1476d342c13 | 168 | { |
eri | 0:c1476d342c13 | 169 | return RPM; |
eri | 0:c1476d342c13 | 170 | } |
eri | 0:c1476d342c13 | 171 | |
eri | 0:c1476d342c13 | 172 | int Ec::getRev() |
eri | 0:c1476d342c13 | 173 | { |
eri | 0:c1476d342c13 | 174 | return rev; |
eri | 0:c1476d342c13 | 175 | } |
eri | 0:c1476d342c13 | 176 | |
eri | 0:c1476d342c13 | 177 | void Ec::changeRPM_th(int th) |
eri | 0:c1476d342c13 | 178 | { |
eri | 0:c1476d342c13 | 179 | RPM_th=th; |
eri | 0:c1476d342c13 | 180 | } |
eri | 0:c1476d342c13 | 181 | |
eri | 0:c1476d342c13 | 182 | double Ec::getDistance_mm() |
eri | 0:c1476d342c13 | 183 | { |
eri | 0:c1476d342c13 | 184 | if(is_diameter_set == 0) printf("please set diameter\r\n"); |
eri | 0:c1476d342c13 | 185 | distance_mm_= count * count_to_distance_mm_; |
eri | 0:c1476d342c13 | 186 | return distance_mm_; |
eri | 0:c1476d342c13 | 187 | } |
eri | 0:c1476d342c13 | 188 | void Ec::setDiameter_mm(double diameter_mm) |
eri | 0:c1476d342c13 | 189 | { |
eri | 0:c1476d342c13 | 190 | diameter_mm_ = diameter_mm; |
eri | 0:c1476d342c13 | 191 | setCountToDistance_mm(); |
eri | 0:c1476d342c13 | 192 | is_diameter_set = 1; |
eri | 0:c1476d342c13 | 193 | } |
eri | 0:c1476d342c13 | 194 | void Ec::setGearRate(double gear_rate){ |
eri | 0:c1476d342c13 | 195 | gear_rate_ = gear_rate; |
eri | 0:c1476d342c13 | 196 | setCountToDistance_mm(); |
eri | 0:c1476d342c13 | 197 | } |
eri | 0:c1476d342c13 | 198 | void Ec::setCountToDistance_mm(){ |
eri | 0:c1476d342c13 | 199 | count_to_distance_mm_ = 1 / (double)solution * M_pi * diameter_mm_ * gear_rate_;//カウント分解能*2Pi=角度(rad)。角度*直径/2 距離を整理した式 |
eri | 0:c1476d342c13 | 200 | } |