@@ -35,72 +35,30 @@ HallSensor::HallSensor(int _encA, int _encB , float _ppr, int _index){
35
35
// extern pullup as default
36
36
pullup = Pullup::EXTERN;
37
37
// enable quadrature HallSensor by default
38
- quadrature = Quadrature::ON;
38
+
39
39
}
40
40
41
41
// HallSensor interrupt callback functions
42
42
// A channel
43
43
void HallSensor::handleA () {
44
44
int A = digitalRead (pinA);
45
- switch (quadrature){
46
- case Quadrature::ON:
47
- // CPR = 4xPPR
48
- if ( A != A_active ) {
49
- pulse_counter += (A_active == B_active) ? 1 : -1 ;
50
- pulse_timestamp = _micros ();
51
- A_active = A;
52
- }
53
- break ;
54
- case Quadrature::OFF:
55
- // CPR = PPR
56
- if (A && !digitalRead (pinB)){
57
- pulse_counter++;
58
- pulse_timestamp = _micros ();
59
- }
60
- break ;
45
+
46
+ if ( A != A_active ) {
47
+ pulse_counter += (A_active == B_active) ? 1 : -1 ;
48
+ pulse_timestamp = _micros ();
49
+ A_active = A;
61
50
}
62
51
}
63
52
// B channel
64
53
void HallSensor::handleB () {
65
54
int B = digitalRead (pinB);
66
- switch (quadrature){
67
- case Quadrature::ON:
68
- // // CPR = 4xPPR
69
- if ( B != B_active ) {
70
- pulse_counter += (A_active != B_active) ? 1 : -1 ;
71
- pulse_timestamp = _micros ();
72
- B_active = B;
73
- }
74
- break ;
75
- case Quadrature::OFF:
76
- // CPR = PPR
77
- if (B && !digitalRead (pinA)){
78
- pulse_counter--;
79
- pulse_timestamp = _micros ();
80
- }
81
- break ;
82
- }
83
- }
84
-
85
- // Index channel
86
- void HallSensor::handleIndex () {
87
- if (hasIndex ()){
88
- int I = digitalRead (index_pin);
89
- if (I && !I_active){
90
- // align HallSensor on each index
91
- if (index_pulse_counter){
92
- long tmp = pulse_counter;
93
- // corrent the counter value
94
- pulse_counter = round ((float )pulse_counter/(float )cpr)*cpr;
95
- // preserve relative speed
96
- prev_pulse_counter += pulse_counter - tmp;
97
- } else {
98
- // initial offset
99
- index_pulse_counter = pulse_counter;
100
- }
101
- }
102
- I_active = I;
55
+
56
+ if ( B != B_active ) {
57
+ pulse_counter += (A_active != B_active) ? 1 : -1 ;
58
+ pulse_timestamp = _micros ();
59
+ B_active = B;
103
60
}
61
+
104
62
}
105
63
106
64
/*
@@ -200,30 +158,15 @@ void HallSensor::init(){
200
158
prev_pulse_counter = 0 ;
201
159
prev_timestamp_us = _micros ();
202
160
203
- // initial cpr = PPR
204
- // change it if the mode is quadrature
205
- if (quadrature == Quadrature::ON) cpr = 4 *cpr;
206
-
207
161
}
208
162
209
163
// function enabling hardware interrupts of the for the callback provided
210
164
// if callback is not provided then the interrupt is not enabled
211
165
void HallSensor::enableInterrupts (void (*doA)(), void(*doB)(), void(*doIndex)()){
212
166
// attach interrupt if functions provided
213
- switch (quadrature){
214
- case Quadrature::ON:
215
- // A callback and B callback
216
- if (doA != nullptr ) attachInterrupt (digitalPinToInterrupt (pinA), doA, CHANGE);
217
- if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, CHANGE);
218
- break ;
219
- case Quadrature::OFF:
220
- // A callback and B callback
221
- if (doA != nullptr ) attachInterrupt (digitalPinToInterrupt (pinA), doA, RISING);
222
- if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, RISING);
223
- break ;
224
- }
225
-
226
- // if index used initialize the index interrupt
227
- if (hasIndex () && doIndex != nullptr ) attachInterrupt (digitalPinToInterrupt (index_pin), doIndex, CHANGE);
167
+
168
+ // A callback and B callback
169
+ if (doA != nullptr ) attachInterrupt (digitalPinToInterrupt (pinA), doA, CHANGE);
170
+ if (doB != nullptr ) attachInterrupt (digitalPinToInterrupt (pinB), doB, CHANGE);
228
171
}
229
172
0 commit comments