9
9
*/
10
10
11
11
Encoder::Encoder (int _encA, int _encB , float _ppr, int _index){
12
-
12
+
13
13
// Encoder measurement structure init
14
14
// hardware pins
15
15
pinA = _encA;
@@ -87,11 +87,11 @@ void Encoder::handleIndex() {
87
87
if (hasIndex ()){
88
88
int I = digitalRead (index_pin);
89
89
if (I && !I_active){
90
- // align encoder on each index
90
+ // align encoder on each index
91
91
if (index_pulse_counter){
92
92
long tmp = pulse_counter;
93
93
// corrent the counter value
94
- pulse_counter = round ((float )pulse_counter/(float )cpr)*cpr;
94
+ pulse_counter = round ((double )pulse_counter/(double )cpr)*cpr;
95
95
// preserve relative speed
96
96
prev_pulse_counter += pulse_counter - tmp;
97
97
} else {
@@ -119,8 +119,8 @@ float Encoder::getVelocity(){
119
119
// sampling time calculation
120
120
float Ts = (timestamp_us - prev_timestamp_us) * 1e-6 ;
121
121
// quick fix for strange cases (micros overflow)
122
- if (Ts <= 0 || Ts > 0.5 ) Ts = 1e-3 ;
123
-
122
+ if (Ts <= 0 || Ts > 0.5 ) Ts = 1e-3 ;
123
+
124
124
// time from last impulse
125
125
float Th = (timestamp_us - pulse_timestamp) * 1e-6 ;
126
126
long dN = pulse_counter - prev_pulse_counter;
@@ -133,7 +133,7 @@ float Encoder::getVelocity(){
133
133
// only increment if some impulses received
134
134
float dt = Ts + prev_Th - Th;
135
135
pulse_per_second = (dN != 0 && dt > Ts/2 ) ? dN / dt : pulse_per_second;
136
-
136
+
137
137
// if more than 0.05 passed in between impulses
138
138
if ( Th > 0.1 ) pulse_per_second = 0 ;
139
139
@@ -176,10 +176,10 @@ int Encoder::hasIndex(){
176
176
}
177
177
178
178
179
- // encoder initialisation of the hardware pins
179
+ // encoder initialisation of the hardware pins
180
180
// and calculation variables
181
181
void Encoder::init (){
182
-
182
+
183
183
// Encoder - check if pullup needed for your encoder
184
184
if (pullup == Pullup::INTERN){
185
185
pinMode (pinA, INPUT_PULLUP);
@@ -190,7 +190,7 @@ void Encoder::init(){
190
190
pinMode (pinB, INPUT);
191
191
if (hasIndex ()) pinMode (index_pin,INPUT);
192
192
}
193
-
193
+
194
194
// counter setup
195
195
pulse_counter = 0 ;
196
196
pulse_timestamp = _micros ();
@@ -222,8 +222,7 @@ void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
222
222
if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, RISING);
223
223
break ;
224
224
}
225
-
225
+
226
226
// if index used initialize the index interrupt
227
227
if (hasIndex () && doIndex != nullptr ) attachInterrupt (digitalPinToInterrupt (index_pin), doIndex, CHANGE);
228
228
}
229
-
0 commit comments