source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 7d2ea4b

Last change on this file since 7d2ea4b was 7d2ea4b, checked in by Steve Castellotti <sc@…>, 6 years ago
  • default Attention trigger set to 66
  • Property mode set to 100644
File size: 28.6 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-10-24
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
64char _throttle=DEFAULT_THROTTLE; //32~127, default 0
65char _yaw=DEFAULT_YAW; //16~127, default 78
66char _pitch=DEFAULT_PITCH; //0~63, default 31
67char _channel='A';
68char _command;
69byte _attention_threshold = 66; //treshold for launching Orbit helicopter
70int _throttle_hover=85;
71
72// EEG
73int eegSignal = 0;
74int eegAttention = 0;
75int eegMeditation = 0;
76int eegPower = 0;
77
78// Operation mode
79boolean modeBluetooth = false;
80boolean modeSerial = false;
81boolean modeADK = false;
82
83// Android ADK
84AndroidAccessory acc(
85        "puzzlebox",
86        "PuzzleboxPyramid",
87        "Puzzlebox Pyramid",
88        "1.2",
89        "http://pyramid.puzzlebox.info",
90        "0000000000000000"
91);
92
93byte msg[2];
94byte led;
95
96// MindWave Mobile Protocol
97#define BAUDRATE 57600
98#define DEBUGOUTPUT 0
99#define powercontrol 10
100
101// Checksum variables
102byte generatedChecksum = 0;
103byte checksum = 0;
104int payloadLength = 0;
105byte payloadData[64] = { 0 };
106byte poorQuality = 0;
107byte attention = 0;
108byte att_buff = 0;
109byte meditation = 0;
110
111// System variables
112long lastReceivedPacket = 0;
113long lastLoop = 0;
114long thisLoop = 0;
115boolean bigPacket = false;
116
117// Bluetooth Connection with MindWave Mobile
118String retSymb = "+RTINQ="; //start symble when there's any return
119String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
120String connectCmd = "\r\n+CONN=";
121int nameIndex = 0;
122int addrIndex = 0;
123String recvBuf;
124String slaveAddr;
125
126// RGB Panel
127byte 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/////////
148void 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/////////////
211void 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
263void 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
313void 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
460byte 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
475void 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
579void 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
629void 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
660void 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
674void 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
689void 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
703long 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
752void 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
790void 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
831void 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
872void 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
955void 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
1020void 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
1037void 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
1080void 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
1129int 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
1153void 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
1246void 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
Note: See TracBrowser for help on using the repository browser.