source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 65d5c40

Last change on this file since 65d5c40 was 65d5c40, checked in by Steve Castellotti <sc@…>, 6 years ago
  • parseHop() added
  • Property mode set to 100644
File size: 38.0 KB
Line 
1/*
2
3Puzzlebox - Pyramid - Microcontroller
4
5Puzzlebox Pyramid microcontroller sketch. This sketch allows
6you to connect the Puzzlebox Pyramid to the NeuroSky MindWave
7Mobile headset over Bluetooth, then control the
8Puzzlebox Orbit helicopter using your brainwaves.
9Choose "Arduino Mega 2560 or Mega ADK" when building and
10uploading the compiled firmware.
11
12Copyright Puzzlebox Productions, LLC (2013)
13
14This code is released under the GNU Pulic License (GPL) version 2
15
16This software is distributed in the hope that it will be useful,
17but WITHOUT ANY WARRANTY; without even the implied warranty of
18MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
19
20You should have received a copy of the GNU General Public
21License along with this code; if not, write to the Free Software
22Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
23
24For more information about this licence please refer to http://www.gnu.org/copyleft/gpl.html
25
26For more details about the product please check http://puzzlebox.info
27
28Original Author: Hao Zhang <hz@puzzlebox.info>
29
30Last Modified 2013-11-12
31by 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
58byte _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
65#define HOP_THROTTLE_MAX 80
66#define HOP_THROTTLE_STEP 12
67#define HOP_YAW_MAX 127
68#define HOP_YAW_MIN 16
69#define HOP_YAW_STEP 2
70#define HOP_PITCH_MAX 63
71#define HOP_PITCH_MIN 15
72#define HOP_PITCH_STEP_FORWARD 11
73#define HOP_PITCH_STEP_BACKWARD 3
74
75char _throttle=DEFAULT_THROTTLE; //32~127, default 0
76char _yaw=DEFAULT_YAW; //16~127, default 78
77char _pitch=DEFAULT_PITCH; //0~63, default 31
78char _channel='A';
79char _command;
80byte _attention_threshold = 66; //treshold for launching Orbit helicopter
81int _throttle_hover=85;
82
83// EEG
84int eegSignal = 0;
85int eegAttention = 0;
86int eegMeditation = 0;
87int eegPower = 0;
88
89// Operation mode
90boolean modeBluetooth = false;
91boolean modeSerial = false;
92boolean modeADK = false;
93
94// Android ADK
95AndroidAccessory acc(
96        "puzzlebox",
97        "PuzzleboxPyramid",
98        "Puzzlebox Pyramid",
99        "1.2",
100        "http://pyramid.puzzlebox.info",
101        "0000000000000000"
102);
103
104byte msg[2];
105byte led;
106
107// MindWave Mobile Protocol
108#define BAUDRATE 57600
109#define DEBUGOUTPUT 0
110#define powercontrol 10
111
112// Checksum variables
113byte generatedChecksum = 0;
114byte checksum = 0;
115int payloadLength = 0;
116byte payloadData[64] = { 0 };
117byte poorQuality = 0;
118byte attention = 0;
119byte att_buff = 0;
120byte meditation = 0;
121
122// System variables
123long lastReceivedPacket = 0;
124long lastLoop = 0;
125long thisLoop = 0;
126boolean bigPacket = false;
127boolean deviceSelected = false;
128
129// Bluetooth Connection with MindWave Mobile
130String retSymb = "+RTINQ="; //start symble when there's any return
131String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
132String connectCmd = "\r\n+CONN=";
133int nameIndex = 0;
134int addrIndex = 0;
135String recvBuf;
136String slaveAddr;
137
138// RGB Panel
139byte RGB_Panel[12][3] = {
140        {0,0,0},
141        {0,0,0},
142        {0,0,0},
143        {0,0,0},
144        {0,0,0},
145        {0,0,0},
146        {0,0,0},
147        {0,0,0},
148        {0,0,0},
149        {0,0,0},
150        {0,0,0},
151        {0,0,0},
152};
153
154
155// ################################################################
156
157/////////
158//SETUP//
159/////////
160void setup() {
161       
162        pinMode(13, OUTPUT); 
163        Serial.begin(115200);
164       
165        // Android ADK
166        delay(500);
167        acc.powerOn();
168       
169       
170        ////////Orbit Flight///////////
171        // initialize all timers except for 0, to save time keeping functions
172        InitTimersSafe();
173       
174        // sets the IR_frequency for the specified pin
175        bool success = SetPinFrequencySafe(IR, IR_frequency);
176       
177        // if the pin IR_frequency was set successfully, turn pin 13 on
178        if(success) {
179                Serial.println("INFO: PWM pin frequency set"); 
180        }
181       
182        /////////////RGB////////////////
183        Wire.begin(); // join i2c bus as master (address optional for master)
184       
185       
186        setStartScreen();
187        delay(1000); // Take a second after startup to receive any USB Serial input
188       
189       
190        if (acc.isConnected()) {
191                modeADK = true;
192                Serial.println("INFO: ADK connection detected, setting modeADK");
193                setColorWheel(255, 0, 0); // red
194        }
195       
196       
197        //////////Bluetooth///////////
198       
199        if ( (! modeADK) & (! Serial.available()) ) {
200               
201                setupBlueToothConnection();
202               
203                // wait 1s and flush the Bluetooth serial buffer
204                delay(1000);
205                Serial1.flush();
206               
207       
208        }
209       
210//      selectInput(); // Determine method for Pyramid control
211       
212       
213} // setup
214
215
216// ################################################################
217
218/////////////
219//MAIN LOOP//
220/////////////
221
222void loop() {
223       
224        if (not deviceSelected)
225                selectInput(); // Determine method for Pyramid control
226       
227        if (modeBluetooth)
228                parseBluetooth();
229       
230        else if (acc.isConnected())
231                parseADK();
232       
233        else if (modeSerial)
234                parseSerialInput();
235       
236       
237        // Read from button
238        if (digitalRead(BUTTON_PIN)){
239                RGB_Panel[11][0]=255;
240                RGB_Panel[11][1]=255;
241                RGB_Panel[11][2]=255;
242                digitalWrite(13,HIGH);
243        }
244        else {
245                digitalWrite(13,LOW);
246        }
247       
248        // Send Flight Command through IR Emitter
249        if (millis()-lastLoop>_IR_period) {
250               
251                // Add Settings for _yaw and _pitch here for custom flight path
252               
253                sendCode(formCode(_throttle,_yaw,_pitch));
254               
255                if (modeBluetooth)
256                        Serial1.flush();
257               
258                thisLoop = millis() - lastLoop;
259               
260//              #if DEBUG_OUTPUT
261//                      Serial.print(" Attention:");
262//                      Serial.print(att_buff);
263//                      Serial.print(" | Time per loop:");
264//                      // Serial.println(millis() - lastLoop, DEC);
265//                      Serial.println(thisLoop);
266//              #endif
267                       
268                lastLoop = millis();
269       
270        } // if (millis()-lastLoop>_IR_period)
271
272} // Main loop
273
274
275// ################################################################
276
277void selectInput() {
278       
279        // Determine method for Pyramid control
280       
281       
282        if (digitalRead(5)) {
283                modeBluetooth=true;
284                deviceSelected=true;
285                Serial.println("INFO: selectInput(): input received, setting modeBluetooth");
286        }
287       
288       
289        if (modeBluetooth)
290                return;
291       
292        else {
293                RGB_Panel[11][0]=0;
294                RGB_Panel[11][1]=0;
295                RGB_Panel[11][2]=0;
296               
297                RGB_Panel[3][0]=0;
298                RGB_Panel[3][1]=0;
299                RGB_Panel[3][2]=0;
300               
301                RGB_Panel[7][0]=0;
302                RGB_Panel[7][1]=0;
303                RGB_Panel[7][2]=0;
304               
305                sendFrame(0);
306        }
307       
308       
309        while ( (! Serial.available()) & (! acc.isConnected()) );
310       
311        if (Serial.available()) {
312                modeSerial = true;
313                deviceSelected = true;
314                Serial.println("INFO: Serial input command received, setting modeSerial");
315                setColorWheel(255, 128, 0);
316        }
317       
318        else if (acc.isConnected()) {
319                modeADK = true;
320                deviceSelected = true;
321                Serial1.end();
322                Serial.println("INFO: ADK connection detected, setting modeADK");
323                setColorWheel(255, 0, 0); // red
324        }
325       
326       
327} // selectInput
328
329
330// ################################################################
331
332void parseBluetooth() {
333       
334        // MindWave Mobile Protocol
335       
336        if(ReadOneByte() == 170) {
337               
338                if(ReadOneByte() == 170) {
339                       
340                        payloadLength = ReadOneByte();
341                        if(payloadLength > 169)                      //Payload length can not be greater than 169
342                                return;
343
344                        generatedChecksum = 0;       
345                        for(int i = 0; i < payloadLength; i++) { 
346                                payloadData[i] = ReadOneByte();            //Read payload into memory
347                                generatedChecksum += payloadData[i];
348                        }   
349
350                        checksum = ReadOneByte();                     //Read checksum byte from stream     
351                        generatedChecksum = 255 - generatedChecksum;  //Take one's compliment of generated checksum
352
353                        if (checksum == generatedChecksum) {   
354
355                                poorQuality = 200;
356                                attention = 0;
357                                meditation = 0;
358
359                                for(int i = 0; i < payloadLength; i++) {   // Parse the payload
360                                        switch (payloadData[i]) {
361                                        case 2:
362                                                i++;           
363                                                poorQuality = payloadData[i];
364                                                bigPacket = true;         
365                                                break;
366                                        case 4:
367                                                i++;
368                                                attention = payloadData[i];
369                                                break;
370                                        case 5:
371                                                i++;
372                                                meditation = payloadData[i];
373                                                break;
374                                        case 0x80:
375                                                i = i + 3;
376                                                break;
377                                        case 0x83:
378                                                i = i + 25;     
379                                                break;
380                                        default:
381                                                break;
382                                        } // switch
383                                } // for loop
384
385                               
386                                // Update RGB Panel Display
387
388                                if(bigPacket) {
389                                       
390                                        #if DEBUG_OUTPUT
391                                                Serial.print("SignalQuality: ");
392                                                Serial.print(poorQuality, DEC);
393                                                Serial.print(" Attention: ");
394                                                Serial.print(attention, DEC);
395                                                Serial.print(" Meditation: ");
396                                                Serial.print(meditation, DEC);
397                                                Serial.print(" Time since last packet: ");
398                                                Serial.print(millis() - lastReceivedPacket, DEC);
399                                                Serial.print("\n");
400                                        #endif
401                                        lastReceivedPacket = millis();
402                                       
403                                       
404                                        if (poorQuality != 0)
405                                                setColorWheel(0,255,0); // Green
406                                       
407                                       
408                                        // Update RGB Panel when received data from MindWave Mobile
409                                        int att, med;
410                                        if (attention==0)
411                                                att=0;
412                                        else
413                                                att=(attention-1)/20+1;
414                                       
415                                        if (meditation==0)
416                                                med=0;
417                                        else
418                                                med=(meditation-1)/20+1;
419                                       
420                                        att_buff=attention;
421                                       
422                                        if (att_buff>_attention_threshold) {
423                                                RGB_Panel[11][0]=255;
424                                                RGB_Panel[11][1]=255;
425                                                RGB_Panel[11][2]=255;
426                                        }
427                                        else {
428                                                if (poorQuality == 0) {
429                                                        RGB_Panel[11][0]=0;
430                                                        RGB_Panel[11][1]=0;
431                                                        RGB_Panel[11][2]=0;
432                                                }
433                                        }
434                                       
435                                        updateFrame(att,med,poorQuality); // update buffer
436                                        sendFrame(0);// send current buffer to RGB Panel with no delay
437                                       
438                                       
439                                        if (att_buff>_attention_threshold) {
440                                               
441                                                // If detected Attention level is above the target threshold
442                                                // then send the control command to fly the helicopter.
443                                                // Otherwise send no control frames, which cause the helicopter
444                                                // to land.
445                                               
446                                                _throttle=_throttle_hover;
447                                       
448                                       
449                                        } else {
450                                               
451                                                _throttle=0;
452                                       
453                                        }
454                               
455                               
456                                } // end if(bigPacket)
457                               
458                                bigPacket = false;       
459                               
460                        } // if (checksum == generatedChecksum)
461                       
462//                      else {
463//                              #if DEBUG_OUTPUT
464//                                      Serial.println("Checksum Error!");
465//                                      // Checksum Error
466//                              #endif
467//                      }  // end if else for checksum
468               
469                } // end if read 0xAA byte (170)
470        } // end if read 0xAA byte (170)
471       
472       
473} // parseBluetooth
474
475
476// ################################################################
477
478// Read data from Serial1 UART on ADK
479byte ReadOneByte() {
480       
481        int ByteRead;
482       
483        while(!Serial1.available());
484       
485        ByteRead = Serial1.read();
486       
487        return ByteRead;
488       
489}
490
491
492// ################################################################
493
494void setupBlueToothConnection() {
495       
496        setColorWheel(0,0,255); // Blue
497
498        Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
499
500        // if you want to change baudrate, say to 115200;
501        //  Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
502        //  delay(2000); // This delay is required.
503        //  Serial1.begin(115200);
504
505        Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
506        Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
507        Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
508        Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
509        Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
510        delay(2000); // This delay is required.
511        //Serial1.flush();
512
513
514        if(digitalRead(BUTTON_PIN)==0){ //if needs pair
515                //update RGB Panel, Red Indicates it's pairing new headset
516                RGB_Panel[5][0]=255;
517                RGB_Panel[5][1]=0;
518                RGB_Panel[5][2]=0;
519                sendFrame(0);
520               
521                Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
522                Serial.println("STATUS: Pyramid is searcing for Bluetooth devices");
523                delay(2000); // This delay is required.
524               
525                //find the target slave, hence MindWave Mobile Headset
526               
527//              Serial.print("print recvChar:");
528                char recvChar;
529                while(1) {
530                        if(Serial1.available()) {
531                                recvChar = Serial1.read();
532//                              Serial.print(recvChar);
533                                recvBuf += recvChar;
534                                nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
535                                //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
536                                if ( nameIndex != -1 ) {
537                                        //Serial.print(recvBuf);
538                                        addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address                 
539                                        slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
540                                        break;
541                                }
542                        }
543                }
544               
545                // Form the full connection command
546                connectCmd += slaveAddr;
547                connectCmd += "\r\n";
548                int connectOK = 0;
549                Serial.print("INFO: Connecting to slave:");
550                Serial.print(slaveAddr);
551                Serial.print(" '");
552                Serial.print(slaveName);
553                Serial.println("'");
554               
555                // Connect to the slaves until successful
556                do {
557                        Serial1.print(connectCmd);//send connection command
558                        recvBuf = "";
559                        while(1) {
560                                if(Serial1.available()) {
561                                        recvChar = Serial1.read();
562                                        recvBuf += recvChar;
563                                        if(recvBuf.indexOf("CONNECT:OK") != -1) {
564                                                connectOK = 1;
565                                                Serial.println("INFO: Connection Successful");
566                                                //Serial1.print("Connected!");
567                                                break;
568                                        } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
569                                                Serial.println("WARN: Connection Failed");
570                                                break;
571                                        }
572                                }
573                        }
574                } while(0 == connectOK);
575               
576        } //end if needs pair
577        else { //if auto connected
578                Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
579                //update bottom RGB LED to blue
580//              setColorWheel(0,0,0); // Black
581//              RGB_Panel[5][0]=0;
582//              RGB_Panel[5][1]=0;
583//              RGB_Panel[5][2]=255;
584//              sendFrame(0);
585        }
586
587        delay(4000);
588
589        // Determine if Bluetooth connection is established
590        if (digitalRead(5)) { //D5 for Pyramid Shield, A1 for testing
591                modeBluetooth=true;
592                Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
593        }
594       
595}
596// End setupBluetoothConnection
597
598
599// ################################################################
600
601void updateFrame(byte attention, byte meditation, byte poorQuality) {
602        // update RGB_Panel[][] here to set next Frame you want to send
603        // For Example:
604        // RGB_Panel[0][0]=0;
605        // RGB_Panel[0][1]=0;
606        // RGB_Panel[0][2]=255;
607        // will update the LED on 1:00 position to be blue.
608
609        // This following code update RGB Panel based on data
610        // received from MindWave Mobile.
611
612        // if the signal is good enough, light 6:00 LED in green.
613        if (poorQuality <1) {
614                RGB_Panel[5][0]=0;
615                RGB_Panel[5][1]=255;
616                RGB_Panel[5][2]=0;
617        } else
618                return;
619
620
621
622        //light up & dim red LED according to attention level
623        for(int i=6; i<6+attention; i++) {
624                RGB_Panel[i][0]=255;
625                RGB_Panel[i][1]=0;
626                RGB_Panel[i][2]=0;
627        }
628        for(int i=6+attention; i<11; i++) {
629                RGB_Panel[i][0]=0;
630                RGB_Panel[i][1]=0;
631                RGB_Panel[i][2]=0;
632        }
633
634        //light up & dim blue LED according to meditation level
635        for(int i=4; i>4-meditation; i--) {
636                RGB_Panel[i][0]=0;
637                RGB_Panel[i][1]=0;
638                RGB_Panel[i][2]=255;
639        }
640        for(int i=4-meditation; i>-1; i--) {
641                RGB_Panel[i][0]=0;
642                RGB_Panel[i][1]=0;
643                RGB_Panel[i][2]=0;
644        }
645
646}// end updateFrame
647
648
649// ################################################################
650
651void sendFrame(int delayTime) {
652        // Maximum bytes that can be send over I2C in one
653        // transmission is 32 bytes, since we need 36 bytes
654        // to update a full frame, we just split one frame
655        // to two frames.
656
657        //delayTime=delayTime/2;
658
659        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
660        Wire.write(0);
661        for(int i=0;i<6;i++){
662                for(int j=0;j<3;j++)
663                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
664        }
665        Wire.endTransmission();    // stop transmitting
666        //delay(delayTime);
667
668        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
669        Wire.write(18);
670        for(int i=6;i<12;i++){
671                for(int j=0;j<3;j++)
672                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
673        }
674        Wire.endTransmission();    // stop transmitting
675        //delay(delayTime);
676       
677} // sendFrame
678
679
680// ################################################################
681
682void innerCycle(int onTime, int offTime) {
683       
684        // generate ON/OFF control signals, with starting and stopping PWM generator
685       
686        pwmWrite(IR, ON);
687        delayMicroseconds(onTime);
688        pwmWrite(IR, OFF);
689        delayMicroseconds(offTime);
690       
691} // innerCycle
692
693
694// ################################################################
695
696void emitCode(char BIT) {
697       
698        // emitCode generate LOWs between HIGHs as same as the parameter.
699
700        if
701                (BIT) innerCycle(671,732); // 1
702        else
703                innerCycle(337,402); // 0
704       
705       
706} // emitCode
707
708
709// ################################################################
710
711void sendCode(long code) {
712        char n;
713        //starting code, with special time period.
714        innerCycle(730,392); //(773 414)
715        innerCycle(730,392);
716
717        for (n=28;n>=0;n--)
718                emitCode((code >> n) & 1); //getting bits out from code
719
720} // sendCode
721
722
723// ################################################################
724
725long formCode(char throttle,char yaw,char pitch) {
726        char n;
727        long mainCode=0;
728        int checkSum=0;
729
730        //throttle
731        for (n=6; n>=0; n--)
732                bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
733
734        bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
735
736        //channel selection first half
737        if (_channel=='C')
738                bitWrite(mainCode,15,1);
739        else
740                bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
741
742        for (n=6; n>=0; n--)
743                bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
744
745        //channel selection second half
746        if (_channel=='A')
747                bitWrite(mainCode,7,1);
748        else
749                bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
750
751        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
752
753        for (n=5; n>=0; n--)
754                bitWrite(mainCode,n,bitRead(pitch,n)); //pitch 
755               
756        // CheckSum
757        for (n=0; n<=20; n=n+4)
758                checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
759
760        checkSum=checkSum & B1111; // get the last 4 bits of the sum
761        checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
762
763        mainCode= (mainCode << 5) | (checkSum << 1); // get the last 4 digit of CheckSum
764
765        bitWrite(mainCode,0,1);  // finish code
766       
767        return mainCode; // mainCode is a 29 bit binary number
768       
769} // formCode
770
771
772// ################################################################
773
774void setThrottle() {
775
776        char inByte=0;
777        int a=0;
778        int b=0;
779        int c=0;
780        int newThrottle=0;
781
782        while (Serial.available() == 0);
783        inByte = Serial.read() - '0';
784        a = inByte;
785
786        while (Serial.available() == 0);
787        inByte = Serial.read() - '0';
788        b = inByte;
789
790        while (Serial.available() == 0);
791        inByte = Serial.read() - '0';
792        c = inByte;
793
794        newThrottle = (a * 100) + (b * 10) + c;
795
796        if (newThrottle < 0)
797                newThrottle=0;
798
799        if (newThrottle > 100)
800                newThrottle=100;
801               
802        _throttle=newThrottle;
803
804        Serial.print("_throttle=");
805        Serial.println(int(_throttle));
806
807} // setThrottle
808
809
810// ################################################################
811
812void setYaw() {
813
814        char inByte=0;
815        int a=0;
816        int b=0;
817        int c=0;
818        int newYaw=0;
819
820        while (Serial.available() == 0);
821        inByte = Serial.read() - '0';
822        //Serial.println(inByte);
823        a = inByte;
824
825        while (Serial.available() == 0);
826        inByte = Serial.read() - '0';
827        //Serial.println(inByte);
828        b = inByte;
829
830        while (Serial.available() == 0);
831        inByte = Serial.read() - '0';
832        //Serial.println(inByte);
833        c = inByte;
834
835        newYaw = (a * 100) + (b * 10) + c;
836
837        if (newYaw < 0)
838                newYaw=0;
839
840        if (newYaw > 100)
841                newYaw=100;
842               
843        _yaw=newYaw;
844
845        Serial.print("_yaw=");
846        Serial.println(int(_yaw));
847
848} // setYaw
849
850
851// ################################################################
852
853void setPitch() {
854
855        char inByte=0;
856        int a=0;
857        int b=0;
858        int c=0;
859        int newPitch=0;
860
861        while (Serial.available() == 0);
862        inByte = Serial.read() - '0';
863        //Serial.println(inByte);
864        a = inByte;
865
866        while (Serial.available() == 0);
867        inByte = Serial.read() - '0';
868        //Serial.println(inByte);
869        b = inByte;
870
871        while (Serial.available() == 0);
872        inByte = Serial.read() - '0';
873        //Serial.println(inByte);
874        c = inByte;
875
876        newPitch = (a * 100) + (b * 10) + c;
877
878        if (newPitch < 0)
879                newPitch=0;
880
881        if (newPitch > 100)
882                newPitch=100;
883               
884        _pitch=newPitch;
885
886        Serial.print("_pitch=");
887        Serial.println(int(_pitch));
888
889} // setPitch
890
891
892// ################################################################
893
894void setColor() {
895       
896        char inByte=0;
897       
898        int position=0;
899       
900        int p1=0;
901        int p2=0;
902       
903        int red=0;
904        int blue=0;
905        int green=0;
906       
907        int r1=0;
908        int r2=0;
909        int r3=0;
910
911        int g1=0;
912        int g2=0;
913        int g3=0;
914
915        int b1=0;
916        int b2=0;
917        int b3=0;
918       
919       
920        while (Serial.available() == 0);
921        p1 = Serial.read() - '0';
922        while (Serial.available() == 0);
923        p2 = Serial.read() - '0';
924       
925        while (Serial.available() == 0);
926        r1 = Serial.read() - '0';
927        while (Serial.available() == 0);
928        r2 = Serial.read() - '0';
929        while (Serial.available() == 0);
930        r3 = Serial.read() - '0';
931       
932        while (Serial.available() == 0);
933        g1 = Serial.read() - '0';
934        while (Serial.available() == 0);
935        g2 = Serial.read() - '0';
936        while (Serial.available() == 0);
937        g3 = Serial.read() - '0';
938       
939        while (Serial.available() == 0);
940        b1 = Serial.read() - '0';
941        while (Serial.available() == 0);
942        b2 = Serial.read() - '0';
943        while (Serial.available() == 0);
944        b3 = Serial.read() - '0';
945       
946       
947        position = (p1 * 10) + p2;
948       
949        red = (r1 * 100) + (r2 * 10) + r3;
950        green = (g1 * 100) + (g2 * 10) + g3;
951        blue = (b1 * 100) + (b2 * 10) + b3;
952       
953       
954        RGB_Panel[position][0]=red;
955        RGB_Panel[position][1]=green;
956        RGB_Panel[position][2]=blue;
957       
958        sendFrame(0);
959       
960        #if DEBUG_OUTPUT
961                Serial.print("Color(");
962                Serial.print(red);
963                Serial.print(",");
964                Serial.print(green);
965                Serial.print(",");
966                Serial.print(blue);
967                Serial.print(") - Position[");
968                Serial.print(position);
969                Serial.println("]");
970        #endif
971
972
973} // setColor()
974
975
976// ################################################################
977
978void parseColorWheel() {
979       
980        char inByte=0;
981       
982        int red=0;
983        int blue=0;
984        int green=0;
985       
986        int r1=0;
987        int r2=0;
988        int r3=0;
989
990        int g1=0;
991        int g2=0;
992        int g3=0;
993
994        int b1=0;
995        int b2=0;
996        int b3=0;
997       
998       
999        while (Serial.available() == 0);
1000        r1 = Serial.read() - '0';
1001        while (Serial.available() == 0);
1002        r2 = Serial.read() - '0';
1003        while (Serial.available() == 0);
1004        r3 = Serial.read() - '0';
1005       
1006        while (Serial.available() == 0);
1007        g1 = Serial.read() - '0';
1008        while (Serial.available() == 0);
1009        g2 = Serial.read() - '0';
1010        while (Serial.available() == 0);
1011        g3 = Serial.read() - '0';
1012       
1013        while (Serial.available() == 0);
1014        b1 = Serial.read() - '0';
1015        while (Serial.available() == 0);
1016        b2 = Serial.read() - '0';
1017        while (Serial.available() == 0);
1018        b3 = Serial.read() - '0';
1019       
1020       
1021        red = (r1 * 100) + (r2 * 10) + r3;
1022        green = (g1 * 100) + (g2 * 10) + g3;
1023        blue = (b1 * 100) + (b2 * 10) + b3;
1024       
1025       
1026        setColorWheel(red,green,blue);
1027       
1028       
1029        #if DEBUG_OUTPUT
1030                Serial.print("ColorWheel(");
1031                Serial.print(red);
1032                Serial.print(",");
1033                Serial.print(green);
1034                Serial.print(",");
1035                Serial.print(blue);
1036                Serial.println(")");
1037        #endif
1038
1039} // parseColorWheel()
1040
1041
1042// ################################################################
1043
1044void setColorWheel(int red, int green, int blue) {
1045       
1046        for (int hour=0; hour < 12; hour++) {
1047       
1048                RGB_Panel[hour][0]=red;
1049                RGB_Panel[hour][1]=green;
1050                RGB_Panel[hour][2]=blue;
1051       
1052        }
1053       
1054        sendFrame(0);
1055       
1056}
1057
1058
1059// ################################################################
1060
1061void setStartScreen() {
1062       
1063        // White
1064        RGB_Panel[11][0]=255;
1065        RGB_Panel[11][1]=255;
1066        RGB_Panel[11][2]=255;
1067       
1068        RGB_Panel[3][0]=255;
1069        RGB_Panel[3][1]=255;
1070        RGB_Panel[3][2]=255;
1071       
1072        RGB_Panel[7][0]=255;
1073        RGB_Panel[7][1]=255;
1074        RGB_Panel[7][2]=255;
1075       
1076        // Red
1077        for (int hour=0; hour < 3; hour++) {
1078                RGB_Panel[hour][0]=255;
1079                RGB_Panel[hour][1]=0;
1080                RGB_Panel[hour][2]=0;
1081        }
1082       
1083        // Green
1084        for (int hour=4; hour < 7; hour++) {
1085                RGB_Panel[hour][0]=0;
1086                RGB_Panel[hour][1]=255;
1087                RGB_Panel[hour][2]=0;
1088        }
1089       
1090        // Blue
1091        for (int hour=8; hour < 11; hour++) {
1092                RGB_Panel[hour][0]=0;
1093                RGB_Panel[hour][1]=0;
1094                RGB_Panel[hour][2]=255;
1095        }
1096       
1097        sendFrame(0);
1098       
1099}
1100
1101// ################################################################
1102
1103void setColorDirection(char direction) {
1104       
1105        int p1;
1106        int p2;
1107        int p3;
1108       
1109        // Set Color Pattern
1110       
1111        setColorWheel(0, 0, 0); // black
1112       
1113        if (direction == 'f') {
1114                #if DEBUG_OUTPUT
1115                        Serial.println("Direction: Forward");
1116                #endif
1117                p1 = 10;
1118                p2 = 11;
1119                p3 = 0;
1120        }
1121        else if (direction == 'b') {
1122                #if DEBUG_OUTPUT
1123                        Serial.println("Direction: Backwards");
1124                #endif
1125                p1 = 4;
1126                p2 = 5;
1127                p3 = 6;
1128        }
1129        else if (direction == 'l') {
1130                #if DEBUG_OUTPUT
1131                        Serial.println("Direction: Left");
1132                #endif
1133                p1 = 7;
1134                p2 = 8;
1135                p3 = 9;
1136        }
1137        else if (direction == 'r') {
1138                #if DEBUG_OUTPUT
1139                        Serial.println("Direction: Right");
1140                #endif
1141                p1 = 1;
1142                p2 = 2;
1143                p3 = 3;
1144        }
1145        else if (direction == 'u') {
1146                #if DEBUG_OUTPUT
1147                        Serial.println("Direction: Up");
1148                #endif
1149                p1 = 11;
1150                p2 = 3;
1151                p3 = 7;
1152        }
1153
1154       
1155       
1156        // White
1157        RGB_Panel[p1][0]=255;
1158        RGB_Panel[p1][1]=255;
1159        RGB_Panel[p1][2]=255;
1160       
1161        RGB_Panel[p2][0]=255;
1162        RGB_Panel[p2][1]=255;
1163        RGB_Panel[p2][2]=255;
1164       
1165        RGB_Panel[p3][0]=255;
1166        RGB_Panel[p3][1]=255;
1167        RGB_Panel[p3][2]=255;
1168       
1169       
1170        sendFrame(0);
1171
1172
1173} // setColorDirection
1174
1175
1176// ################################################################
1177
1178void manualSlide(char direction, int hopThrottle, int hopYaw, int hopPitch) {
1179       
1180//      char inByte=0;
1181//     
1182//      char direction;
1183//     
1184//      int p1;
1185//      int p2;
1186//      int p3;
1187//     
1188//      while (Serial.available() == 0);
1189//      direction = Serial.read();
1190//     
1191//      Serial.print("Serial.read(): ");
1192//      Serial.println(_command);
1193       
1194       
1195        // Set Color Pattern
1196        setColorDirection(direction);
1197       
1198       
1199        // Begin Hop
1200                #if DEBUG_OUTPUT
1201                        Serial.println("Hop: (Ascending)");
1202                #endif
1203        while (hopThrottle < HOP_THROTTLE_MAX) {
1204               
1205                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1206                if (hopThrottle > HOP_THROTTLE_MAX)
1207                        hopThrottle = HOP_THROTTLE_MAX;
1208               
1209                if (direction == 'f') {
1210                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1211                        if (hopPitch > HOP_PITCH_MAX)
1212                                hopPitch = HOP_PITCH_MAX;
1213                }
1214               
1215                else if (direction == 'b') {
1216                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1217                        if (hopPitch < HOP_PITCH_MIN)
1218                                hopPitch = HOP_PITCH_MIN;
1219                }
1220               
1221                else if (direction == 'l') {
1222                        hopYaw = hopYaw + HOP_YAW_STEP;
1223                        if (hopYaw > HOP_YAW_MAX)
1224                                hopYaw = HOP_YAW_MAX;
1225                }
1226               
1227                else if (direction == 'r') {
1228                        hopYaw = hopYaw - HOP_YAW_STEP;
1229                        if (hopYaw < HOP_YAW_MIN)
1230                                hopYaw = HOP_YAW_MIN;
1231                }
1232               
1233                #if DEBUG_OUTPUT
1234                        Serial.print("Sending: (");
1235                        Serial.print(hopThrottle);
1236                        Serial.print(", ");
1237                        Serial.print(hopYaw);
1238                        Serial.print(", ");
1239                        Serial.print(hopPitch);
1240                        Serial.println(")");
1241                #endif
1242               
1243                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1244                delay(80); //cycle();
1245        }
1246       
1247        // Descend
1248                #if DEBUG_OUTPUT
1249                        Serial.println("Hop: (Descending)");
1250                #endif
1251        while (hopThrottle > DEFAULT_THROTTLE) {
1252               
1253                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1254                if (hopThrottle < DEFAULT_THROTTLE)
1255                        hopThrottle = DEFAULT_THROTTLE;
1256               
1257               
1258                #if DEBUG_OUTPUT
1259                        Serial.print("Sending: (");
1260                        Serial.print(hopThrottle);
1261                        Serial.print(", ");
1262                        Serial.print(hopYaw);
1263                        Serial.print(", ");
1264                        Serial.print(hopPitch);
1265                        Serial.println(")");
1266                #endif
1267
1268               
1269                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1270                delay(80); //cycle();
1271        }
1272       
1273       
1274        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1275        setColorWheel(255, 255, 0); // yellow
1276
1277       
1278} // manualSlide
1279
1280
1281// ################################################################
1282
1283void parseHop() {
1284       
1285        char inByte=0;
1286       
1287        char direction;
1288       
1289        int p1;
1290        int p2;
1291        int p3;
1292       
1293        int hopThrottle = DEFAULT_THROTTLE;
1294        int hopYaw = DEFAULT_YAW;
1295        int hopPitch = DEFAULT_PITCH;
1296       
1297        while (Serial.available() == 0);
1298        direction = Serial.read();
1299       
1300        Serial.print("Serial.read(): ");
1301        Serial.println(direction);
1302       
1303        // Set Color Pattern
1304        setColorDirection(direction);
1305       
1306        manualHop(direction, hopThrottle, hopYaw, hopPitch);
1307       
1308        setColorWheel(255, 255, 0); // yellow
1309       
1310        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1311       
1312       
1313}
1314
1315
1316// ################################################################
1317
1318void manualHop(char direction, int hopThrottle, int hopYaw, int hopPitch) {
1319       
1320        // Begin Hop
1321                #if DEBUG_OUTPUT
1322                        Serial.println("Hop: (Ascending)");
1323                #endif
1324        while (hopThrottle < HOP_THROTTLE_MAX) {
1325               
1326                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1327                if (hopThrottle > HOP_THROTTLE_MAX)
1328                        hopThrottle = HOP_THROTTLE_MAX;
1329               
1330                if (direction == 'f') {
1331                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1332                        if (hopPitch > HOP_PITCH_MAX)
1333                                hopPitch = HOP_PITCH_MAX;
1334                }
1335               
1336                else if (direction == 'b') {
1337                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1338                        if (hopPitch < HOP_PITCH_MIN)
1339                                hopPitch = HOP_PITCH_MIN;
1340                }
1341               
1342                else if (direction == 'l') {
1343                        hopYaw = hopYaw + HOP_YAW_STEP;
1344                        if (hopYaw > HOP_YAW_MAX)
1345                                hopYaw = HOP_YAW_MAX;
1346                }
1347               
1348                else if (direction == 'r') {
1349                        hopYaw = hopYaw - HOP_YAW_STEP;
1350                        if (hopYaw < HOP_YAW_MIN)
1351                                hopYaw = HOP_YAW_MIN;
1352                }
1353               
1354                #if DEBUG_OUTPUT
1355                        Serial.print("Sending: (");
1356                        Serial.print(hopThrottle);
1357                        Serial.print(", ");
1358                        Serial.print(hopYaw);
1359                        Serial.print(", ");
1360                        Serial.print(hopPitch);
1361                        Serial.println(")");
1362                #endif
1363               
1364                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1365                delay(80); //cycle();
1366        }
1367       
1368        // Descend
1369                #if DEBUG_OUTPUT
1370                        Serial.println("Hop: (Descending)");
1371                #endif
1372        while (hopThrottle > DEFAULT_THROTTLE) {
1373               
1374                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1375                if (hopThrottle < DEFAULT_THROTTLE)
1376                        hopThrottle = DEFAULT_THROTTLE;
1377               
1378               
1379                #if DEBUG_OUTPUT
1380                        Serial.print("Sending: (");
1381                        Serial.print(hopThrottle);
1382                        Serial.print(", ");
1383                        Serial.print(hopYaw);
1384                        Serial.print(", ");
1385                        Serial.print(hopPitch);
1386                        Serial.println(")");
1387                #endif
1388
1389               
1390                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1391                delay(80); //cycle();
1392        }
1393       
1394       
1395} // manualHop
1396
1397
1398// ################################################################
1399
1400void executeHop() {
1401       
1402        char inByte=0;
1403       
1404        char direction;
1405       
1406        int p1;
1407        int p2;
1408        int p3;
1409       
1410        int hopThrottle = DEFAULT_THROTTLE;
1411        int hopYaw = DEFAULT_YAW;
1412        int hopPitch = DEFAULT_PITCH;
1413       
1414        while (Serial.available() == 0);
1415        direction = Serial.read();
1416       
1417        Serial.print("Serial.read(): ");
1418        Serial.println(direction);
1419       
1420       
1421        // Set Color Pattern
1422       
1423        setColorWheel(0, 0, 0); // black
1424       
1425        if (direction == 'f') {
1426                #if DEBUG_OUTPUT
1427                        Serial.println("Hop: Forward");
1428                #endif
1429                p1 = 10;
1430                p2 = 11;
1431                p3 = 0;
1432        }
1433        else if (direction == 'b') {
1434                #if DEBUG_OUTPUT
1435                        Serial.println("Hop: Backwards");
1436                #endif
1437                p1 = 4;
1438                p2 = 5;
1439                p3 = 6;
1440        }
1441        else if (direction == 'l') {
1442                #if DEBUG_OUTPUT
1443                        Serial.println("Hop: Left");
1444                #endif
1445                p1 = 7;
1446                p2 = 8;
1447                p3 = 9;
1448        }
1449        else if (direction == 'r') {
1450                #if DEBUG_OUTPUT
1451                        Serial.println("Hop: Right");
1452                #endif
1453                p1 = 1;
1454                p2 = 2;
1455                p3 = 3;
1456        }
1457        else if (direction == 'u') {
1458                #if DEBUG_OUTPUT
1459                        Serial.println("Hop: Up");
1460                #endif
1461                p1 = 11;
1462                p2 = 3;
1463                p3 = 7;
1464        }
1465
1466       
1467       
1468        // White
1469        RGB_Panel[p1][0]=255;
1470        RGB_Panel[p1][1]=255;
1471        RGB_Panel[p1][2]=255;
1472       
1473        RGB_Panel[p2][0]=255;
1474        RGB_Panel[p2][1]=255;
1475        RGB_Panel[p2][2]=255;
1476       
1477        RGB_Panel[p3][0]=255;
1478        RGB_Panel[p3][1]=255;
1479        RGB_Panel[p3][2]=255;
1480       
1481       
1482        sendFrame(0);
1483       
1484       
1485       
1486        // Begin Hop
1487                #if DEBUG_OUTPUT
1488                        Serial.println("Hop: (Ascending)");
1489                #endif
1490        while (hopThrottle < HOP_THROTTLE_MAX) {
1491               
1492                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1493                if (hopThrottle > HOP_THROTTLE_MAX)
1494                        hopThrottle = HOP_THROTTLE_MAX;
1495               
1496                if (direction == 'f') {
1497                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1498                        if (hopPitch > HOP_PITCH_MAX)
1499                                hopPitch = HOP_PITCH_MAX;
1500                }
1501               
1502                else if (direction == 'b') {
1503                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1504                        if (hopPitch < HOP_PITCH_MIN)
1505                                hopPitch = HOP_PITCH_MIN;
1506                }
1507               
1508                else if (direction == 'l') {
1509                        hopYaw = hopYaw + HOP_YAW_STEP;
1510                        if (hopYaw > HOP_YAW_MAX)
1511                                hopYaw = HOP_YAW_MAX;
1512                }
1513               
1514                else if (direction == 'r') {
1515                        hopYaw = hopYaw - HOP_YAW_STEP;
1516                        if (hopYaw < HOP_YAW_MIN)
1517                                hopYaw = HOP_YAW_MIN;
1518                }
1519               
1520                #if DEBUG_OUTPUT
1521                        Serial.print("Sending: (");
1522                        Serial.print(hopThrottle);
1523                        Serial.print(", ");
1524                        Serial.print(hopYaw);
1525                        Serial.print(", ");
1526                        Serial.print(hopPitch);
1527                        Serial.println(")");
1528                #endif
1529               
1530                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1531                delay(80); //cycle();
1532        }
1533       
1534        // Descend
1535                #if DEBUG_OUTPUT
1536                        Serial.println("Hop: (Descending)");
1537                #endif
1538        while (hopThrottle > DEFAULT_THROTTLE) {
1539               
1540                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1541                if (hopThrottle < DEFAULT_THROTTLE)
1542                        hopThrottle = DEFAULT_THROTTLE;
1543               
1544               
1545                #if DEBUG_OUTPUT
1546                        Serial.print("Sending: (");
1547                        Serial.print(hopThrottle);
1548                        Serial.print(", ");
1549                        Serial.print(hopYaw);
1550                        Serial.print(", ");
1551                        Serial.print(hopPitch);
1552                        Serial.println(")");
1553                #endif
1554
1555               
1556                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1557                delay(80); //cycle();
1558        }
1559       
1560       
1561        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1562        setColorWheel(255, 255, 0); // yellow
1563       
1564       
1565} // executeHop()
1566
1567
1568// ################################################################
1569
1570int calculateMeter(int value) {
1571
1572        int result = 0;
1573
1574        if (value == 0)
1575                result = 0;
1576        else if (value <= 20)
1577                result = 1;
1578        else if (value <= 40)
1579                result = 2;
1580        else if (value <= 60)
1581                result = 3;
1582        else if (value <= 80)
1583                result = 4;
1584        else if (value <= 100)
1585                result = 5;
1586
1587        return(result);
1588
1589} // calculateMeter
1590
1591
1592// ################################################################
1593
1594void updateFrameADK() {
1595
1596        #if DEBUG_OUTPUT
1597                Serial.print("eegSignal: ");
1598                Serial.print(eegSignal, DEC);
1599                Serial.print(" | eegAttention: ");
1600                Serial.print(eegAttention, DEC);
1601                Serial.print(" | eegMeditation: ");
1602                Serial.print(eegMeditation, DEC);
1603                Serial.print(" | eegPower: ");
1604                Serial.println(eegPower, DEC);
1605                Serial.println();
1606               
1607                Serial.print("Thottle: ");
1608                Serial.print(_throttle, DEC);
1609                Serial.print(" | Yaw: ");
1610                Serial.print(_yaw, DEC);
1611                Serial.print(" | Pitch: ");
1612                Serial.println(_pitch, DEC);
1613        #endif
1614       
1615//      setColorWheel(0, 0, 0); // black
1616       
1617       
1618        // if the signal is good enough, light 6:00 LED in green.
1619        if (eegSignal == 100) {
1620                RGB_Panel[5][0]=0;
1621                RGB_Panel[5][1]=255;
1622                RGB_Panel[5][2]=0;
1623        } else {
1624                RGB_Panel[5][0]=0;
1625                RGB_Panel[5][1]=63;
1626                RGB_Panel[5][2]=0;
1627                // The following two lines can optionally be used
1628                // to set all lights to red to indicate ADK mode
1629                // when the EEG signal quality is insufficient for processing
1630//              setColorWheel(255, 0, 0); // red
1631//              return;
1632        }
1633       
1634       
1635        int attention = calculateMeter(eegAttention);
1636        int meditation = calculateMeter(eegMeditation);
1637       
1638       
1639                //light up & dim red LED according to attention level
1640        for(int i=6; i<6+attention; i++) {
1641                RGB_Panel[i][0]=255;
1642                RGB_Panel[i][1]=0;
1643                RGB_Panel[i][2]=0;
1644        }
1645        for(int i=6+attention; i<11; i++) {
1646                RGB_Panel[i][0]=0;
1647                RGB_Panel[i][1]=0;
1648                RGB_Panel[i][2]=0;
1649        }
1650
1651        //light up & dim blue LED according to meditation level
1652        for(int i=4; i>4-meditation; i--) {
1653                RGB_Panel[i][0]=0;
1654                RGB_Panel[i][1]=0;
1655                RGB_Panel[i][2]=255;
1656        }
1657        for(int i=4-meditation; i>-1; i--) {
1658                RGB_Panel[i][0]=0;
1659                RGB_Panel[i][1]=0;
1660                RGB_Panel[i][2]=0;
1661        }
1662
1663
1664        if (eegPower > 0) {
1665                RGB_Panel[11][0]=255;
1666                RGB_Panel[11][1]=255;
1667                RGB_Panel[11][2]=255;
1668//              _throttle=_throttle_hover;
1669//              Serial.println("Throttle On");
1670        } else {
1671                RGB_Panel[11][0]=0;
1672                RGB_Panel[11][1]=0;
1673                RGB_Panel[11][2]=0;
1674//              _throttle=0;
1675//              Serial.println("Throttle Off");
1676        } // eegPower
1677       
1678       
1679        sendFrame(0);
1680
1681
1682} // updateFrameADK()
1683
1684
1685// ################################################################
1686
1687void parseADK() {
1688       
1689        // Android ADK
1690       
1691        if (acc.isConnected()) {
1692               
1693                int len = acc.read(msg, sizeof(msg), 1);
1694               
1695                if (! modeADK) {
1696                        modeADK = true;
1697                        modeBluetooth = false;
1698                        modeSerial = false;
1699                        Serial1.end();
1700                        Serial.println("INFO: parseADK connection detected, setting modeADK");
1701                        setColorWheel(255, 0, 0); // red
1702                }
1703               
1704                // Action taken by Arduino is tied to the message it receives from Android
1705               
1706                if (len > 0) {
1707                       
1708//                      Serial.println("INFO: ADK message received");
1709                       
1710                        if (msg[0] == 0x1) {
1711//                              Serial.println("0x1");
1712                                eegSignal = (int)msg[1];
1713//                              Serial.println(eegSignal);
1714                        }
1715                       
1716                        else if(msg[0] == 0x2) {
1717                                eegAttention = (int)msg[1];
1718                        }
1719                       
1720                        else if (msg[0] == 0x3) {
1721                                eegMeditation = (int)msg[1];
1722                        }
1723                       
1724                        else if (msg[0] == 0x4) {
1725                                eegPower = (int)msg[1];
1726                        }
1727                       
1728                        else if(msg[0] == 0x5) {
1729                                _throttle = (int)msg[1];
1730                        }
1731                       
1732                        else if (msg[0] == 0x6) {
1733                                _yaw = (int)msg[1];
1734                        }
1735                       
1736                        else if (msg[0] == 0x7) {
1737                                _pitch = (int)msg[1];
1738                        }
1739                       
1740                        else if (msg[0] == 0x8) {
1741                                if (msg[1] == 0x1)
1742                                        _channel = 'A';
1743                                else if (msg[1] == 0x2)
1744                                        _channel = 'B';
1745                                else if (msg[1] == 0x3)
1746                                        _channel = 'C';
1747                        }
1748                       
1749                       
1750                        // Orbit Hover
1751                        if (msg[0] == 0x9) {
1752                               
1753                                if (msg[1] == 0x1) {
1754                                        setColorWheel(255,255,255); // white
1755                                        _throttle = _throttle_hover;
1756                                        _yaw = DEFAULT_YAW;
1757                                        _pitch = DEFAULT_PITCH;
1758                                }
1759                                else if (msg[1] == 0x0) {
1760                                        setColorWheel(255,0,0); // red
1761                                        _throttle = DEFAULT_THROTTLE;
1762                                        _yaw = DEFAULT_YAW;
1763                                        _pitch = DEFAULT_PITCH;
1764                                }
1765                               
1766                        } else {
1767                                // Update the color wheel with all values if not forcing hover mode
1768                                updateFrameADK();
1769                        } // Hover
1770                       
1771               
1772                } // len
1773               
1774                sendCode(formCode(_throttle,_yaw,_pitch));
1775                delay(80); //cycle();
1776               
1777               
1778        } // if acc.isConnected()
1779
1780
1781} // parseADK
1782
1783
1784// ################################################################
1785
1786void parseSerialInput() {
1787       
1788        if (Serial.available() > 0)  {
1789               
1790                if (! modeSerial) {
1791                        Serial.println("INFO: Serial input command received, setting modeSerial");
1792                        setColorWheel(255, 255, 0);
1793                        modeSerial = true;
1794                       
1795                        Serial1.end();
1796                        modeBluetooth = false;
1797                        modeADK = false;
1798                }
1799               
1800                _command = Serial.read();
1801               
1802                #if DEBUG_OUTPUT
1803                        Serial.print("Serial.read(): ");
1804                        Serial.println(_command);
1805                #endif
1806               
1807                switch (_command) {
1808                       
1809                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1810                        case 'O': _throttle=0; setColorWheel(255,255,0); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1811                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1812                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1813                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1814                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1815                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1816                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1817                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
1818                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
1819                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
1820                        case 'p':  setPitch(); break;
1821                        case 't':  setThrottle(); break;
1822                        case 'y':  setYaw(); break;
1823                        case 'x':  setThrottle(); break;
1824                        case 'c':  setColor(); break;
1825                        case 'Y':  setColorWheel(255, 255, 0); break;
1826                        case 'w':  parseColorWheel(); break;
1827                        case 's':  setStartScreen(); break;
1828                        case 'h':  executeHop(); break;
1829                        case 'o':  parseHop(); break;
1830//                      case 'm':  manualSlide(); break;
1831                }
1832        }
1833
1834
1835} // parseSerialInput()
1836
1837
1838
1839
1840
1841
Note: See TracBrowser for help on using the repository browser.