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

Last change on this file since f5452ab was f5452ab, checked in by Steve Castellotti <sc@…>, 6 years ago
  • ADK visual processing corrected
  • Property mode set to 100644
File size: 29.4 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        Serial.print("updateFrameADK(): ");
1079        Serial.print("eegSignal: ");
1080        Serial.print(eegSignal, DEC);
1081        Serial.print(" | eegAttention: ");
1082        Serial.print(eegAttention, DEC);
1083        Serial.print(" | eegMeditation: ");
1084        Serial.println(eegMeditation, DEC);
1085       
1086       
1087        setColorWheel(0, 0, 0); // black
1088       
1089       
1090        // if the signal is good enough, light 6:00 LED in green.
1091        if (eegSignal == 100) {
1092                RGB_Panel[5][0]=0;
1093                RGB_Panel[5][1]=255;
1094                RGB_Panel[5][2]=0;
1095        } else {
1096                setColorWheel(255, 0, 0); // red
1097//              RGB_Panel[5][0]=0;
1098//              RGB_Panel[5][1]=0;
1099//              RGB_Panel[5][2]=0;
1100                return;
1101        }
1102
1103        int attention = calculateMeter(eegAttention);
1104        int meditation = calculateMeter(eegMeditation);
1105
1106
1107//      for(int i=6; i<=6+attention; i++){
1108//              RGB_Panel[i][0]=255;
1109//              RGB_Panel[i][1]=0;
1110//              RGB_Panel[i][2]=0;
1111//      }
1112//
1113//      if (meditation==6){
1114//              RGB_Panel[11][0]=0;
1115//              RGB_Panel[11][1]=0;
1116//              RGB_Panel[11][2]=255;
1117//      }
1118//             
1119//      for(int i=4; i>=4-meditation; i--){
1120//              RGB_Panel[i][0]=0;
1121//              RGB_Panel[i][1]=0;
1122//              RGB_Panel[i][2]=255;
1123//      }
1124//
1125//      if (attention==6)
1126//              RGB_Panel[11][0]=255;
1127//             
1128//      for(int i=6+attention; i<=11; i++){
1129//              RGB_Panel[i][0]=0;
1130//              RGB_Panel[i][1]=0;
1131//              RGB_Panel[i][2]=0;
1132//      }
1133//
1134//      for(int i=4-meditation; i>=0; i--){
1135//              RGB_Panel[i][0]=0;
1136//              RGB_Panel[i][1]=0;
1137//              RGB_Panel[i][2]=0;
1138//      }
1139       
1140       
1141                //light up & dim red LED according to attention level
1142        for(int i=6; i<6+attention; i++) {
1143                RGB_Panel[i][0]=255;
1144                RGB_Panel[i][1]=0;
1145                RGB_Panel[i][2]=0;
1146        }
1147//      for(int i=6+attention; i<11; i++) {
1148//              RGB_Panel[i][0]=0;
1149//              RGB_Panel[i][1]=0;
1150//              RGB_Panel[i][2]=0;
1151//      }
1152
1153        //light up & dim blue LED according to meditation level
1154        for(int i=4; i>4-meditation; i--) {
1155                RGB_Panel[i][0]=0;
1156                RGB_Panel[i][1]=0;
1157                RGB_Panel[i][2]=255;
1158        }
1159//      for(int i=4-meditation; i>-1; i--) {
1160//              RGB_Panel[i][0]=0;
1161//              RGB_Panel[i][1]=0;
1162//              RGB_Panel[i][2]=0;
1163//      }
1164
1165
1166} // updateFrameADK()
1167
1168
1169// ################################################################
1170
1171void setOrbitThrottle(int throttle) {
1172       
1173        setColorWheel(255,255,0); // Yellow
1174       
1175        _throttle=throttle;
1176        setThrottle();
1177        sendCode(formCode(_throttle,_yaw,_pitch));
1178        delay(80); // originally 36, 51, 66, 80
1179       
1180} // setOrbitThrottle()
1181
1182
1183// ################################################################
1184
1185void parseADK() {
1186       
1187        // Android ADK
1188       
1189        if (acc.isConnected()) {
1190               
1191                int len = acc.read(msg, sizeof(msg), 1);
1192               
1193                if (! modeADK) {
1194                        modeADK = true;
1195                        modeBluetooth = false;
1196                        modeSerial = false;
1197                        Serial1.end();
1198                        Serial.println("INFO: parseADK connection detected, setting modeADK");
1199                        setColorWheel(255, 0, 0); // red
1200                }
1201               
1202                // Action taken by Arduino is tied to the message it receives from Android
1203               
1204                if (len > 0) {
1205                       
1206//                      setColorWheel(255,255,255); // White
1207                       
1208                        if (msg[0] == 0x1) {
1209                                // Reserved
1210                        }
1211                        else if(msg[0] == 0x2) {
1212                                // Reserved
1213                        }
1214                        else if (msg[0] == 0x3) {
1215                                // Reserved
1216                        }
1217                       
1218                        // EEG Signal
1219                        else if (msg[0] == 0x4) {
1220                                eegSignal = 0;
1221                                if (msg[1] == 0x1) {
1222                                        eegSignal = 10;
1223                                } else if (msg[1] == 0x2) {
1224                                        eegSignal = 20;
1225                                } else if (msg[1] == 0x3) {
1226                                        eegSignal = 30;
1227                                } else if (msg[1] == 0x4) {
1228                                        eegSignal = 40;
1229                                } else if (msg[1] == 0x5) {
1230                                        eegSignal = 50;
1231                                } else if (msg[1] == 0x6) {
1232                                        eegSignal = 60;
1233                                } else if (msg[1] == 0x7) {
1234                                        eegSignal = 70;
1235                                } else if (msg[1] == 0x8) {
1236                                        eegSignal = 80;
1237                                } else if (msg[1] == 0x9) {
1238                                        eegSignal = 90;
1239                                } else {
1240                                        eegSignal = 100;
1241                                }
1242                        }
1243                       
1244                       
1245                        // EEG Attention
1246                        else if (msg[0] == 0x5) {
1247                                eegAttention = 0;
1248                                if (msg[1] == 0x1) {
1249                                        eegAttention = 10;
1250                                } else if (msg[1] == 0x2) {
1251                                        eegAttention = 20;
1252                                } else if (msg[1] == 0x3) {
1253                                        eegAttention = 30;
1254                                } else if (msg[1] == 0x4) {
1255                                        eegAttention = 40;
1256                                } else if (msg[1] == 0x5) {
1257                                        eegAttention = 50;
1258                                } else if (msg[1] == 0x6) {
1259                                        eegAttention = 60;
1260                                } else if (msg[1] == 0x7) {
1261                                        eegAttention = 70;
1262                                } else if (msg[1] == 0x8) {
1263                                        eegAttention = 80;
1264                                } else if (msg[1] == 0x9) {
1265                                        eegAttention = 90;
1266                                } else {
1267                                        eegAttention = 100;
1268                                }
1269                        }
1270                       
1271                       
1272                        // EEG Meditation
1273                        else if (msg[0] == 0x6) {
1274                                eegMeditation = 0;
1275                                if (msg[1] == 0x1) {
1276                                        eegMeditation = 10;
1277                                } else if (msg[1] == 0x2) {
1278                                        eegMeditation = 20;
1279                                } else if (msg[1] == 0x3) {
1280                                        eegMeditation = 30;
1281                                } else if (msg[1] == 0x4) {
1282                                        eegMeditation = 40;
1283                                } else if (msg[1] == 0x5) {
1284                                        eegMeditation = 50;
1285                                } else if (msg[1] == 0x6) {
1286                                        eegMeditation = 60;
1287                                } else if (msg[1] == 0x7) {
1288                                        eegMeditation = 70;
1289                                } else if (msg[1] == 0x8) {
1290                                        eegMeditation = 80;
1291                                } else if (msg[1] == 0x9) {
1292                                        eegMeditation = 90;
1293                                } else {
1294                                        eegMeditation = 100;
1295                                }
1296                        }
1297                       
1298                       
1299//                      updateFrame(eegAttention, eegMeditation, eegSignal);
1300                        updateFrameADK();
1301                       
1302                       
1303                        // Orbit Throttle
1304                        if (msg[0] == 0x7) {
1305                               
1306                                Serial.println(" (msg[0] == 0x7) ");
1307                               
1308                                int throttle = 0;
1309                               
1310                                if (msg[1] == 0x1) {
1311                                        throttle = _throttle_hover;
1312                                        Serial.println(" (msg[1] == 0x1) ");
1313}
1314                                else if (msg[1] == 0x0)
1315                                        Serial.println(" (msg[1] == 0x0) ");
1316                               
1317                                setOrbitThrottle(throttle);
1318                       
1319                        } else {
1320                               
1321//                              sendFrame(1000);
1322                                sendFrame(0);
1323                                sendCode(formCode(_throttle,_yaw,_pitch));
1324                                delay(80); //cycle();
1325                        }
1326                       
1327//                      setColorWheel(255,0,0); // Red
1328               
1329                } // len
1330                else {
1331//                      setColorWheel(255, 0, 0); // red
1332                        delay(80); //cycle();
1333                }
1334        } // if acc.isConnected()
1335
1336
1337} // parseADK
1338
1339
1340
1341
Note: See TracBrowser for help on using the repository browser.