source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 77a80d3

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