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