source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 0bad654

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