-
Notifications
You must be signed in to change notification settings - Fork 9
Expand file tree
/
Copy pathRedBot.cpp
More file actions
331 lines (301 loc) · 13 KB
/
RedBot.cpp
File metadata and controls
331 lines (301 loc) · 13 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
/****************************************************************
Main CPP for RedBot. This file handles the pin change interrupts
and how we multiplex between the different potential causes of
a pin change interrupt.
This code is beerware; if you use it, please buy me (or any other
SparkFun employee) a cold beverage next time you run into one of
us at the local.
21 Jan 2014- Mike Hord, SparkFun Electronics
Code developed in Arduino 1.0.5, on an SparkFun Redbot v12.
****************************************************************/
#include "RedBot.h"
#include <avr/interrupt.h>
#include <Arduino.h>
// We need to track what the prior state of our pins for various PCINTS was;
// this varies by interrupt. These values are initialized to the "all high"
// state; we don't want any low-to-high transitions at beginning of code
// execution to be caught.
volatile byte lastPC0PinState = 0x0E; // For pins 9, 10, 11, PB1-3
volatile byte lastPC1PinState = 0x3F; // For pins A0-A5/14-19, PC0-5
volatile byte lastPC2PinState = 0x08; // For pin 3, PD3
// We need some way to exclude short transients on the encoder inputs; we'll do
// that by capturing the most recent rise time with micros() and ignoring
// falling edges that happen within 20us of a rise.
volatile unsigned long lastRRise = 0;
volatile unsigned long lastLRise = 0;
volatile unsigned long lastBumpRise = 0;
#define ENC_HIGH_DELAY 50
#define WHISKER_HIGH_DELAY 0
byte PBMask = 0;
byte PCMask = 0;
byte PDMask = 0;
volatile byte pinFunction[10]; // Store the currently assigned function
// of the PCINT associated with each pin
// in this array. Array indices are of
// the type "PCINT_pinname".
extern void (*whiskerAction[10])(void); // Declared in RedBotBumper.cpp
extern RedBotEncoder *encoderObject; // Declared in RedBotEncoder.cpp
RedBotSoftwareSerial *RBSPObject=0;
// The RedBot uses pin change interrupts for detecting wheel encoder ticks and
// wire bumper contacts. The sources for these are normally high, so we want to
// look for falling edges and take an action when we see one.
ISR(PCINT0_vect)
{
// The first thing we want to do is determine which interrupt(s) we're
// servicing, and what those interrupts are associated with. We can cheat, a
// bit, because we know which pins we care about: for PCINT0, it's only
// bits 1, 2, and 3, which are pins 9, 10, and 11 in Arduino-land, or pins
// PB1, PB2, and PB3.
// Since all the pins are in Port B, we can check for a low-to-high transition
// by masking out the pins on Port B we don't care about and returning if they
// are all high.
byte PBTemp = PINB & PBMask; // Capture the state of the pins-of-interest now,
// before they have a chance to change.
PC0Handler(PBTemp);
}
void PC0Handler(byte PBTemp)
{
// Okay, now we have to figure out what changed, and if the change was a
// high-to-low or a low-to-high transition.
// Was it pin 9, AKA PB1?
if ((lastPC0PinState & 0x02) && !(PBTemp & 0x02)) // a falling edge
{
pinFunctionHandler(PCINT_9);
}
else if (!(lastPC0PinState & 0x02) && (PBTemp & 0x02)) // a rising edge
{
if (pinFunction[PCINT_9] == LENCODER) lastLRise = micros();
if (pinFunction[PCINT_9] == RENCODER) lastRRise = micros();
if (pinFunction[PCINT_9] == WHISKER) lastBumpRise = millis();
}
// Was it pin 10, AKA PB2?
if ((lastPC0PinState & 0x04) && !(PBTemp & 0x04)) // a falling edge
{
pinFunctionHandler(PCINT_10);
}
else if (!(lastPC0PinState & 0x04) && (PBTemp & 0x04)) // a rising edge
{
if (pinFunction[PCINT_10] == LENCODER) lastLRise = micros();
if (pinFunction[PCINT_10] == RENCODER) lastRRise = micros();
if (pinFunction[PCINT_10] == WHISKER) lastBumpRise = millis();
}
// Was it pin 11, AKA PB3?
if ((lastPC0PinState & 0x08) && !(PBTemp & 0x08)) // a falling edge
{
pinFunctionHandler(PCINT_11);
}
else if (!(lastPC0PinState & 0x04) && (PBTemp & 0x04)) // a rising edge
{
if (pinFunction[PCINT_11] == LENCODER) lastLRise = micros();
if (pinFunction[PCINT_11] == RENCODER) lastRRise = micros();
if (pinFunction[PCINT_11] == WHISKER) lastBumpRise = millis();
}
lastPC0PinState = PBTemp;
}
ISR(PCINT1_vect)
{
// The first thing we want to do is determine which interrupt(s) we're
// servicing, and what those interrupts are associated with. We can cheat, a
// bit, because we know which pins we care about: for PCINT1, it's only
// bits 0-5, PC0-PC5, or for Arduino, A0-A5/14-19.
// Since all the pins are in Port C, we can check for a low-to-high transition
// by masking out the pins on Port C we don't care about and returning if they
// are all high.
byte PCTemp = PINC & PCMask; // Capture the state of the pins-of-interest now,
// before they have a chance to change.
PC1Handler(PCTemp);
}
void PC1Handler(byte PCTemp)
{
// Okay, now we have to figure out what changed, and if the change was a
// high-to-low or a low-to-high transition. All these if() statements check
// for a high-to-low transition; we want to ignore the low-to-highs.
// Was it pin A0/14, AKA PC0?
if ((lastPC1PinState & 0x01) && !(PCTemp & 0x01))
{
pinFunctionHandler(PCINT_A0);
}
else if (!(lastPC1PinState & 0x01) && (PCTemp & 0x01))
{
if (pinFunction[PCINT_A0] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A0] == RENCODER) lastRRise = millis();
}
// Was it pin A1/15, AKA PC1?
if ((lastPC1PinState & 0x02) && !(PCTemp & 0x02))
{
pinFunctionHandler(PCINT_A1);
}
else if (!(lastPC1PinState & 0x02) && (PCTemp & 0x02))
{
if (pinFunction[PCINT_A1] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A1] == RENCODER) lastRRise = millis();
}
// Was it pin A2/16, AKA PC2?
if ((lastPC1PinState & 0x04) && !(PCTemp & 0x04))
{
pinFunctionHandler(PCINT_A2);
}
else if (!(lastPC1PinState & 0x04) && (PCTemp & 0x04))
{
if (pinFunction[PCINT_A2] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A2] == RENCODER) lastRRise = millis();
}
// Was it pin A3/17, AKA PC3?
if ((lastPC1PinState & 0x08) && !(PCTemp & 0x08))
{
pinFunctionHandler(PCINT_A3);
}
else if (!(lastPC1PinState & 0x08) && (PCTemp & 0x08))
{
if (pinFunction[PCINT_A3] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A3] == RENCODER) lastRRise = millis();
}
// Was it pin A4/18, AKA PC4?
if ((lastPC1PinState & 0x10) && !(PCTemp & 0x10))
{
pinFunctionHandler(PCINT_A4);
}
else if (!(lastPC1PinState & 0x10) && (PCTemp & 0x10))
{
if (pinFunction[PCINT_A4] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A4] == RENCODER) lastRRise = millis();
}
// Was it pin A5/19, AKA PC5?
if ((lastPC1PinState & 0x20) && !(PCTemp & 0x20))
{
pinFunctionHandler(PCINT_A5);
}
else if (!(lastPC1PinState & 0x20) && (PCTemp & 0x20))
{
if (pinFunction[PCINT_A5] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_A5] == RENCODER) lastRRise = millis();
}
lastPC1PinState = PCTemp;
}
ISR(PCINT2_vect)
{
// The first thing we want to do is determine which interrupt(s) we're
// servicing, and what those interrupts are associated with. We can cheat, a
// bit, because we know which pins we care about: for PCINT2, it's only
// bit 3, PD3 or pin 3 in Arduino-speke.
// First, check if that pin is high. If so, we don't need to know any more.
byte PDTemp = PIND & PDMask;// Capture the state of the pin-of-interest now,
// before they have a chance to change.
PC2Handler(PDTemp);
}
void PC2Handler(byte PDTemp)
{
// Okay, now we know that at least one of our pin-of-interest is low. Which one
// has GONE low since the last time we called this function?
// Was it pin 3, AKA PD3?
if ((lastPC2PinState & 0x08) && !(PDTemp & 0x08))
{
pinFunctionHandler(PCINT_3);
}
else if (!(lastPC2PinState & 0x08) && (PDTemp & 0x08))
{
if (pinFunction[PCINT_3] == LENCODER) lastLRise = millis();
if (pinFunction[PCINT_3] == RENCODER) lastRRise = millis();
}
lastPC2PinState = PDTemp;
}
void pinFunctionHandler(byte pinIndex)
{
switch(pinFunction[pinIndex])
{
case LENCODER:
if (lastLRise + ENC_HIGH_DELAY < micros()) encoderObject->wheelTick(LEFT);
//encoderObject->wheelTick(LEFT);
break;
case RENCODER:
if (lastRRise + ENC_HIGH_DELAY < micros()) encoderObject->wheelTick(RIGHT);
//encoderObject->wheelTick(RIGHT);
break;
case WHISKER:
(*whiskerAction[pinIndex])();
break;
case SW_SERIAL:
RBSPObject->recv();
case NOT_IN_USE:
break;
}
}
void setPinChangeInterrupt(int pin, byte role)
{
switch(pin)
{
// Start with the analog pins, and provide a means for either the analog
// name or the digital name to enter that case.
case A0: // PCINT 8: PCMSK1, bit 0, PC0
PCMSK1 |= 0x01; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A0] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x01; // Add this pin to our mask bits for Port C.
break;
case A1: // PCINT 9: PCMSK1, bit 1, PC1
PCMSK1 |= 0x02; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A1] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x02; // Add this pin to our mask bits for Port C.
break;
case A2: // PCINT 10: PCMSK1, bit 2, PC2
PCMSK1 |= 0x04; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A2] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x04; // Add this pin to our mask bits for Port C.
break;
case A3: // PCINT 11: PCMSK1, bit 3, PC3
PCMSK1 |= 0x08; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A3] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x08; // Add this pin to our mask bits for Port C.
break;
case A4: // PCINT 12: PCMSK1, bit 4
PCMSK1 |= 0x10; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A4] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x10; // Add this pin to our mask bits for Port C.
break;
case A5: // PCINT 13: PCMSK1, bit 5
PCMSK1 |= 0x20; // Enable the pin change interrupt for this pin.
PCICR |= 0x02; // Enable pin change interrupts for this group.
pinFunction[PCINT_A5] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PCMask |= 0x20; // Add this pin to our mask bits for Port C.
break;
// On to the digital pins.
case 3: // PCINT 19: PCMSK2, bit 3
PCMSK2 |= 0x08; // Enable the pin change interrupt for this pin.
PCICR |= 0x04; // Enable pin change interrupts for this group.
pinFunction[PCINT_3] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PDMask |= 0x08; // Add this pin to our mask bits for Port D.
break;
case 9: // PCINT 1: PCMSK0, bit 1
PCMSK0 |= 0x02; // Enable the pin change interrupt for this pin.
PCICR |= 0x01; // Enable pin change interrupts for this group.
pinFunction[PCINT_9] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PBMask |= 0x02; // Add this pin to our mask bits for Port B.
break;
case 10: // PCINT 2: PCMSK0, bit 2
PCMSK0 |= 0x04; // Enable the pin change interrupt for this pin.
PCICR |= 0x01; // Enable pin change interrupts for this group.
pinFunction[PCINT_10] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PBMask |= 0x04; // Add this pin to our mask bits for Port B.
break;
case 11: // PCINT 3: PCMSK0, bit 3
PCMSK0 |= 0x08; // Enable the pin change interrupt for this pin.
PCICR |= 0x01; // Enable pin change interrupts for this group.
pinFunction[PCINT_11] = role; // Set the role for this pin- ENCODER,
// whisker, serial, etc.
PBMask |= 0x08; // Add this pin to our mask bits for Port B.
break;
}
}