Skip to content

Commit 515bb79

Browse files
committed
removing quadrature and moving pullup resistors to sensor
1 parent e5a4545 commit 515bb79

File tree

4 files changed

+24
-99
lines changed

4 files changed

+24
-99
lines changed

src/Encoder.h

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -6,14 +6,6 @@
66
#include "Sensor.h"
77

88

9-
/**
10-
* Pullup configuration structure
11-
*/
12-
enum Pullup{
13-
INTERN, //!< Use internal pullups
14-
EXTERN //!< Use external pullups
15-
};
16-
179
/**
1810
* Quadrature mode configuration structure
1911
*/

src/HallSensor.cpp

Lines changed: 16 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -35,72 +35,30 @@ HallSensor::HallSensor(int _encA, int _encB , float _ppr, int _index){
3535
// extern pullup as default
3636
pullup = Pullup::EXTERN;
3737
// enable quadrature HallSensor by default
38-
quadrature = Quadrature::ON;
38+
3939
}
4040

4141
// HallSensor interrupt callback functions
4242
// A channel
4343
void HallSensor::handleA() {
4444
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;
6150
}
6251
}
6352
// B channel
6453
void HallSensor::handleB() {
6554
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;
10360
}
61+
10462
}
10563

10664
/*
@@ -200,30 +158,15 @@ void HallSensor::init(){
200158
prev_pulse_counter = 0;
201159
prev_timestamp_us = _micros();
202160

203-
// initial cpr = PPR
204-
// change it if the mode is quadrature
205-
if(quadrature == Quadrature::ON) cpr = 4*cpr;
206-
207161
}
208162

209163
// function enabling hardware interrupts of the for the callback provided
210164
// if callback is not provided then the interrupt is not enabled
211165
void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doIndex)()){
212166
// 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);
228171
}
229172

src/HallSensor.h

Lines changed: 0 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -5,23 +5,6 @@
55
#include "FOCutils.h"
66
#include "Sensor.h"
77

8-
9-
/**
10-
* Pullup configuration structure
11-
*/
12-
enum Pullup{
13-
INTERN, //!< Use internal pullups
14-
EXTERN //!< Use external pullups
15-
};
16-
17-
/**
18-
* Quadrature mode configuration structure
19-
*/
20-
enum Quadrature{
21-
ON, //!< Enable quadrature mode CPR = 4xPPR
22-
OFF //!< Disable quadrature mode / CPR = PPR
23-
};
24-
258
class HallSensor: public Sensor{
269
public:
2710
/**
@@ -62,7 +45,6 @@ class HallSensor: public Sensor{
6245

6346
// HallSensor configuration
6447
Pullup pullup; //!< Configuration parameter internal or external pullups
65-
Quadrature quadrature;//!< Configuration parameter enable or disable quadrature mode
6648
float cpr;//!< HallSensor cpr number
6749

6850
// Abstract functions of the Sensor class implementation

src/Sensor.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,14 @@
11
#ifndef SENSOR_H
22
#define SENSOR_H
33

4+
/**
5+
* Pullup configuration structure
6+
*/
7+
enum Pullup{
8+
INTERN, //!< Use internal pullups
9+
EXTERN //!< Use external pullups
10+
};
11+
412
/**
513
* Sensor abstract class defintion
614
* Each sensor needs to have these functions implemented

0 commit comments

Comments
 (0)