/
RedBotBumper.cpp
93 lines (83 loc) · 2.93 KB
/
RedBotBumper.cpp
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
/****************************************************************
Main CPP for RedBot whisker bumper.
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 <Arduino.h>
void (*whiskerAction[10])(void); // Pointer to an array of functions
// describing what should happen if a
// given pin has a whisker event.
// Standard class constructor, assumes that you want to halt the motors on a
// bump. A more skilled programmer than I could figure out the error message
// I get if I try to use the brake() function that's a part of the RedBotMotor
// class; I worked around it by making a globally available one.
RedBotBumper::RedBotBumper(int pin)
{
setPinChangeInterrupt(pin, WHISKER);
pinMode(pin, INPUT_PULLUP);
setBumpFunction(pin, &brake);
}
// Bonus points constructor, which allows the user to connect a custom function
// to a bumper.
RedBotBumper::RedBotBumper(int pin, void(*functionPointer)(void))
{
setPinChangeInterrupt(pin, WHISKER);
pinMode(pin, INPUT_PULLUP);
setBumpFunction(pin, functionPointer);
}
// Non-class function that puts the brakes on. This is the default behavior if
// the user doesn't specify a custom function for the bumper trigger.
void brake(void)
{
digitalWrite(L_CTRL_1, HIGH);
digitalWrite(L_CTRL_2, HIGH);
analogWrite(PWM_L, 0);
digitalWrite(R_CTRL_1, HIGH);
digitalWrite(R_CTRL_2, HIGH);
analogWrite(PWM_R, 0);
}
// I elected to create a function just to do this, rather than trying to wrap
// it into the setPinChangeInterrupt() function. When a bumper action occurs,
// a function will be called. By default, it's to brake the motors; users can
// write a function of their own and this function will point the interrupt
// at that custom function instead.
void RedBotBumper::setBumpFunction(int pin, void(*functionPointer)(void))
{
switch(pin)
{
case A0:
whiskerAction[PCINT_A0] = functionPointer;
break;
case A1:
whiskerAction[PCINT_A1] = functionPointer;
break;
case A2:
whiskerAction[PCINT_A2] = functionPointer;
break;
case A3:
whiskerAction[PCINT_A3] = functionPointer;
break;
case A4:
whiskerAction[PCINT_A4] = functionPointer;
break;
case A5:
whiskerAction[PCINT_A5] = functionPointer;
break;
case 3:
whiskerAction[PCINT_3] = functionPointer;
break;
case 9:
whiskerAction[PCINT_9] = functionPointer;
break;
case 10:
whiskerAction[PCINT_10] = functionPointer;
break;
case 11:
whiskerAction[PCINT_11] = functionPointer;
break;
}
}