1 | /* |
---|
2 | |
---|
3 | Puzzlebox - Pyramid - Microcontroller |
---|
4 | |
---|
5 | Puzzlebox Pyramid microcontroller sketch. This sketch allows |
---|
6 | you to connect the Puzzlebox Pyramid to the NeuroSky MindWave |
---|
7 | Mobile headset over Bluetooth, then control the |
---|
8 | Puzzlebox Orbit helicopter using your brainwaves. |
---|
9 | Choose "Arduino Mega 2560 or Mega ADK" when building and |
---|
10 | uploading the compiled firmware. |
---|
11 | |
---|
12 | Copyright Puzzlebox Productions, LLC (2013) |
---|
13 | |
---|
14 | This code is released under the GNU Pulic License (GPL) version 2 |
---|
15 | |
---|
16 | This software is distributed in the hope that it will be useful, |
---|
17 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
---|
19 | |
---|
20 | You should have received a copy of the GNU General Public |
---|
21 | License along with this code; if not, write to the Free Software |
---|
22 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA |
---|
23 | |
---|
24 | For more information about this licence please refer to http://www.gnu.org/copyleft/gpl.html |
---|
25 | |
---|
26 | For more details about the product please check http://puzzlebox.info |
---|
27 | |
---|
28 | Original Author: Hao Zhang <hz@puzzlebox.info> |
---|
29 | |
---|
30 | Last Modified 2013-10-24 |
---|
31 | by Steve Castellotti <sc@puzzlebox.info> |
---|
32 | |
---|
33 | */ |
---|
34 | |
---|
35 | #include <Wire.h> //For I2C communication with RGB LED Panel |
---|
36 | #include <PWM.h> //For sending 38kHz Infrared to fly Orbit helicopter |
---|
37 | |
---|
38 | |
---|
39 | // ADK |
---|
40 | #include <Max3421e.h> |
---|
41 | #include <Usb.h> |
---|
42 | #include <AndroidAccessory.h> |
---|
43 | |
---|
44 | |
---|
45 | #define DEBUG_OUTPUT 1 // 1 for debug |
---|
46 | |
---|
47 | |
---|
48 | // Orbit Flight |
---|
49 | #define IR 6 |
---|
50 | #define ON 128 |
---|
51 | #define OFF 0 |
---|
52 | |
---|
53 | // Push Button |
---|
54 | #define BUTTON_PIN 4 |
---|
55 | |
---|
56 | // PWM IR_frequency (in Hz) required by IR |
---|
57 | #define IR_frequency 38400 |
---|
58 | byte _IR_period = 40; |
---|
59 | |
---|
60 | // Global variables for controlling Orbit helicopter |
---|
61 | #define DEFAULT_THROTTLE 0 |
---|
62 | #define DEFAULT_YAW 78 |
---|
63 | #define DEFAULT_PITCH 31 |
---|
64 | char _throttle=DEFAULT_THROTTLE; //32~127, default 0 |
---|
65 | char _yaw=DEFAULT_YAW; //16~127, default 78 |
---|
66 | char _pitch=DEFAULT_PITCH; //0~63, default 31 |
---|
67 | char _channel='A'; |
---|
68 | char _command; |
---|
69 | byte _attention_threshold = 66; //treshold for launching Orbit helicopter |
---|
70 | int _throttle_hover=85; |
---|
71 | |
---|
72 | // EEG |
---|
73 | int eegSignal = 0; |
---|
74 | int eegAttention = 0; |
---|
75 | int eegMeditation = 0; |
---|
76 | int eegPower = 0; |
---|
77 | |
---|
78 | // Operation mode |
---|
79 | boolean modeBluetooth = false; |
---|
80 | boolean modeSerial = false; |
---|
81 | boolean modeADK = false; |
---|
82 | |
---|
83 | // Android ADK |
---|
84 | AndroidAccessory acc( |
---|
85 | "puzzlebox", |
---|
86 | "PuzzleboxPyramid", |
---|
87 | "Puzzlebox Pyramid", |
---|
88 | "1.2", |
---|
89 | "http://pyramid.puzzlebox.info", |
---|
90 | "0000000000000000" |
---|
91 | ); |
---|
92 | |
---|
93 | byte msg[2]; |
---|
94 | byte led; |
---|
95 | |
---|
96 | // MindWave Mobile Protocol |
---|
97 | #define BAUDRATE 57600 |
---|
98 | #define DEBUGOUTPUT 0 |
---|
99 | #define powercontrol 10 |
---|
100 | |
---|
101 | // Checksum variables |
---|
102 | byte generatedChecksum = 0; |
---|
103 | byte checksum = 0; |
---|
104 | int payloadLength = 0; |
---|
105 | byte payloadData[64] = { 0 }; |
---|
106 | byte poorQuality = 0; |
---|
107 | byte attention = 0; |
---|
108 | byte att_buff = 0; |
---|
109 | byte meditation = 0; |
---|
110 | |
---|
111 | // System variables |
---|
112 | long lastReceivedPacket = 0; |
---|
113 | long lastLoop = 0; |
---|
114 | long thisLoop = 0; |
---|
115 | boolean bigPacket = false; |
---|
116 | |
---|
117 | // Bluetooth Connection with MindWave Mobile |
---|
118 | String retSymb = "+RTINQ="; //start symble when there's any return |
---|
119 | String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right. |
---|
120 | String connectCmd = "\r\n+CONN="; |
---|
121 | int nameIndex = 0; |
---|
122 | int addrIndex = 0; |
---|
123 | String recvBuf; |
---|
124 | String slaveAddr; |
---|
125 | |
---|
126 | // RGB Panel |
---|
127 | byte RGB_Panel[12][3] = { |
---|
128 | {0,0,0}, |
---|
129 | {0,0,0}, |
---|
130 | {0,0,0}, |
---|
131 | {0,0,0}, |
---|
132 | {0,0,0}, |
---|
133 | {0,0,0}, |
---|
134 | {0,0,0}, |
---|
135 | {0,0,0}, |
---|
136 | {0,0,0}, |
---|
137 | {0,0,0}, |
---|
138 | {0,0,0}, |
---|
139 | {0,0,0}, |
---|
140 | }; |
---|
141 | |
---|
142 | |
---|
143 | // ################################################################ |
---|
144 | |
---|
145 | ///////// |
---|
146 | //SETUP// |
---|
147 | ///////// |
---|
148 | void setup() { |
---|
149 | |
---|
150 | pinMode(13, OUTPUT); |
---|
151 | Serial.begin(115200); |
---|
152 | |
---|
153 | // Android ADK |
---|
154 | delay(500); |
---|
155 | acc.powerOn(); |
---|
156 | |
---|
157 | |
---|
158 | ////////Orbit Flight/////////// |
---|
159 | // initialize all timers except for 0, to save time keeping functions |
---|
160 | InitTimersSafe(); |
---|
161 | |
---|
162 | // sets the IR_frequency for the specified pin |
---|
163 | bool success = SetPinFrequencySafe(IR, IR_frequency); |
---|
164 | |
---|
165 | // if the pin IR_frequency was set successfully, turn pin 13 on |
---|
166 | if(success) { |
---|
167 | Serial.println("INFO: PWM pin frequency set"); |
---|
168 | } |
---|
169 | |
---|
170 | /////////////RGB//////////////// |
---|
171 | Wire.begin(); // join i2c bus as master (address optional for master) |
---|
172 | |
---|
173 | |
---|
174 | setStartScreen(); |
---|
175 | delay(1000); // Take a second after startup to receive any USB Serial input |
---|
176 | |
---|
177 | |
---|
178 | if (acc.isConnected()) { |
---|
179 | modeADK = true; |
---|
180 | Serial.println("INFO: ADK connection detected, setting modeADK"); |
---|
181 | setColorWheel(255, 0, 0); // red |
---|
182 | } |
---|
183 | |
---|
184 | |
---|
185 | //////////Bluetooth/////////// |
---|
186 | |
---|
187 | if ( (! modeADK) & (! Serial.available()) ) { |
---|
188 | |
---|
189 | // delay(500); |
---|
190 | |
---|
191 | setupBlueToothConnection(); |
---|
192 | |
---|
193 | // wait 1s and flush the Bluetooth serial buffer |
---|
194 | delay(1000); |
---|
195 | Serial1.flush(); |
---|
196 | |
---|
197 | |
---|
198 | } |
---|
199 | |
---|
200 | selectInput(); // Determine method for Pyramid control |
---|
201 | |
---|
202 | |
---|
203 | } // setup |
---|
204 | |
---|
205 | |
---|
206 | // ################################################################ |
---|
207 | |
---|
208 | ///////////// |
---|
209 | //MAIN LOOP// |
---|
210 | ///////////// |
---|
211 | void loop() { |
---|
212 | |
---|
213 | if (acc.isConnected()) |
---|
214 | parseADK(); |
---|
215 | |
---|
216 | else if (modeSerial) |
---|
217 | parseSerialInput(); |
---|
218 | |
---|
219 | else if (modeBluetooth) |
---|
220 | parseBluetooth(); |
---|
221 | |
---|
222 | |
---|
223 | // Read from button |
---|
224 | if (digitalRead(BUTTON_PIN)){ |
---|
225 | RGB_Panel[11][0]=255; |
---|
226 | RGB_Panel[11][1]=255; |
---|
227 | RGB_Panel[11][2]=255; |
---|
228 | digitalWrite(13,HIGH); |
---|
229 | } |
---|
230 | else { |
---|
231 | digitalWrite(13,LOW); |
---|
232 | } |
---|
233 | |
---|
234 | // Send Flight Command through IR Emitter |
---|
235 | if (millis()-lastLoop>_IR_period) { |
---|
236 | |
---|
237 | // Add Settings for _yaw and _pitch here for custom flight path |
---|
238 | |
---|
239 | sendCode(formCode(_throttle,_yaw,_pitch)); |
---|
240 | |
---|
241 | if (modeBluetooth) |
---|
242 | Serial1.flush(); |
---|
243 | |
---|
244 | thisLoop = millis() - lastLoop; |
---|
245 | |
---|
246 | // #if DEBUG_OUTPUT |
---|
247 | // Serial.print(" Attention:"); |
---|
248 | // Serial.print(att_buff); |
---|
249 | // Serial.print(" | Time per loop:"); |
---|
250 | // // Serial.println(millis() - lastLoop, DEC); |
---|
251 | // Serial.println(thisLoop); |
---|
252 | // #endif |
---|
253 | |
---|
254 | lastLoop = millis(); |
---|
255 | |
---|
256 | } // if (millis()-lastLoop>_IR_period) |
---|
257 | |
---|
258 | } // Main loop |
---|
259 | |
---|
260 | |
---|
261 | // ################################################################ |
---|
262 | |
---|
263 | void selectInput() { |
---|
264 | |
---|
265 | // Determine method for Pyramid control |
---|
266 | |
---|
267 | if (modeBluetooth) |
---|
268 | return; |
---|
269 | |
---|
270 | else { |
---|
271 | RGB_Panel[11][0]=0; |
---|
272 | RGB_Panel[11][1]=0; |
---|
273 | RGB_Panel[11][2]=0; |
---|
274 | |
---|
275 | RGB_Panel[3][0]=0; |
---|
276 | RGB_Panel[3][1]=0; |
---|
277 | RGB_Panel[3][2]=0; |
---|
278 | |
---|
279 | RGB_Panel[7][0]=0; |
---|
280 | RGB_Panel[7][1]=0; |
---|
281 | RGB_Panel[7][2]=0; |
---|
282 | |
---|
283 | sendFrame(0); |
---|
284 | } |
---|
285 | |
---|
286 | |
---|
287 | while ( (! Serial.available()) & (! acc.isConnected()) ); |
---|
288 | |
---|
289 | if (Serial.available()) { |
---|
290 | modeSerial = true; |
---|
291 | Serial.println("INFO: Serial input command received, setting modeSerial"); |
---|
292 | setColorWheel(255, 128, 0); |
---|
293 | } |
---|
294 | |
---|
295 | else if (acc.isConnected()) { |
---|
296 | modeADK = true; |
---|
297 | Serial1.end(); |
---|
298 | Serial.println("INFO: ADK connection detected, setting modeADK"); |
---|
299 | setColorWheel(255, 0, 0); // red |
---|
300 | } |
---|
301 | |
---|
302 | else if (Serial1.available()) { |
---|
303 | modeBluetooth = true; |
---|
304 | Serial.println("INFO: Bluetooth input received, setting modeBluetooth"); |
---|
305 | } |
---|
306 | |
---|
307 | |
---|
308 | } // selectInput |
---|
309 | |
---|
310 | |
---|
311 | // ################################################################ |
---|
312 | |
---|
313 | void parseBluetooth() { |
---|
314 | |
---|
315 | // MindWave Mobile Protocol |
---|
316 | |
---|
317 | if(ReadOneByte() == 170) { |
---|
318 | |
---|
319 | if(ReadOneByte() == 170) { |
---|
320 | |
---|
321 | payloadLength = ReadOneByte(); |
---|
322 | if(payloadLength > 169) //Payload length can not be greater than 169 |
---|
323 | return; |
---|
324 | |
---|
325 | generatedChecksum = 0; |
---|
326 | for(int i = 0; i < payloadLength; i++) { |
---|
327 | payloadData[i] = ReadOneByte(); //Read payload into memory |
---|
328 | generatedChecksum += payloadData[i]; |
---|
329 | } |
---|
330 | |
---|
331 | checksum = ReadOneByte(); //Read checksum byte from stream |
---|
332 | generatedChecksum = 255 - generatedChecksum; //Take one's compliment of generated checksum |
---|
333 | |
---|
334 | if (checksum == generatedChecksum) { |
---|
335 | |
---|
336 | poorQuality = 200; |
---|
337 | attention = 0; |
---|
338 | meditation = 0; |
---|
339 | |
---|
340 | for(int i = 0; i < payloadLength; i++) { // Parse the payload |
---|
341 | switch (payloadData[i]) { |
---|
342 | case 2: |
---|
343 | i++; |
---|
344 | poorQuality = payloadData[i]; |
---|
345 | bigPacket = true; |
---|
346 | break; |
---|
347 | case 4: |
---|
348 | i++; |
---|
349 | attention = payloadData[i]; |
---|
350 | break; |
---|
351 | case 5: |
---|
352 | i++; |
---|
353 | meditation = payloadData[i]; |
---|
354 | break; |
---|
355 | case 0x80: |
---|
356 | i = i + 3; |
---|
357 | break; |
---|
358 | case 0x83: |
---|
359 | i = i + 25; |
---|
360 | break; |
---|
361 | default: |
---|
362 | break; |
---|
363 | } // switch |
---|
364 | } // for loop |
---|
365 | |
---|
366 | |
---|
367 | // Update RGB Panel Display |
---|
368 | |
---|
369 | if(bigPacket) { |
---|
370 | |
---|
371 | #if DEBUG_OUTPUT |
---|
372 | Serial.print("SignalQuality: "); |
---|
373 | Serial.print(poorQuality, DEC); |
---|
374 | Serial.print(" Attention: "); |
---|
375 | Serial.print(attention, DEC); |
---|
376 | Serial.print(" Meditation: "); |
---|
377 | Serial.print(meditation, DEC); |
---|
378 | Serial.print(" Time since last packet: "); |
---|
379 | Serial.print(millis() - lastReceivedPacket, DEC); |
---|
380 | Serial.print("\n"); |
---|
381 | #endif |
---|
382 | lastReceivedPacket = millis(); |
---|
383 | |
---|
384 | |
---|
385 | if (poorQuality != 0) |
---|
386 | setColorWheel(0,255,0); // Green |
---|
387 | |
---|
388 | |
---|
389 | // Update RGB Panel when received data from MindWave Mobile |
---|
390 | int att, med; |
---|
391 | if (attention==0) |
---|
392 | att=0; |
---|
393 | else |
---|
394 | att=(attention-1)/20+1; |
---|
395 | |
---|
396 | if (meditation==0) |
---|
397 | med=0; |
---|
398 | else |
---|
399 | med=(meditation-1)/20+1; |
---|
400 | |
---|
401 | att_buff=attention; |
---|
402 | |
---|
403 | if (att_buff>_attention_threshold) { |
---|
404 | RGB_Panel[11][0]=255; |
---|
405 | RGB_Panel[11][1]=255; |
---|
406 | RGB_Panel[11][2]=255; |
---|
407 | } |
---|
408 | else { |
---|
409 | if (poorQuality == 0) { |
---|
410 | RGB_Panel[11][0]=0; |
---|
411 | RGB_Panel[11][1]=0; |
---|
412 | RGB_Panel[11][2]=0; |
---|
413 | } |
---|
414 | } |
---|
415 | |
---|
416 | updateFrame(att,med,poorQuality); // update buffer |
---|
417 | sendFrame(0);// send current buffer to RGB Panel with no delay |
---|
418 | |
---|
419 | |
---|
420 | if (att_buff>_attention_threshold) { |
---|
421 | |
---|
422 | // If detected Attention level is above the target threshold |
---|
423 | // then send the control command to fly the helicopter. |
---|
424 | // Otherwise send no control frames, which cause the helicopter |
---|
425 | // to land. |
---|
426 | |
---|
427 | _throttle=_throttle_hover; |
---|
428 | |
---|
429 | |
---|
430 | } else { |
---|
431 | |
---|
432 | _throttle=0; |
---|
433 | |
---|
434 | } |
---|
435 | |
---|
436 | |
---|
437 | } // end if(bigPacket) |
---|
438 | |
---|
439 | bigPacket = false; |
---|
440 | |
---|
441 | } // if (checksum == generatedChecksum) |
---|
442 | |
---|
443 | // else { |
---|
444 | // #if DEBUG_OUTPUT |
---|
445 | // Serial.println("Checksum Error!"); |
---|
446 | // // Checksum Error |
---|
447 | // #endif |
---|
448 | // } // end if else for checksum |
---|
449 | |
---|
450 | } // end if read 0xAA byte (170) |
---|
451 | } // end if read 0xAA byte (170) |
---|
452 | |
---|
453 | |
---|
454 | } // parseBluetooth |
---|
455 | |
---|
456 | |
---|
457 | // ################################################################ |
---|
458 | |
---|
459 | // Read data from Serial1 UART on ADK |
---|
460 | byte ReadOneByte() { |
---|
461 | |
---|
462 | int ByteRead; |
---|
463 | |
---|
464 | while(!Serial1.available()); |
---|
465 | |
---|
466 | ByteRead = Serial1.read(); |
---|
467 | |
---|
468 | return ByteRead; |
---|
469 | |
---|
470 | } |
---|
471 | |
---|
472 | |
---|
473 | // ################################################################ |
---|
474 | |
---|
475 | void setupBlueToothConnection() { |
---|
476 | |
---|
477 | setColorWheel(0,0,255); // Blue |
---|
478 | |
---|
479 | Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400 |
---|
480 | |
---|
481 | // if you want to change baudrate, say to 115200; |
---|
482 | // Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate |
---|
483 | // delay(2000); // This delay is required. |
---|
484 | // Serial1.begin(115200); |
---|
485 | |
---|
486 | Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode |
---|
487 | Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid" |
---|
488 | Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection |
---|
489 | Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device |
---|
490 | Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000 |
---|
491 | delay(2000); // This delay is required. |
---|
492 | //Serial1.flush(); |
---|
493 | |
---|
494 | |
---|
495 | if(digitalRead(BUTTON_PIN)==0){ //if needs pair |
---|
496 | //update RGB Panel, Red Indicates it's pairing new headset |
---|
497 | RGB_Panel[5][0]=255; |
---|
498 | RGB_Panel[5][1]=0; |
---|
499 | RGB_Panel[5][2]=0; |
---|
500 | sendFrame(0); |
---|
501 | |
---|
502 | Serial1.print("\r\n+INQ=1\r\n");//make the master inquire |
---|
503 | Serial.println("Pyramid is inquiring!"); |
---|
504 | delay(2000); // This delay is required. |
---|
505 | |
---|
506 | //find the target slave, hence MindWave Mobile Headset |
---|
507 | |
---|
508 | Serial.print("print recvChar:"); |
---|
509 | char recvChar; |
---|
510 | while(1) { |
---|
511 | if(Serial1.available()) { |
---|
512 | recvChar = Serial1.read(); |
---|
513 | Serial.print(recvChar); |
---|
514 | recvBuf += recvChar; |
---|
515 | nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name |
---|
516 | //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address |
---|
517 | if ( nameIndex != -1 ) { |
---|
518 | //Serial.print(recvBuf); |
---|
519 | addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address |
---|
520 | slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address |
---|
521 | break; |
---|
522 | } |
---|
523 | } |
---|
524 | } |
---|
525 | |
---|
526 | Serial.println(); |
---|
527 | |
---|
528 | //form the full connection command |
---|
529 | connectCmd += slaveAddr; |
---|
530 | connectCmd += "\r\n"; |
---|
531 | int connectOK = 0; |
---|
532 | Serial.print("Connecting to slave:"); |
---|
533 | Serial.print(slaveAddr); |
---|
534 | Serial.println(slaveName); |
---|
535 | //connecting the slave till they are connected |
---|
536 | do { |
---|
537 | Serial1.print(connectCmd);//send connection command |
---|
538 | recvBuf = ""; |
---|
539 | while(1) { |
---|
540 | if(Serial1.available()) { |
---|
541 | recvChar = Serial1.read(); |
---|
542 | recvBuf += recvChar; |
---|
543 | if(recvBuf.indexOf("CONNECT:OK") != -1) { |
---|
544 | connectOK = 1; |
---|
545 | Serial.println("Connected!"); |
---|
546 | //Serial1.print("Connected!"); |
---|
547 | break; |
---|
548 | } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){ |
---|
549 | Serial.println("Connect again!"); |
---|
550 | break; |
---|
551 | } |
---|
552 | } |
---|
553 | } |
---|
554 | } while(0 == connectOK); |
---|
555 | |
---|
556 | } //end if needs pair |
---|
557 | else { //if auto connected |
---|
558 | Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection |
---|
559 | //update bottom RGB LED to blue |
---|
560 | // setColorWheel(0,0,0); // Black |
---|
561 | // RGB_Panel[5][0]=0; |
---|
562 | // RGB_Panel[5][1]=0; |
---|
563 | // RGB_Panel[5][2]=255; |
---|
564 | // sendFrame(0); |
---|
565 | } |
---|
566 | |
---|
567 | delay(3000); |
---|
568 | |
---|
569 | // Determine if Bluetooth connection is established |
---|
570 | if(digitalRead(5)) //D5 for Pyramid Shield, A1 for testing |
---|
571 | modeBluetooth=true; |
---|
572 | |
---|
573 | } |
---|
574 | // End setupBluetoothConnection |
---|
575 | |
---|
576 | |
---|
577 | // ################################################################ |
---|
578 | |
---|
579 | void updateFrame(byte attention, byte meditation, byte poorQuality) { |
---|
580 | // update RGB_Panel[][] here to set next Frame you want to send |
---|
581 | // For Example: |
---|
582 | // RGB_Panel[0][0]=0; |
---|
583 | // RGB_Panel[0][1]=0; |
---|
584 | // RGB_Panel[0][2]=255; |
---|
585 | // will update the LED on 1:00 position to be blue. |
---|
586 | |
---|
587 | // This following code update RGB Panel based on data |
---|
588 | // received from MindWave Mobile. |
---|
589 | |
---|
590 | // if the signal is good enough, light 6:00 LED in green. |
---|
591 | if (poorQuality <1) { |
---|
592 | RGB_Panel[5][0]=0; |
---|
593 | RGB_Panel[5][1]=255; |
---|
594 | RGB_Panel[5][2]=0; |
---|
595 | } else |
---|
596 | return; |
---|
597 | |
---|
598 | |
---|
599 | |
---|
600 | //light up & dim red LED according to attention level |
---|
601 | for(int i=6; i<6+attention; i++) { |
---|
602 | RGB_Panel[i][0]=255; |
---|
603 | RGB_Panel[i][1]=0; |
---|
604 | RGB_Panel[i][2]=0; |
---|
605 | } |
---|
606 | for(int i=6+attention; i<11; i++) { |
---|
607 | RGB_Panel[i][0]=0; |
---|
608 | RGB_Panel[i][1]=0; |
---|
609 | RGB_Panel[i][2]=0; |
---|
610 | } |
---|
611 | |
---|
612 | //light up & dim blue LED according to meditation level |
---|
613 | for(int i=4; i>4-meditation; i--) { |
---|
614 | RGB_Panel[i][0]=0; |
---|
615 | RGB_Panel[i][1]=0; |
---|
616 | RGB_Panel[i][2]=255; |
---|
617 | } |
---|
618 | for(int i=4-meditation; i>-1; i--) { |
---|
619 | RGB_Panel[i][0]=0; |
---|
620 | RGB_Panel[i][1]=0; |
---|
621 | RGB_Panel[i][2]=0; |
---|
622 | } |
---|
623 | |
---|
624 | }// end updateFrame |
---|
625 | |
---|
626 | |
---|
627 | // ################################################################ |
---|
628 | |
---|
629 | void sendFrame(int delayTime) { |
---|
630 | // Maximum bytes that can be send over I2C in one |
---|
631 | // transmission is 32 bytes, since we need 36 bytes |
---|
632 | // to update a full frame, we just split one frame |
---|
633 | // to two frames. |
---|
634 | |
---|
635 | //delayTime=delayTime/2; |
---|
636 | |
---|
637 | Wire.beginTransmission(1); // transmit to device #1 (RGB Panel) |
---|
638 | Wire.write(0); |
---|
639 | for(int i=0;i<6;i++){ |
---|
640 | for(int j=0;j<3;j++) |
---|
641 | Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6 |
---|
642 | } |
---|
643 | Wire.endTransmission(); // stop transmitting |
---|
644 | //delay(delayTime); |
---|
645 | |
---|
646 | Wire.beginTransmission(1); // transmit to device #1 (RGB Panel) |
---|
647 | Wire.write(18); |
---|
648 | for(int i=6;i<12;i++){ |
---|
649 | for(int j=0;j<3;j++) |
---|
650 | Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12 |
---|
651 | } |
---|
652 | Wire.endTransmission(); // stop transmitting |
---|
653 | //delay(delayTime); |
---|
654 | |
---|
655 | } // sendFrame |
---|
656 | |
---|
657 | |
---|
658 | // ################################################################ |
---|
659 | |
---|
660 | void innerCycle(int onTime, int offTime) { |
---|
661 | |
---|
662 | // generate ON/OFF control signals, with starting and stopping PWM generator |
---|
663 | |
---|
664 | pwmWrite(IR, ON); |
---|
665 | delayMicroseconds(onTime); |
---|
666 | pwmWrite(IR, OFF); |
---|
667 | delayMicroseconds(offTime); |
---|
668 | |
---|
669 | } // innerCycle |
---|
670 | |
---|
671 | |
---|
672 | // ################################################################ |
---|
673 | |
---|
674 | void emitCode(char BIT) { |
---|
675 | |
---|
676 | // emitCode generate LOWs between HIGHs as same as the parameter. |
---|
677 | |
---|
678 | if |
---|
679 | (BIT) innerCycle(671,732); // 1 |
---|
680 | else |
---|
681 | innerCycle(337,402); // 0 |
---|
682 | |
---|
683 | |
---|
684 | } // emitCode |
---|
685 | |
---|
686 | |
---|
687 | // ################################################################ |
---|
688 | |
---|
689 | void sendCode(long code) { |
---|
690 | char n; |
---|
691 | //starting code, with special time period. |
---|
692 | innerCycle(730,392); //(773 414) |
---|
693 | innerCycle(730,392); |
---|
694 | |
---|
695 | for (n=28;n>=0;n--) |
---|
696 | emitCode((code >> n) & 1); //getting bits out from code |
---|
697 | |
---|
698 | } // sendCode |
---|
699 | |
---|
700 | |
---|
701 | // ################################################################ |
---|
702 | |
---|
703 | long formCode(char throttle,char yaw,char pitch) { |
---|
704 | char n; |
---|
705 | long mainCode=0; |
---|
706 | int checkSum=0; |
---|
707 | |
---|
708 | //throttle |
---|
709 | for (n=6; n>=0; n--) |
---|
710 | bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode |
---|
711 | |
---|
712 | bitWrite(mainCode,16,1); //meaning unclear, possibly left button. |
---|
713 | |
---|
714 | //channel selection first half |
---|
715 | if (_channel=='C') |
---|
716 | bitWrite(mainCode,15,1); |
---|
717 | else |
---|
718 | bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B |
---|
719 | |
---|
720 | for (n=6; n>=0; n--) |
---|
721 | bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw |
---|
722 | |
---|
723 | //channel selection second half |
---|
724 | if (_channel=='A') |
---|
725 | bitWrite(mainCode,7,1); |
---|
726 | else |
---|
727 | bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0; |
---|
728 | |
---|
729 | bitWrite(mainCode,6,0); // meaning unclear, possibly right button. |
---|
730 | |
---|
731 | for (n=5; n>=0; n--) |
---|
732 | bitWrite(mainCode,n,bitRead(pitch,n)); //pitch |
---|
733 | |
---|
734 | // CheckSum |
---|
735 | for (n=0; n<=20; n=n+4) |
---|
736 | checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code |
---|
737 | |
---|
738 | checkSum=checkSum & B1111; // get the last 4 bits of the sum |
---|
739 | checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter |
---|
740 | |
---|
741 | mainCode= (mainCode << 5) | (checkSum << 1); // get the last 4 digit of CheckSum |
---|
742 | |
---|
743 | bitWrite(mainCode,0,1); // finish code |
---|
744 | |
---|
745 | return mainCode; // mainCode is a 29 bit binary number |
---|
746 | |
---|
747 | } // formCode |
---|
748 | |
---|
749 | |
---|
750 | // ################################################################ |
---|
751 | |
---|
752 | void setThrottle() { |
---|
753 | |
---|
754 | char inByte=0; |
---|
755 | int a=0; |
---|
756 | int b=0; |
---|
757 | int c=0; |
---|
758 | int newThrottle=0; |
---|
759 | |
---|
760 | while (Serial.available() == 0); |
---|
761 | inByte = Serial.read() - '0'; |
---|
762 | a = inByte; |
---|
763 | |
---|
764 | while (Serial.available() == 0); |
---|
765 | inByte = Serial.read() - '0'; |
---|
766 | b = inByte; |
---|
767 | |
---|
768 | while (Serial.available() == 0); |
---|
769 | inByte = Serial.read() - '0'; |
---|
770 | c = inByte; |
---|
771 | |
---|
772 | newThrottle = (a * 100) + (b * 10) + c; |
---|
773 | |
---|
774 | if (newThrottle < 0) |
---|
775 | newThrottle=0; |
---|
776 | |
---|
777 | if (newThrottle > 100) |
---|
778 | newThrottle=100; |
---|
779 | |
---|
780 | _throttle=newThrottle; |
---|
781 | |
---|
782 | Serial.print("_throttle="); |
---|
783 | Serial.println(int(_throttle)); |
---|
784 | |
---|
785 | } // setThrottle |
---|
786 | |
---|
787 | |
---|
788 | // ################################################################ |
---|
789 | |
---|
790 | void setYaw() { |
---|
791 | |
---|
792 | char inByte=0; |
---|
793 | int a=0; |
---|
794 | int b=0; |
---|
795 | int c=0; |
---|
796 | int newYaw=0; |
---|
797 | |
---|
798 | while (Serial.available() == 0); |
---|
799 | inByte = Serial.read() - '0'; |
---|
800 | //Serial.println(inByte); |
---|
801 | a = inByte; |
---|
802 | |
---|
803 | while (Serial.available() == 0); |
---|
804 | inByte = Serial.read() - '0'; |
---|
805 | //Serial.println(inByte); |
---|
806 | b = inByte; |
---|
807 | |
---|
808 | while (Serial.available() == 0); |
---|
809 | inByte = Serial.read() - '0'; |
---|
810 | //Serial.println(inByte); |
---|
811 | c = inByte; |
---|
812 | |
---|
813 | newYaw = (a * 100) + (b * 10) + c; |
---|
814 | |
---|
815 | if (newYaw < 0) |
---|
816 | newYaw=0; |
---|
817 | |
---|
818 | if (newYaw > 100) |
---|
819 | newYaw=100; |
---|
820 | |
---|
821 | _yaw=newYaw; |
---|
822 | |
---|
823 | Serial.print("_yaw="); |
---|
824 | Serial.println(int(_yaw)); |
---|
825 | |
---|
826 | } // setYaw |
---|
827 | |
---|
828 | |
---|
829 | // ################################################################ |
---|
830 | |
---|
831 | void setPitch() { |
---|
832 | |
---|
833 | char inByte=0; |
---|
834 | int a=0; |
---|
835 | int b=0; |
---|
836 | int c=0; |
---|
837 | int newPitch=0; |
---|
838 | |
---|
839 | while (Serial.available() == 0); |
---|
840 | inByte = Serial.read() - '0'; |
---|
841 | //Serial.println(inByte); |
---|
842 | a = inByte; |
---|
843 | |
---|
844 | while (Serial.available() == 0); |
---|
845 | inByte = Serial.read() - '0'; |
---|
846 | //Serial.println(inByte); |
---|
847 | b = inByte; |
---|
848 | |
---|
849 | while (Serial.available() == 0); |
---|
850 | inByte = Serial.read() - '0'; |
---|
851 | //Serial.println(inByte); |
---|
852 | c = inByte; |
---|
853 | |
---|
854 | newPitch = (a * 100) + (b * 10) + c; |
---|
855 | |
---|
856 | if (newPitch < 0) |
---|
857 | newPitch=0; |
---|
858 | |
---|
859 | if (newPitch > 100) |
---|
860 | newPitch=100; |
---|
861 | |
---|
862 | _pitch=newPitch; |
---|
863 | |
---|
864 | Serial.print("_pitch="); |
---|
865 | Serial.println(int(_pitch)); |
---|
866 | |
---|
867 | } // setPitch |
---|
868 | |
---|
869 | |
---|
870 | // ################################################################ |
---|
871 | |
---|
872 | void setColor() { |
---|
873 | |
---|
874 | char inByte=0; |
---|
875 | |
---|
876 | int position=0; |
---|
877 | |
---|
878 | int p1=0; |
---|
879 | int p2=0; |
---|
880 | |
---|
881 | int red=0; |
---|
882 | int blue=0; |
---|
883 | int green=0; |
---|
884 | |
---|
885 | int r1=0; |
---|
886 | int r2=0; |
---|
887 | int r3=0; |
---|
888 | |
---|
889 | int g1=0; |
---|
890 | int g2=0; |
---|
891 | int g3=0; |
---|
892 | |
---|
893 | int b1=0; |
---|
894 | int b2=0; |
---|
895 | int b3=0; |
---|
896 | |
---|
897 | |
---|
898 | while (Serial.available() == 0); |
---|
899 | p1 = Serial.read() - '0'; |
---|
900 | while (Serial.available() == 0); |
---|
901 | p2 = Serial.read() - '0'; |
---|
902 | |
---|
903 | while (Serial.available() == 0); |
---|
904 | r1 = Serial.read() - '0'; |
---|
905 | while (Serial.available() == 0); |
---|
906 | r2 = Serial.read() - '0'; |
---|
907 | while (Serial.available() == 0); |
---|
908 | r3 = Serial.read() - '0'; |
---|
909 | |
---|
910 | while (Serial.available() == 0); |
---|
911 | g1 = Serial.read() - '0'; |
---|
912 | while (Serial.available() == 0); |
---|
913 | g2 = Serial.read() - '0'; |
---|
914 | while (Serial.available() == 0); |
---|
915 | g3 = Serial.read() - '0'; |
---|
916 | |
---|
917 | while (Serial.available() == 0); |
---|
918 | b1 = Serial.read() - '0'; |
---|
919 | while (Serial.available() == 0); |
---|
920 | b2 = Serial.read() - '0'; |
---|
921 | while (Serial.available() == 0); |
---|
922 | b3 = Serial.read() - '0'; |
---|
923 | |
---|
924 | |
---|
925 | position = (p1 * 10) + p2; |
---|
926 | |
---|
927 | red = (r1 * 100) + (r2 * 10) + r3; |
---|
928 | green = (g1 * 100) + (g2 * 10) + g3; |
---|
929 | blue = (b1 * 100) + (b2 * 10) + b3; |
---|
930 | |
---|
931 | |
---|
932 | RGB_Panel[position][0]=red; |
---|
933 | RGB_Panel[position][1]=green; |
---|
934 | RGB_Panel[position][2]=blue; |
---|
935 | |
---|
936 | sendFrame(0); |
---|
937 | |
---|
938 | |
---|
939 | Serial.print("Color("); |
---|
940 | Serial.print(red); |
---|
941 | Serial.print(","); |
---|
942 | Serial.print(green); |
---|
943 | Serial.print(","); |
---|
944 | Serial.print(blue); |
---|
945 | Serial.print(") - Position["); |
---|
946 | Serial.print(position); |
---|
947 | Serial.println("]"); |
---|
948 | |
---|
949 | |
---|
950 | } // setColor() |
---|
951 | |
---|
952 | |
---|
953 | // ################################################################ |
---|
954 | |
---|
955 | void parseColorWheel() { |
---|
956 | |
---|
957 | char inByte=0; |
---|
958 | |
---|
959 | int red=0; |
---|
960 | int blue=0; |
---|
961 | int green=0; |
---|
962 | |
---|
963 | int r1=0; |
---|
964 | int r2=0; |
---|
965 | int r3=0; |
---|
966 | |
---|
967 | int g1=0; |
---|
968 | int g2=0; |
---|
969 | int g3=0; |
---|
970 | |
---|
971 | int b1=0; |
---|
972 | int b2=0; |
---|
973 | int b3=0; |
---|
974 | |
---|
975 | |
---|
976 | while (Serial.available() == 0); |
---|
977 | r1 = Serial.read() - '0'; |
---|
978 | while (Serial.available() == 0); |
---|
979 | r2 = Serial.read() - '0'; |
---|
980 | while (Serial.available() == 0); |
---|
981 | r3 = Serial.read() - '0'; |
---|
982 | |
---|
983 | while (Serial.available() == 0); |
---|
984 | g1 = Serial.read() - '0'; |
---|
985 | while (Serial.available() == 0); |
---|
986 | g2 = Serial.read() - '0'; |
---|
987 | while (Serial.available() == 0); |
---|
988 | g3 = Serial.read() - '0'; |
---|
989 | |
---|
990 | while (Serial.available() == 0); |
---|
991 | b1 = Serial.read() - '0'; |
---|
992 | while (Serial.available() == 0); |
---|
993 | b2 = Serial.read() - '0'; |
---|
994 | while (Serial.available() == 0); |
---|
995 | b3 = Serial.read() - '0'; |
---|
996 | |
---|
997 | |
---|
998 | red = (r1 * 100) + (r2 * 10) + r3; |
---|
999 | green = (g1 * 100) + (g2 * 10) + g3; |
---|
1000 | blue = (b1 * 100) + (b2 * 10) + b3; |
---|
1001 | |
---|
1002 | |
---|
1003 | setColorWheel(red,green,blue); |
---|
1004 | |
---|
1005 | |
---|
1006 | Serial.print("ColorWheel("); |
---|
1007 | Serial.print(red); |
---|
1008 | Serial.print(","); |
---|
1009 | Serial.print(green); |
---|
1010 | Serial.print(","); |
---|
1011 | Serial.print(blue); |
---|
1012 | Serial.println(")"); |
---|
1013 | |
---|
1014 | |
---|
1015 | } // parseColorWheel() |
---|
1016 | |
---|
1017 | |
---|
1018 | // ################################################################ |
---|
1019 | |
---|
1020 | void setColorWheel(int red, int green, int blue) { |
---|
1021 | |
---|
1022 | for (int hour=0; hour < 12; hour++) { |
---|
1023 | |
---|
1024 | RGB_Panel[hour][0]=red; |
---|
1025 | RGB_Panel[hour][1]=green; |
---|
1026 | RGB_Panel[hour][2]=blue; |
---|
1027 | |
---|
1028 | } |
---|
1029 | |
---|
1030 | sendFrame(0); |
---|
1031 | |
---|
1032 | } |
---|
1033 | |
---|
1034 | |
---|
1035 | // ################################################################ |
---|
1036 | |
---|
1037 | void setStartScreen() { |
---|
1038 | |
---|
1039 | // White |
---|
1040 | RGB_Panel[11][0]=255; |
---|
1041 | RGB_Panel[11][1]=255; |
---|
1042 | RGB_Panel[11][2]=255; |
---|
1043 | |
---|
1044 | RGB_Panel[3][0]=255; |
---|
1045 | RGB_Panel[3][1]=255; |
---|
1046 | RGB_Panel[3][2]=255; |
---|
1047 | |
---|
1048 | RGB_Panel[7][0]=255; |
---|
1049 | RGB_Panel[7][1]=255; |
---|
1050 | RGB_Panel[7][2]=255; |
---|
1051 | |
---|
1052 | // Red |
---|
1053 | for (int hour=0; hour < 3; hour++) { |
---|
1054 | RGB_Panel[hour][0]=255; |
---|
1055 | RGB_Panel[hour][1]=0; |
---|
1056 | RGB_Panel[hour][2]=0; |
---|
1057 | } |
---|
1058 | |
---|
1059 | // Green |
---|
1060 | for (int hour=4; hour < 7; hour++) { |
---|
1061 | RGB_Panel[hour][0]=0; |
---|
1062 | RGB_Panel[hour][1]=255; |
---|
1063 | RGB_Panel[hour][2]=0; |
---|
1064 | } |
---|
1065 | |
---|
1066 | // Blue |
---|
1067 | for (int hour=8; hour < 11; hour++) { |
---|
1068 | RGB_Panel[hour][0]=0; |
---|
1069 | RGB_Panel[hour][1]=0; |
---|
1070 | RGB_Panel[hour][2]=255; |
---|
1071 | } |
---|
1072 | |
---|
1073 | sendFrame(0); |
---|
1074 | |
---|
1075 | } |
---|
1076 | |
---|
1077 | |
---|
1078 | // ################################################################ |
---|
1079 | |
---|
1080 | void parseSerialInput() { |
---|
1081 | |
---|
1082 | if (Serial.available() > 0) { |
---|
1083 | |
---|
1084 | if (! modeSerial) { |
---|
1085 | Serial.println("INFO: Serial input command received, setting modeSerial"); |
---|
1086 | setColorWheel(255, 255, 0); |
---|
1087 | modeSerial = true; |
---|
1088 | |
---|
1089 | Serial1.end(); |
---|
1090 | modeBluetooth = false; |
---|
1091 | modeADK = false; |
---|
1092 | } |
---|
1093 | |
---|
1094 | _command = Serial.read(); |
---|
1095 | |
---|
1096 | Serial.print("Serial.read(): "); |
---|
1097 | Serial.println(_command); |
---|
1098 | |
---|
1099 | switch (_command) { |
---|
1100 | |
---|
1101 | case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break; |
---|
1102 | case 'O': _throttle=0; setColorWheel(255,255,0); Serial.print("_throttle="); Serial.println(int(_throttle)); break; |
---|
1103 | case 'U': _throttle+=1; Serial.print("_throttle="); Serial.println(int(_throttle)); break; |
---|
1104 | case 'D': _throttle-=1; Serial.print("_throttle="); Serial.println(int(_throttle)); break; |
---|
1105 | case 'L': _yaw+=15; Serial.print("_yaw="); Serial.println(int(_yaw)); break; |
---|
1106 | case 'R': _yaw-=15; Serial.print("_yaw="); Serial.println(int(_yaw)); break; |
---|
1107 | case 'F': _pitch+=5; Serial.print("_pitch="); Serial.println(int(_pitch)); break; |
---|
1108 | case 'B': _pitch-=5; Serial.print("_pitch="); Serial.println(int(_pitch)); break; |
---|
1109 | case '1': _channel='A'; Serial.println("_channel=A"); break; |
---|
1110 | case '2': _channel='B'; Serial.println("_channel=B"); break; |
---|
1111 | case '3': _channel='C'; Serial.println("_channel=C"); break; |
---|
1112 | case 'p': setPitch(); break; |
---|
1113 | case 't': setThrottle(); break; |
---|
1114 | case 'y': setYaw(); break; |
---|
1115 | case 'x': setThrottle(); break; |
---|
1116 | case 'c': setColor(); break; |
---|
1117 | case 'Y': setColorWheel(255, 255, 0); break; |
---|
1118 | case 'w': parseColorWheel(); break; |
---|
1119 | case 's': setStartScreen(); break; |
---|
1120 | } |
---|
1121 | } |
---|
1122 | |
---|
1123 | |
---|
1124 | } // parseSerialInput() |
---|
1125 | |
---|
1126 | |
---|
1127 | // ################################################################ |
---|
1128 | |
---|
1129 | int calculateMeter(int value) { |
---|
1130 | |
---|
1131 | int result = 0; |
---|
1132 | |
---|
1133 | if (value == 0) |
---|
1134 | result = 0; |
---|
1135 | else if (value <= 20) |
---|
1136 | result = 1; |
---|
1137 | else if (value <= 40) |
---|
1138 | result = 2; |
---|
1139 | else if (value <= 60) |
---|
1140 | result = 3; |
---|
1141 | else if (value <= 80) |
---|
1142 | result = 4; |
---|
1143 | else if (value <= 100) |
---|
1144 | result = 5; |
---|
1145 | |
---|
1146 | return(result); |
---|
1147 | |
---|
1148 | } // calculateMeter |
---|
1149 | |
---|
1150 | |
---|
1151 | // ################################################################ |
---|
1152 | |
---|
1153 | void updateFrameADK() { |
---|
1154 | |
---|
1155 | #if DEBUG_OUTPUT |
---|
1156 | Serial.print("eegSignal: "); |
---|
1157 | Serial.print(eegSignal, DEC); |
---|
1158 | Serial.print(" | eegAttention: "); |
---|
1159 | Serial.print(eegAttention, DEC); |
---|
1160 | Serial.print(" | eegMeditation: "); |
---|
1161 | Serial.print(eegMeditation, DEC); |
---|
1162 | Serial.print(" | eegPower: "); |
---|
1163 | Serial.println(eegPower, DEC); |
---|
1164 | Serial.println(); |
---|
1165 | |
---|
1166 | Serial.print("Thottle: "); |
---|
1167 | Serial.print(_throttle, DEC); |
---|
1168 | Serial.print(" | Yaw: "); |
---|
1169 | Serial.print(_yaw, DEC); |
---|
1170 | Serial.print(" | Pitch: "); |
---|
1171 | Serial.println(_pitch, DEC); |
---|
1172 | #endif |
---|
1173 | |
---|
1174 | // setColorWheel(0, 0, 0); // black |
---|
1175 | |
---|
1176 | |
---|
1177 | // if the signal is good enough, light 6:00 LED in green. |
---|
1178 | if (eegSignal == 100) { |
---|
1179 | RGB_Panel[5][0]=0; |
---|
1180 | RGB_Panel[5][1]=255; |
---|
1181 | RGB_Panel[5][2]=0; |
---|
1182 | } else { |
---|
1183 | RGB_Panel[5][0]=0; |
---|
1184 | RGB_Panel[5][1]=63; |
---|
1185 | RGB_Panel[5][2]=0; |
---|
1186 | // The following two lines can optionally be used |
---|
1187 | // to set all lights to red to indicate ADK mode |
---|
1188 | // when the EEG signal quality is insufficient for processing |
---|
1189 | // setColorWheel(255, 0, 0); // red |
---|
1190 | // return; |
---|
1191 | } |
---|
1192 | |
---|
1193 | |
---|
1194 | int attention = calculateMeter(eegAttention); |
---|
1195 | int meditation = calculateMeter(eegMeditation); |
---|
1196 | |
---|
1197 | |
---|
1198 | //light up & dim red LED according to attention level |
---|
1199 | for(int i=6; i<6+attention; i++) { |
---|
1200 | RGB_Panel[i][0]=255; |
---|
1201 | RGB_Panel[i][1]=0; |
---|
1202 | RGB_Panel[i][2]=0; |
---|
1203 | } |
---|
1204 | for(int i=6+attention; i<11; i++) { |
---|
1205 | RGB_Panel[i][0]=0; |
---|
1206 | RGB_Panel[i][1]=0; |
---|
1207 | RGB_Panel[i][2]=0; |
---|
1208 | } |
---|
1209 | |
---|
1210 | //light up & dim blue LED according to meditation level |
---|
1211 | for(int i=4; i>4-meditation; i--) { |
---|
1212 | RGB_Panel[i][0]=0; |
---|
1213 | RGB_Panel[i][1]=0; |
---|
1214 | RGB_Panel[i][2]=255; |
---|
1215 | } |
---|
1216 | for(int i=4-meditation; i>-1; i--) { |
---|
1217 | RGB_Panel[i][0]=0; |
---|
1218 | RGB_Panel[i][1]=0; |
---|
1219 | RGB_Panel[i][2]=0; |
---|
1220 | } |
---|
1221 | |
---|
1222 | |
---|
1223 | if (eegPower > 0) { |
---|
1224 | RGB_Panel[11][0]=255; |
---|
1225 | RGB_Panel[11][1]=255; |
---|
1226 | RGB_Panel[11][2]=255; |
---|
1227 | // _throttle=_throttle_hover; |
---|
1228 | // Serial.println("Throttle On"); |
---|
1229 | } else { |
---|
1230 | RGB_Panel[11][0]=0; |
---|
1231 | RGB_Panel[11][1]=0; |
---|
1232 | RGB_Panel[11][2]=0; |
---|
1233 | // _throttle=0; |
---|
1234 | // Serial.println("Throttle Off"); |
---|
1235 | } // eegPower |
---|
1236 | |
---|
1237 | |
---|
1238 | sendFrame(0); |
---|
1239 | |
---|
1240 | |
---|
1241 | } // updateFrameADK() |
---|
1242 | |
---|
1243 | |
---|
1244 | // ################################################################ |
---|
1245 | |
---|
1246 | void parseADK() { |
---|
1247 | |
---|
1248 | // Android ADK |
---|
1249 | |
---|
1250 | if (acc.isConnected()) { |
---|
1251 | |
---|
1252 | int len = acc.read(msg, sizeof(msg), 1); |
---|
1253 | |
---|
1254 | if (! modeADK) { |
---|
1255 | modeADK = true; |
---|
1256 | modeBluetooth = false; |
---|
1257 | modeSerial = false; |
---|
1258 | Serial1.end(); |
---|
1259 | Serial.println("INFO: parseADK connection detected, setting modeADK"); |
---|
1260 | setColorWheel(255, 0, 0); // red |
---|
1261 | } |
---|
1262 | |
---|
1263 | // Action taken by Arduino is tied to the message it receives from Android |
---|
1264 | |
---|
1265 | if (len > 0) { |
---|
1266 | |
---|
1267 | // Serial.println("INFO: ADK message received"); |
---|
1268 | |
---|
1269 | if (msg[0] == 0x1) { |
---|
1270 | // Serial.println("0x1"); |
---|
1271 | eegSignal = (int)msg[1]; |
---|
1272 | // Serial.println(eegSignal); |
---|
1273 | } |
---|
1274 | |
---|
1275 | else if(msg[0] == 0x2) { |
---|
1276 | eegAttention = (int)msg[1]; |
---|
1277 | } |
---|
1278 | |
---|
1279 | else if (msg[0] == 0x3) { |
---|
1280 | eegMeditation = (int)msg[1]; |
---|
1281 | } |
---|
1282 | |
---|
1283 | else if (msg[0] == 0x4) { |
---|
1284 | eegPower = (int)msg[1]; |
---|
1285 | } |
---|
1286 | |
---|
1287 | else if(msg[0] == 0x5) { |
---|
1288 | _throttle = (int)msg[1]; |
---|
1289 | } |
---|
1290 | |
---|
1291 | else if (msg[0] == 0x6) { |
---|
1292 | _yaw = (int)msg[1]; |
---|
1293 | } |
---|
1294 | |
---|
1295 | else if (msg[0] == 0x7) { |
---|
1296 | _pitch = (int)msg[1]; |
---|
1297 | } |
---|
1298 | |
---|
1299 | else if (msg[0] == 0x8) { |
---|
1300 | if (msg[1] == 0x1) |
---|
1301 | _channel = 'A'; |
---|
1302 | else if (msg[1] == 0x2) |
---|
1303 | _channel = 'B'; |
---|
1304 | else if (msg[1] == 0x3) |
---|
1305 | _channel = 'C'; |
---|
1306 | } |
---|
1307 | |
---|
1308 | |
---|
1309 | // Orbit Hover |
---|
1310 | if (msg[0] == 0x9) { |
---|
1311 | |
---|
1312 | if (msg[1] == 0x1) { |
---|
1313 | setColorWheel(255,255,255); // white |
---|
1314 | _throttle = _throttle_hover; |
---|
1315 | _yaw = DEFAULT_YAW; |
---|
1316 | _pitch = DEFAULT_PITCH; |
---|
1317 | } |
---|
1318 | else if (msg[1] == 0x0) { |
---|
1319 | setColorWheel(255,0,0); // red |
---|
1320 | _throttle = DEFAULT_THROTTLE; |
---|
1321 | _yaw = DEFAULT_YAW; |
---|
1322 | _pitch = DEFAULT_PITCH; |
---|
1323 | } |
---|
1324 | |
---|
1325 | } else { |
---|
1326 | // Update the color wheel with all values if not forcing hover mode |
---|
1327 | updateFrameADK(); |
---|
1328 | } // Hover |
---|
1329 | |
---|
1330 | |
---|
1331 | } // len |
---|
1332 | |
---|
1333 | sendCode(formCode(_throttle,_yaw,_pitch)); |
---|
1334 | delay(80); //cycle(); |
---|
1335 | |
---|
1336 | |
---|
1337 | } // if acc.isConnected() |
---|
1338 | |
---|
1339 | |
---|
1340 | } // parseADK |
---|
1341 | |
---|