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

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