source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ b8dc886

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