source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 08b50b3

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