source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 85e090e

Last change on this file since 85e090e was 85e090e, checked in by Steve Castellotti <sc@…>, 6 years ago

Pyramid.ino:

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