Zumo32U4 library
Zumo32U4Encoders.cpp
1 // Copyright Pololu Corporation. For more information, see http://www.pololu.com/
2 
3 #include <Zumo32U4Encoders.h>
4 #include <FastGPIO.h>
5 #include <avr/interrupt.h>
6 #include <Arduino.h>
7 
8 #define LEFT_XOR 8
9 #define LEFT_B IO_E2
10 #define RIGHT_XOR 7
11 #define RIGHT_B 23
12 
13 static volatile bool lastLeftA;
14 static volatile bool lastLeftB;
15 static volatile bool lastRightA;
16 static volatile bool lastRightB;
17 
18 static volatile bool errorLeft;
19 static volatile bool errorRight;
20 
21 // These count variables are uint16_t instead of int16_t because
22 // signed integer overflow is undefined behavior in C++.
23 static volatile uint16_t countLeft;
24 static volatile uint16_t countRight;
25 
26 ISR(PCINT0_vect)
27 {
28  bool newLeftB = FastGPIO::Pin<LEFT_B>::isInputHigh();
29  bool newLeftA = FastGPIO::Pin<LEFT_XOR>::isInputHigh() ^ newLeftB;
30 
31  countLeft += (newLeftA ^ lastLeftB) - (lastLeftA ^ newLeftB);
32 
33  if((lastLeftA ^ newLeftA) & (lastLeftB ^ newLeftB))
34  {
35  errorLeft = true;
36  }
37 
38  lastLeftA = newLeftA;
39  lastLeftB = newLeftB;
40 }
41 
42 static void rightISR()
43 {
44  bool newRightB = FastGPIO::Pin<RIGHT_B>::isInputHigh();
45  bool newRightA = FastGPIO::Pin<RIGHT_XOR>::isInputHigh() ^ newRightB;
46 
47  countRight += (newRightA ^ lastRightB) - (lastRightA ^ newRightB);
48 
49  if((lastRightA ^ newRightA) & (lastRightB ^ newRightB))
50  {
51  errorRight = true;
52  }
53 
54  lastRightA = newRightA;
55  lastRightB = newRightB;
56 }
57 
58 void Zumo32U4Encoders::init2()
59 {
60  // Set the pins as pulled-up inputs.
61  FastGPIO::Pin<LEFT_XOR>::setInputPulledUp();
62  FastGPIO::Pin<LEFT_B>::setInputPulledUp();
63  FastGPIO::Pin<RIGHT_XOR>::setInputPulledUp();
64  FastGPIO::Pin<RIGHT_B>::setInputPulledUp();
65 
66  // Enable pin-change interrupt on PB4 for left encoder, and disable other
67  // pin-change interrupts.
68  PCICR = (1 << PCIE0);
69  PCMSK0 = (1 << PCINT4);
70  PCIFR = (1 << PCIF0); // Clear its interrupt flag by writing a 1.
71 
72  // Enable interrupt on PE6 for the right encoder. We use attachInterrupt
73  // instead of defining ISR(INT6_vect) ourselves so that this class will be
74  // compatible with other code that uses attachInterrupt.
75  attachInterrupt(4, rightISR, CHANGE);
76 
77  // Initialize the variables. It's good to do this after enabling the
78  // interrupts in case the interrupts fired by accident as we were enabling
79  // them.
80  lastLeftB = FastGPIO::Pin<LEFT_B>::isInputHigh();
81  lastLeftA = FastGPIO::Pin<LEFT_XOR>::isInputHigh() ^ lastLeftB;
82  countLeft = 0;
83  errorLeft = 0;
84 
85  lastRightB = FastGPIO::Pin<RIGHT_B>::isInputHigh();
86  lastRightA = FastGPIO::Pin<RIGHT_XOR>::isInputHigh() ^ lastRightB;
87  countRight = 0;
88  errorRight = 0;
89 }
90 
92 {
93  init();
94 
95  cli();
96  int16_t counts = countLeft;
97  sei();
98  return counts;
99 }
100 
102 {
103  init();
104 
105  cli();
106  int16_t counts = countRight;
107  sei();
108  return counts;
109 }
110 
112 {
113  init();
114 
115  cli();
116  int16_t counts = countLeft;
117  countLeft = 0;
118  sei();
119  return counts;
120 }
121 
123 {
124  init();
125 
126  cli();
127  int16_t counts = countRight;
128  countRight = 0;
129  sei();
130  return counts;
131 }
132 
134 {
135  init();
136 
137  bool error = errorLeft;
138  errorLeft = 0;
139  return error;
140 }
141 
143 {
144  init();
145 
146  bool error = errorRight;
147  errorRight = 0;
148  return error;
149 }
static int16_t getCountsLeft()
static int16_t getCountsAndResetRight()
static int16_t getCountsAndResetLeft()
static bool checkErrorRight()
static bool checkErrorLeft()
static int16_t getCountsRight()
static void init()