SimpleFOClibrary  2.1
Encoder.cpp
Go to the documentation of this file.
1 #include "Encoder.h"
2 
3 
4 /*
5  Encoder(int encA, int encB , int cpr, int index)
6  - encA, encB - encoder A and B pins
7  - cpr - counts per rotation number (cpm=ppm*4)
8  - index pin - (optional input)
9 */
10 
11 Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){
12 
13  // Encoder measurement structure init
14  // hardware pins
15  pinA = _encA;
16  pinB = _encB;
17  // counter setup
18  pulse_counter = 0;
19  pulse_timestamp = 0;
20 
21  cpr = _ppr;
22  A_active = 0;
23  B_active = 0;
24  I_active = 0;
25  // index pin
26  index_pin = _index; // its 0 if not used
27 
28  // velocity calculation variables
29  prev_Th = 0;
30  pulse_per_second = 0;
31  prev_pulse_counter = 0;
32  prev_timestamp_us = _micros();
33 
34  // extern pullup as default
36  // enable quadrature encoder by default
38 }
39 
40 // Encoder interrupt callback functions
41 // A channel
43  bool A = digitalRead(pinA);
44  switch (quadrature){
45  case Quadrature::ON:
46  // CPR = 4xPPR
47  if ( A != A_active ) {
48  pulse_counter += (A_active == B_active) ? 1 : -1;
49  pulse_timestamp = _micros();
50  A_active = A;
51  }
52  break;
53  case Quadrature::OFF:
54  // CPR = PPR
55  if(A && !digitalRead(pinB)){
56  pulse_counter++;
57  pulse_timestamp = _micros();
58  }
59  break;
60  }
61 }
62 // B channel
64  bool B = digitalRead(pinB);
65  switch (quadrature){
66  case Quadrature::ON:
67  // // CPR = 4xPPR
68  if ( B != B_active ) {
69  pulse_counter += (A_active != B_active) ? 1 : -1;
70  pulse_timestamp = _micros();
71  B_active = B;
72  }
73  break;
74  case Quadrature::OFF:
75  // CPR = PPR
76  if(B && !digitalRead(pinA)){
77  pulse_counter--;
78  pulse_timestamp = _micros();
79  }
80  break;
81  }
82 }
83 
84 // Index channel
86  if(hasIndex()){
87  bool I = digitalRead(index_pin);
88  if(I && !I_active){
89  index_found = true;
90  // align encoder on each index
91  long tmp = pulse_counter;
92  // corrent the counter value
93  pulse_counter = round((double)pulse_counter/(double)cpr)*cpr;
94  // preserve relative speed
95  prev_pulse_counter += pulse_counter - tmp;
96  }
97  I_active = I;
98  }
99 }
100 
101 /*
102  Shaft angle calculation
103 */
105  return _2PI * (pulse_counter) / ((float)cpr);
106 }
107 /*
108  Shaft velocity calculation
109  function using mixed time and frequency measurement technique
110 */
112  // timestamp
113  long timestamp_us = _micros();
114  // sampling time calculation
115  float Ts = (timestamp_us - prev_timestamp_us) * 1e-6;
116  // quick fix for strange cases (micros overflow)
117  if(Ts <= 0 || Ts > 0.5) Ts = 1e-3;
118 
119  // time from last impulse
120  float Th = (timestamp_us - pulse_timestamp) * 1e-6;
121  long dN = pulse_counter - prev_pulse_counter;
122 
123  // Pulse per second calculation (Eq.3.)
124  // dN - impulses received
125  // Ts - sampling time - time in between function calls
126  // Th - time from last impulse
127  // Th_1 - time form last impulse of the previous call
128  // only increment if some impulses received
129  float dt = Ts + prev_Th - Th;
130  pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second;
131 
132  // if more than 0.05 passed in between impulses
133  if ( Th > 0.1) pulse_per_second = 0;
134 
135  // velocity calculation
136  float velocity = pulse_per_second / ((float)cpr) * (_2PI);
137 
138  // save variables for next pass
139  prev_timestamp_us = timestamp_us;
140  // save velocity calculation variables
141  prev_Th = Th;
142  prev_pulse_counter = pulse_counter;
143  return velocity;
144 }
145 
146 // getter for index pin
147 // return -1 if no index
149  return hasIndex() && !index_found;
150 }
151 
152 // private function used to determine if encoder has index
153 int Encoder::hasIndex(){
154  return index_pin != 0;
155 }
156 
157 
158 // encoder initialisation of the hardware pins
159 // and calculation variables
161 
162  // Encoder - check if pullup needed for your encoder
163  if(pullup == Pullup::INTERN){
164  pinMode(pinA, INPUT_PULLUP);
165  pinMode(pinB, INPUT_PULLUP);
166  if(hasIndex()) pinMode(index_pin,INPUT_PULLUP);
167  }else{
168  pinMode(pinA, INPUT);
169  pinMode(pinB, INPUT);
170  if(hasIndex()) pinMode(index_pin,INPUT);
171  }
172 
173  // counter setup
174  pulse_counter = 0;
175  pulse_timestamp = _micros();
176  // velocity calculation variables
177  prev_Th = 0;
178  pulse_per_second = 0;
179  prev_pulse_counter = 0;
180  prev_timestamp_us = _micros();
181 
182  // initial cpr = PPR
183  // change it if the mode is quadrature
184  if(quadrature == Quadrature::ON) cpr = 4*cpr;
185 
186 }
187 
188 // function enabling hardware interrupts of the for the callback provided
189 // if callback is not provided then the interrupt is not enabled
190 void Encoder::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
191  // attach interrupt if functions provided
192  switch(quadrature){
193  case Quadrature::ON:
194  // A callback and B callback
195  if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, CHANGE);
196  if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE);
197  break;
198  case Quadrature::OFF:
199  // A callback and B callback
200  if(doA != nullptr) attachInterrupt(digitalPinToInterrupt(pinA), doA, RISING);
201  if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, RISING);
202  break;
203  }
204 
205  // if index used initialize the index interrupt
206  if(hasIndex() && doIndex != nullptr) attachInterrupt(digitalPinToInterrupt(index_pin), doIndex, CHANGE);
207 }
Encoder::pinA
int pinA
encoder hardware pin A
Definition: Encoder.h:52
Encoder::Encoder
Encoder(int encA, int encB, float ppr, int index=0)
Definition: Encoder.cpp:11
Encoder::index_pin
int index_pin
index pin
Definition: Encoder.h:54
Encoder::handleIndex
void handleIndex()
Definition: Encoder.cpp:85
INTERN
@ INTERN
Use internal pullups.
Definition: Sensor.h:18
Encoder::handleB
void handleB()
Definition: Encoder.cpp:63
ON
@ ON
Enable quadrature mode CPR = 4xPPR.
Definition: Encoder.h:14
Encoder::init
void init()
Definition: Encoder.cpp:160
Encoder::enableInterrupts
void enableInterrupts(void(*doA)()=nullptr, void(*doB)()=nullptr, void(*doIndex)()=nullptr)
Definition: Encoder.cpp:190
EXTERN
@ EXTERN
Use external pullups.
Definition: Sensor.h:19
velocity
@ velocity
Velocity motion control.
Definition: FOCMotor.h:29
Encoder::pullup
Pullup pullup
Configuration parameter internal or external pullups.
Definition: Encoder.h:57
OFF
@ OFF
Disable quadrature mode / CPR = PPR.
Definition: Encoder.h:15
Encoder::quadrature
Quadrature quadrature
Configuration parameter enable or disable quadrature mode.
Definition: Encoder.h:58
_2PI
#define _2PI
Definition: foc_utils.h:23
Encoder::cpr
float cpr
encoder cpr number
Definition: Encoder.h:59
Encoder::getVelocity
float getVelocity() override
Definition: Encoder.cpp:111
Encoder::needsSearch
int needsSearch() override
Definition: Encoder.cpp:148
Encoder::handleA
void handleA()
Definition: Encoder.cpp:42
_micros
unsigned long _micros()
Definition: time_utils.cpp:21
Encoder::getAngle
float getAngle() override
Definition: Encoder.cpp:104
Encoder::pinB
int pinB
encoder hardware pin B
Definition: Encoder.h:53
Encoder.h