source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 5202e8d

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