99*/
1010
1111Encoder::Encoder (int _encA, int _encB , float _ppr, int _index){
12-
12+
1313 // Encoder measurement structure init
1414 // hardware pins
1515 pinA = _encA;
@@ -87,11 +87,11 @@ void Encoder::handleIndex() {
8787 if (hasIndex ()){
8888 int I = digitalRead (index_pin);
8989 if (I && !I_active){
90- // align encoder on each index
90+ // align encoder on each index
9191 if (index_pulse_counter){
9292 long tmp = pulse_counter;
9393 // corrent the counter value
94- pulse_counter = round ((float )pulse_counter/(float )cpr)*cpr;
94+ pulse_counter = round ((double )pulse_counter/(double )cpr)*cpr;
9595 // preserve relative speed
9696 prev_pulse_counter += pulse_counter - tmp;
9797 } else {
@@ -119,8 +119,8 @@ float Encoder::getVelocity(){
119119 // sampling time calculation
120120 float Ts = (timestamp_us - prev_timestamp_us) * 1e-6 ;
121121 // 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+
124124 // time from last impulse
125125 float Th = (timestamp_us - pulse_timestamp) * 1e-6 ;
126126 long dN = pulse_counter - prev_pulse_counter;
@@ -133,7 +133,7 @@ float Encoder::getVelocity(){
133133 // only increment if some impulses received
134134 float dt = Ts + prev_Th - Th;
135135 pulse_per_second = (dN != 0 && dt > Ts/2 ) ? dN / dt : pulse_per_second;
136-
136+
137137 // if more than 0.05 passed in between impulses
138138 if ( Th > 0.1 ) pulse_per_second = 0 ;
139139
@@ -176,10 +176,10 @@ int Encoder::hasIndex(){
176176}
177177
178178
179- // encoder initialisation of the hardware pins
179+ // encoder initialisation of the hardware pins
180180// and calculation variables
181181void Encoder::init (){
182-
182+
183183 // Encoder - check if pullup needed for your encoder
184184 if (pullup == Pullup::INTERN){
185185 pinMode (pinA, INPUT_PULLUP);
@@ -190,7 +190,7 @@ void Encoder::init(){
190190 pinMode (pinB, INPUT);
191191 if (hasIndex ()) pinMode (index_pin,INPUT);
192192 }
193-
193+
194194 // counter setup
195195 pulse_counter = 0 ;
196196 pulse_timestamp = _micros ();
@@ -222,8 +222,7 @@ void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
222222 if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, RISING);
223223 break ;
224224 }
225-
225+
226226 // if index used initialize the index interrupt
227227 if (hasIndex () && doIndex != nullptr ) attachInterrupt (digitalPinToInterrupt (index_pin), doIndex, CHANGE);
228228}
229-
0 commit comments