robobuggy / arduino / InterruptRCRecieverReference / InterruptRCReciever.ino @ c5d6b0e8
History | View | Annotate | Download (6.14 KB)
1 | c5d6b0e8 | msebek | #include "RCArduinoFastLib.h" |
---|---|---|---|
2 | |||
3 | // include the pinchangeint library - see the links in the related topics section above for details |
||
4 | #include "PinChangeInt.h" |
||
5 | |||
6 | // Assign your channel in pins |
||
7 | #define THROTTLE_IN_PIN 5 |
||
8 | #define STEERING_IN_PIN 6 |
||
9 | #define AUX_IN_PIN 7 |
||
10 | |||
11 | // Assign your channel out pins |
||
12 | #define THROTTLE_OUT_PIN 9//8 |
||
13 | #define STEERING_OUT_PIN 8//9 |
||
14 | #define AUX_OUT_PIN 10 |
||
15 | |||
16 | // Assign servo indexes |
||
17 | #define SERVO_THROTTLE 0 |
||
18 | #define SERVO_STEERING 1 |
||
19 | #define SERVO_AUX 2 |
||
20 | #define SERVO_FRAME_SPACE 3 |
||
21 | |||
22 | // These bit flags are set in bUpdateFlagsShared to indicate which |
||
23 | // channels have new signals |
||
24 | #define THROTTLE_FLAG 1 |
||
25 | #define STEERING_FLAG 2 |
||
26 | #define AUX_FLAG 4 |
||
27 | |||
28 | // holds the update flags defined above |
||
29 | volatile uint8_t bUpdateFlagsShared; |
||
30 | |||
31 | // shared variables are updated by the ISR and read by loop. |
||
32 | // In loop we immediatley take local copies so that the ISR can keep ownership of the |
||
33 | // shared ones. To access these in loop |
||
34 | // we first turn interrupts off with noInterrupts |
||
35 | // we take a copy to use in loop and the turn interrupts back on |
||
36 | // as quickly as possible, this ensures that we are always able to receive new signals |
||
37 | volatile uint16_t unThrottleInShared; |
||
38 | volatile uint16_t unSteeringInShared; |
||
39 | volatile uint16_t unAuxInShared; |
||
40 | |||
41 | // These are used to record the rising edge of a pulse in the calcInput functions |
||
42 | // They do not need to be volatile as they are only used in the ISR. If we wanted |
||
43 | // to refer to these in loop and the ISR then they would need to be declared volatile |
||
44 | uint16_t unThrottleInStart; |
||
45 | uint16_t unSteeringInStart; |
||
46 | uint16_t unAuxInStart; |
||
47 | |||
48 | uint16_t unLastAuxIn = 0; |
||
49 | uint32_t ulVariance = 0; |
||
50 | uint32_t ulGetNextSampleMillis = 0; |
||
51 | uint16_t unMaxDifference = 0; |
||
52 | |||
53 | void setup() |
||
54 | { |
||
55 | Serial.begin(115200); |
||
56 | |||
57 | Serial.println("multiChannels"); |
||
58 | |||
59 | // attach servo objects, these will generate the correct |
||
60 | // pulses for driving Electronic speed controllers, servos or other devices |
||
61 | // designed to interface directly with RC Receivers |
||
62 | CRCArduinoFastServos::attach(SERVO_THROTTLE,THROTTLE_OUT_PIN); |
||
63 | CRCArduinoFastServos::attach(SERVO_STEERING,STEERING_OUT_PIN); |
||
64 | CRCArduinoFastServos::attach(SERVO_AUX,AUX_OUT_PIN); |
||
65 | |||
66 | // lets set a standard rate of 50 Hz by setting a frame space of 10 * 2000 = 3 Servos + 7 times 2000 |
||
67 | CRCArduinoFastServos::setFrameSpaceA(SERVO_FRAME_SPACE,7*2000); |
||
68 | |||
69 | CRCArduinoFastServos::begin(); |
||
70 | |||
71 | // using the PinChangeInt library, attach the interrupts |
||
72 | // used to read the channels |
||
73 | PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE); |
||
74 | PCintPort::attachInterrupt(STEERING_IN_PIN, calcSteering,CHANGE); |
||
75 | PCintPort::attachInterrupt(AUX_IN_PIN, calcAux,CHANGE); |
||
76 | } |
||
77 | |||
78 | void loop() |
||
79 | { |
||
80 | // create local variables to hold a local copies of the channel inputs |
||
81 | // these are declared static so that thier values will be retained |
||
82 | // between calls to loop. |
||
83 | static uint16_t unThrottleIn; |
||
84 | static uint16_t unSteeringIn; |
||
85 | static uint16_t unAuxIn; |
||
86 | // local copy of update flags |
||
87 | static uint8_t bUpdateFlags; |
||
88 | |||
89 | // check shared update flags to see if any channels have a new signal |
||
90 | if(bUpdateFlagsShared) |
||
91 | { |
||
92 | noInterrupts(); // turn interrupts off quickly while we take local copies of the shared variables |
||
93 | |||
94 | // take a local copy of which channels were updated in case we need to use this in the rest of loop |
||
95 | bUpdateFlags = bUpdateFlagsShared; |
||
96 | |||
97 | // in the current code, the shared values are always populated |
||
98 | // so we could copy them without testing the flags |
||
99 | // however in the future this could change, so lets |
||
100 | // only copy when the flags tell us we can. |
||
101 | |||
102 | if(bUpdateFlags & THROTTLE_FLAG) |
||
103 | { |
||
104 | unThrottleIn = unThrottleInShared; |
||
105 | } |
||
106 | |||
107 | if(bUpdateFlags & STEERING_FLAG) |
||
108 | { |
||
109 | unSteeringIn = unSteeringInShared; |
||
110 | } |
||
111 | |||
112 | if(bUpdateFlags & AUX_FLAG) |
||
113 | { |
||
114 | unAuxIn = unAuxInShared; |
||
115 | } |
||
116 | |||
117 | // clear shared copy of updated flags as we have already taken the updates |
||
118 | // we still have a local copy if we need to use it in bUpdateFlags |
||
119 | bUpdateFlagsShared = 0; |
||
120 | |||
121 | interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on |
||
122 | // as soon as interrupts are back on, we can no longer use the shared copies, the interrupt |
||
123 | // service routines own these and could update them at any time. During the update, the |
||
124 | // shared copies may contain junk. Luckily we have our local copies to work with :-) |
||
125 | } |
||
126 | |||
127 | // do any processing from here onwards |
||
128 | // only use the local values unAuxIn, unThrottleIn and unSteeringIn, the shared |
||
129 | // variables unAuxInShared, unThrottleInShared, unSteeringInShared are always owned by |
||
130 | // the interrupt routines and should not be used in loop |
||
131 | |||
132 | // the following code provides simple pass through |
||
133 | // this is a good initial test, the Arduino will pass through |
||
134 | // receiver input as if the Arduino is not there. |
||
135 | // This should be used to confirm the circuit and power |
||
136 | // before attempting any custom processing in a project. |
||
137 | |||
138 | // we are checking to see if the channel value has changed, this is indicated |
||
139 | // by the flags. For the simple pass through we don't really need this check, |
||
140 | // but for a more complex project where a new signal requires significant processing |
||
141 | // this allows us to only calculate new values when we have new inputs, rather than |
||
142 | // on every cycle. |
||
143 | if(bUpdateFlags & THROTTLE_FLAG) |
||
144 | { |
||
145 | CRCArduinoFastServos::writeMicroseconds(SERVO_THROTTLE,unThrottleIn); |
||
146 | } |
||
147 | |||
148 | if(bUpdateFlags & STEERING_FLAG) |
||
149 | { |
||
150 | CRCArduinoFastServos::writeMicroseconds(SERVO_STEERING,unSteeringIn); |
||
151 | } |
||
152 | |||
153 | if(bUpdateFlags & AUX_FLAG) |
||
154 | { |
||
155 | CRCArduinoFastServos::writeMicroseconds(SERVO_AUX,unAuxIn); |
||
156 | } |
||
157 | |||
158 | delay(500); |
||
159 | bUpdateFlags = 0; |
||
160 | } |
||
161 | |||
162 | |||
163 | // simple interrupt service routine |
||
164 | void calcThrottle() |
||
165 | { |
||
166 | if(PCintPort::pinState) |
||
167 | { |
||
168 | unThrottleInStart = TCNT1; |
||
169 | } |
||
170 | else |
||
171 | { |
||
172 | unThrottleInShared = (TCNT1 - unThrottleInStart)>>1; |
||
173 | bUpdateFlagsShared |= THROTTLE_FLAG; |
||
174 | } |
||
175 | } |
||
176 | |||
177 | void calcSteering() |
||
178 | { |
||
179 | if(PCintPort::pinState) |
||
180 | { |
||
181 | unSteeringInStart = TCNT1; |
||
182 | } |
||
183 | else |
||
184 | { |
||
185 | unSteeringInShared = (TCNT1 - unSteeringInStart)>>1; |
||
186 | |||
187 | bUpdateFlagsShared |= STEERING_FLAG; |
||
188 | } |
||
189 | } |
||
190 | |||
191 | void calcAux() |
||
192 | { |
||
193 | if(PCintPort::pinState) |
||
194 | { |
||
195 | unAuxInStart = TCNT1; |
||
196 | } |
||
197 | else |
||
198 | { |
||
199 | unAuxInShared = (TCNT1 - unAuxInStart)>>1; |
||
200 | bUpdateFlagsShared |= AUX_FLAG; } |
||
201 | } |
||
202 |