source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 124e727

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