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

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