source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 7e25c48

Last change on this file since 7e25c48 was 7e25c48, checked in by Steve Castellotti <sc@…>, 7 years ago
  • fixed left and right manual slide
  • Property mode set to 100644
File size: 42.0 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 THROTTLE_MAX 127
66#define THROTTLE_MIN 0
67#define YAW_MAX 127
68#define YAW_MIN 0
69#define PITCH_MAX 63
70#define PITCH_MIN 0
71
72#define HOP_THROTTLE_MAX 80
73#define HOP_THROTTLE_STEP 12
74#define HOP_YAW_MAX 127
75#define HOP_YAW_MIN 16
76#define HOP_YAW_STEP 2
77#define HOP_PITCH_MAX 63
78#define HOP_PITCH_MIN 15
79#define HOP_PITCH_STEP_FORWARD 11
80#define HOP_PITCH_STEP_BACKWARD 3
81
82char _throttle=DEFAULT_THROTTLE; //32~127, default 0
83char _yaw=DEFAULT_YAW; //16~127, default 78
84char _pitch=DEFAULT_PITCH; //0~63, default 31
85char _channel='A';
86char _command;
87byte _attention_threshold = 66; //treshold for launching Orbit helicopter
88int _throttle_hover=85;
89
90// EEG
91int eegSignal = 0;
92int eegAttention = 0;
93int eegMeditation = 0;
94int eegPower = 0;
95
96// Operation mode
97boolean modeBluetooth = false;
98boolean modeSerial = false;
99boolean modeADK = false;
100
101// Android ADK
102AndroidAccessory acc(
103        "puzzlebox",
104        "PuzzleboxPyramid",
105        "Puzzlebox Pyramid",
106        "1.4",
107        "http://pyramid.puzzlebox.info",
108        "0000000000000000"
109);
110
111byte msg[2];
112byte led;
113
114// MindWave Mobile Protocol
115#define BAUDRATE 57600
116#define DEBUGOUTPUT 0
117#define powercontrol 10
118
119// Checksum variables
120byte generatedChecksum = 0;
121byte checksum = 0;
122int payloadLength = 0;
123byte payloadData[64] = { 0 };
124byte poorQuality = 0;
125byte attention = 0;
126byte att_buff = 0;
127byte meditation = 0;
128
129// System variables
130long lastReceivedPacket = 0;
131long lastLoop = 0;
132long thisLoop = 0;
133boolean bigPacket = false;
134boolean deviceSelected = false;
135
136// Bluetooth Connection with MindWave Mobile
137String retSymb = "+RTINQ="; //start symble when there's any return
138String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
139String connectCmd = "\r\n+CONN=";
140int nameIndex = 0;
141int addrIndex = 0;
142String recvBuf;
143String slaveAddr;
144
145// RGB Panel
146byte RGB_Panel[12][3] = {
147        {0,0,0},
148        {0,0,0},
149        {0,0,0},
150        {0,0,0},
151        {0,0,0},
152        {0,0,0},
153        {0,0,0},
154        {0,0,0},
155        {0,0,0},
156        {0,0,0},
157        {0,0,0},
158        {0,0,0},
159};
160
161
162// ################################################################
163
164/////////
165//SETUP//
166/////////
167void setup() {
168       
169        pinMode(13, OUTPUT); 
170        Serial.begin(115200);
171       
172        // Android ADK
173        delay(500);
174        acc.powerOn();
175       
176       
177        ////////Orbit Flight///////////
178        // initialize all timers except for 0, to save time keeping functions
179        InitTimersSafe();
180       
181        // sets the IR_frequency for the specified pin
182        bool success = SetPinFrequencySafe(IR, IR_frequency);
183       
184        // if the pin IR_frequency was set successfully, turn pin 13 on
185        if(success) {
186                Serial.println("INFO: PWM pin frequency set"); 
187        }
188       
189        /////////////RGB////////////////
190        Wire.begin(); // join i2c bus as master (address optional for master)
191       
192       
193        setStartScreen();
194        delay(1000); // Take a second after startup to receive any USB Serial input
195       
196       
197        if (acc.isConnected()) {
198                modeADK = true;
199                Serial.println("INFO: ADK connection detected, setting modeADK");
200                setColorWheel(255, 0, 0); // red
201        }
202       
203       
204        //////////Bluetooth///////////
205       
206        if ( (! modeADK) & (! Serial.available()) ) {
207               
208                setupBlueToothConnection();
209               
210                // wait 1s and flush the Bluetooth serial buffer
211                delay(1000);
212                Serial1.flush();
213               
214       
215        }
216       
217//      selectInput(); // Determine method for Pyramid control
218       
219       
220} // setup
221
222
223// ################################################################
224
225/////////////
226//MAIN LOOP//
227/////////////
228
229void loop() {
230       
231        if (not deviceSelected)
232                selectInput(); // Determine method for Pyramid control
233       
234        if (modeBluetooth)
235                parseBluetooth();
236       
237        else if (acc.isConnected())
238                parseADK();
239       
240        else if (modeSerial)
241                parseSerialInput();
242       
243       
244        // Read from button
245        if (digitalRead(BUTTON_PIN)){
246                RGB_Panel[11][0]=255;
247                RGB_Panel[11][1]=255;
248                RGB_Panel[11][2]=255;
249                digitalWrite(13,HIGH);
250        }
251        else {
252                digitalWrite(13,LOW);
253        }
254       
255        // Send Flight Command through IR Emitter
256        if (millis()-lastLoop>_IR_period) {
257               
258                // Add Settings for _yaw and _pitch here for custom flight path
259               
260                sendCode(formCode(_throttle,_yaw,_pitch));
261               
262                if (modeBluetooth)
263                        Serial1.flush();
264               
265                thisLoop = millis() - lastLoop;
266               
267//              #if DEBUG_OUTPUT
268//                      Serial.print(" Attention:");
269//                      Serial.print(att_buff);
270//                      Serial.print(" | Time per loop:");
271//                      // Serial.println(millis() - lastLoop, DEC);
272//                      Serial.println(thisLoop);
273//              #endif
274                       
275                lastLoop = millis();
276       
277        } // if (millis()-lastLoop>_IR_period)
278
279} // Main loop
280
281
282// ################################################################
283
284void selectInput() {
285       
286        // Determine method for Pyramid control
287       
288       
289        if (digitalRead(5)) {
290                modeBluetooth=true;
291                deviceSelected=true;
292                Serial.println("INFO: selectInput(): input received, setting modeBluetooth");
293        }
294       
295       
296        if (modeBluetooth)
297                return;
298       
299        else {
300                RGB_Panel[11][0]=0;
301                RGB_Panel[11][1]=0;
302                RGB_Panel[11][2]=0;
303               
304                RGB_Panel[3][0]=0;
305                RGB_Panel[3][1]=0;
306                RGB_Panel[3][2]=0;
307               
308                RGB_Panel[7][0]=0;
309                RGB_Panel[7][1]=0;
310                RGB_Panel[7][2]=0;
311               
312                sendFrame(0);
313        }
314       
315       
316        while ( (! Serial.available()) & (! acc.isConnected()) );
317       
318        if (Serial.available()) {
319                modeSerial = true;
320                deviceSelected = true;
321                Serial.println("INFO: Serial input command received, setting modeSerial");
322                setColorWheel(255, 128, 0);
323        }
324       
325        else if (acc.isConnected()) {
326                modeADK = true;
327                deviceSelected = true;
328                Serial1.end();
329                Serial.println("INFO: ADK connection detected, setting modeADK");
330                setColorWheel(255, 0, 0); // red
331        }
332       
333       
334} // selectInput
335
336
337// ################################################################
338
339void parseBluetooth() {
340       
341        // MindWave Mobile Protocol
342       
343        if(ReadOneByte() == 170) {
344               
345                if(ReadOneByte() == 170) {
346                       
347                        payloadLength = ReadOneByte();
348                        if(payloadLength > 169)                      //Payload length can not be greater than 169
349                                return;
350
351                        generatedChecksum = 0;       
352                        for(int i = 0; i < payloadLength; i++) { 
353                                payloadData[i] = ReadOneByte();            //Read payload into memory
354                                generatedChecksum += payloadData[i];
355                        }   
356
357                        checksum = ReadOneByte();                     //Read checksum byte from stream     
358                        generatedChecksum = 255 - generatedChecksum;  //Take one's compliment of generated checksum
359
360                        if (checksum == generatedChecksum) {   
361
362                                poorQuality = 200;
363                                attention = 0;
364                                meditation = 0;
365
366                                for(int i = 0; i < payloadLength; i++) {   // Parse the payload
367                                        switch (payloadData[i]) {
368                                        case 2:
369                                                i++;           
370                                                poorQuality = payloadData[i];
371                                                bigPacket = true;         
372                                                break;
373                                        case 4:
374                                                i++;
375                                                attention = payloadData[i];
376                                                break;
377                                        case 5:
378                                                i++;
379                                                meditation = payloadData[i];
380                                                break;
381                                        case 0x80:
382                                                i = i + 3;
383                                                break;
384                                        case 0x83:
385                                                i = i + 25;     
386                                                break;
387                                        default:
388                                                break;
389                                        } // switch
390                                } // for loop
391
392                               
393                                // Update RGB Panel Display
394
395                                if(bigPacket) {
396                                       
397                                        #if DEBUG_OUTPUT
398                                                Serial.print("SignalQuality: ");
399                                                Serial.print(poorQuality, DEC);
400                                                Serial.print(" Attention: ");
401                                                Serial.print(attention, DEC);
402                                                Serial.print(" Meditation: ");
403                                                Serial.print(meditation, DEC);
404                                                Serial.print(" Time since last packet: ");
405                                                Serial.print(millis() - lastReceivedPacket, DEC);
406                                                Serial.print("\n");
407                                        #endif
408                                        lastReceivedPacket = millis();
409                                       
410                                       
411                                        if (poorQuality != 0)
412                                                setColorWheel(0,255,0); // Green
413                                       
414                                       
415                                        // Update RGB Panel when received data from MindWave Mobile
416                                        int att, med;
417                                        if (attention==0)
418                                                att=0;
419                                        else
420                                                att=(attention-1)/20+1;
421                                       
422                                        if (meditation==0)
423                                                med=0;
424                                        else
425                                                med=(meditation-1)/20+1;
426                                       
427                                        att_buff=attention;
428                                       
429                                        if (att_buff>_attention_threshold) {
430                                                RGB_Panel[11][0]=255;
431                                                RGB_Panel[11][1]=255;
432                                                RGB_Panel[11][2]=255;
433                                        }
434                                        else {
435                                                if (poorQuality == 0) {
436                                                        RGB_Panel[11][0]=0;
437                                                        RGB_Panel[11][1]=0;
438                                                        RGB_Panel[11][2]=0;
439                                                }
440                                        }
441                                       
442                                        updateFrame(att,med,poorQuality); // update buffer
443                                        sendFrame(0);// send current buffer to RGB Panel with no delay
444                                       
445                                       
446                                        if (att_buff>_attention_threshold) {
447                                               
448                                                // If detected Attention level is above the target threshold
449                                                // then send the control command to fly the helicopter.
450                                                // Otherwise send no control frames, which cause the helicopter
451                                                // to land.
452                                               
453                                                _throttle=_throttle_hover;
454                                       
455                                       
456                                        } else {
457                                               
458                                                _throttle=0;
459                                       
460                                        }
461                               
462                               
463                                } // end if(bigPacket)
464                               
465                                bigPacket = false;       
466                               
467                        } // if (checksum == generatedChecksum)
468                       
469//                      else {
470//                              #if DEBUG_OUTPUT
471//                                      Serial.println("Checksum Error!");
472//                                      // Checksum Error
473//                              #endif
474//                      }  // end if else for checksum
475               
476                } // end if read 0xAA byte (170)
477        } // end if read 0xAA byte (170)
478       
479       
480} // parseBluetooth
481
482
483// ################################################################
484
485// Read data from Serial1 UART on ADK
486byte ReadOneByte() {
487       
488        int ByteRead;
489       
490        while(!Serial1.available());
491       
492        ByteRead = Serial1.read();
493       
494        return ByteRead;
495       
496}
497
498
499// ################################################################
500
501void setupBlueToothConnection() {
502       
503        setColorWheel(0,0,255); // Blue
504
505        Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
506
507        // if you want to change baudrate, say to 115200;
508        //  Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
509        //  delay(2000); // This delay is required.
510        //  Serial1.begin(115200);
511
512        Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
513        Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
514        Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
515        Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
516        Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
517        delay(2000); // This delay is required.
518        //Serial1.flush();
519
520
521        if(digitalRead(BUTTON_PIN)==0){ //if needs pair
522                //update RGB Panel, Red Indicates it's pairing new headset
523                RGB_Panel[5][0]=255;
524                RGB_Panel[5][1]=0;
525                RGB_Panel[5][2]=0;
526                sendFrame(0);
527               
528                Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
529                Serial.println("STATUS: Pyramid is searcing for Bluetooth devices");
530                delay(2000); // This delay is required.
531               
532                //find the target slave, hence MindWave Mobile Headset
533               
534//              Serial.print("print recvChar:");
535                char recvChar;
536                while(1) {
537                        if(Serial1.available()) {
538                                recvChar = Serial1.read();
539//                              Serial.print(recvChar);
540                                recvBuf += recvChar;
541                                nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
542                                //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
543                                if ( nameIndex != -1 ) {
544                                        //Serial.print(recvBuf);
545                                        addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address                 
546                                        slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
547                                        break;
548                                }
549                        }
550                }
551               
552                // Form the full connection command
553                connectCmd += slaveAddr;
554                connectCmd += "\r\n";
555                int connectOK = 0;
556                Serial.print("INFO: Connecting to slave:");
557                Serial.print(slaveAddr);
558                Serial.print(" '");
559                Serial.print(slaveName);
560                Serial.println("'");
561               
562                // Connect to the slaves until successful
563                do {
564                        Serial1.print(connectCmd);//send connection command
565                        recvBuf = "";
566                        while(1) {
567                                if(Serial1.available()) {
568                                        recvChar = Serial1.read();
569                                        recvBuf += recvChar;
570                                        if(recvBuf.indexOf("CONNECT:OK") != -1) {
571                                                connectOK = 1;
572                                                Serial.println("INFO: Connection Successful");
573                                                //Serial1.print("Connected!");
574                                                break;
575                                        } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
576                                                Serial.println("WARN: Connection Failed");
577                                                break;
578                                        }
579                                }
580                        }
581                } while(0 == connectOK);
582               
583        } //end if needs pair
584        else { //if auto connected
585                Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
586                //update bottom RGB LED to blue
587//              setColorWheel(0,0,0); // Black
588//              RGB_Panel[5][0]=0;
589//              RGB_Panel[5][1]=0;
590//              RGB_Panel[5][2]=255;
591//              sendFrame(0);
592        }
593
594        delay(4000);
595
596        // Determine if Bluetooth connection is established
597        if (digitalRead(5)) { //D5 for Pyramid Shield, A1 for testing
598                modeBluetooth=true;
599                Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
600        }
601       
602}
603// End setupBluetoothConnection
604
605
606// ################################################################
607
608void updateFrame(byte attention, byte meditation, byte poorQuality) {
609        // update RGB_Panel[][] here to set next Frame you want to send
610        // For Example:
611        // RGB_Panel[0][0]=0;
612        // RGB_Panel[0][1]=0;
613        // RGB_Panel[0][2]=255;
614        // will update the LED on 1:00 position to be blue.
615
616        // This following code update RGB Panel based on data
617        // received from MindWave Mobile.
618
619        // if the signal is good enough, light 6:00 LED in green.
620        if (poorQuality <1) {
621                RGB_Panel[5][0]=0;
622                RGB_Panel[5][1]=255;
623                RGB_Panel[5][2]=0;
624        } else
625                return;
626
627
628
629        //light up & dim red LED according to attention level
630        for(int i=6; i<6+attention; i++) {
631                RGB_Panel[i][0]=255;
632                RGB_Panel[i][1]=0;
633                RGB_Panel[i][2]=0;
634        }
635        for(int i=6+attention; i<11; i++) {
636                RGB_Panel[i][0]=0;
637                RGB_Panel[i][1]=0;
638                RGB_Panel[i][2]=0;
639        }
640
641        //light up & dim blue LED according to meditation level
642        for(int i=4; i>4-meditation; i--) {
643                RGB_Panel[i][0]=0;
644                RGB_Panel[i][1]=0;
645                RGB_Panel[i][2]=255;
646        }
647        for(int i=4-meditation; i>-1; i--) {
648                RGB_Panel[i][0]=0;
649                RGB_Panel[i][1]=0;
650                RGB_Panel[i][2]=0;
651        }
652
653}// end updateFrame
654
655
656// ################################################################
657
658void sendFrame(int delayTime) {
659        // Maximum bytes that can be send over I2C in one
660        // transmission is 32 bytes, since we need 36 bytes
661        // to update a full frame, we just split one frame
662        // to two frames.
663
664        //delayTime=delayTime/2;
665
666        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
667        Wire.write(0);
668        for(int i=0;i<6;i++){
669                for(int j=0;j<3;j++)
670                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
671        }
672        Wire.endTransmission();    // stop transmitting
673        //delay(delayTime);
674
675        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
676        Wire.write(18);
677        for(int i=6;i<12;i++){
678                for(int j=0;j<3;j++)
679                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
680        }
681        Wire.endTransmission();    // stop transmitting
682        //delay(delayTime);
683       
684} // sendFrame
685
686
687// ################################################################
688
689void innerCycle(int onTime, int offTime) {
690       
691        // generate ON/OFF control signals, with starting and stopping PWM generator
692       
693        pwmWrite(IR, ON);
694        delayMicroseconds(onTime);
695        pwmWrite(IR, OFF);
696        delayMicroseconds(offTime);
697       
698} // innerCycle
699
700
701// ################################################################
702
703void emitCode(char BIT) {
704       
705        // emitCode generate LOWs between HIGHs as same as the parameter.
706
707        if
708                (BIT) innerCycle(671,732); // 1
709        else
710                innerCycle(337,402); // 0
711       
712       
713} // emitCode
714
715
716// ################################################################
717
718void sendCode(long code) {
719        char n;
720        //starting code, with special time period.
721        innerCycle(730,392); //(773 414)
722        innerCycle(730,392);
723
724        for (n=28;n>=0;n--)
725                emitCode((code >> n) & 1); //getting bits out from code
726
727} // sendCode
728
729
730// ################################################################
731
732long formCode(char throttle,char yaw,char pitch) {
733        char n;
734        long mainCode=0;
735        int checkSum=0;
736
737        //throttle
738        for (n=6; n>=0; n--)
739                bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
740
741        bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
742
743        //channel selection first half
744        if (_channel=='C')
745                bitWrite(mainCode,15,1);
746        else
747                bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
748
749        for (n=6; n>=0; n--)
750                bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
751
752        //channel selection second half
753        if (_channel=='A')
754                bitWrite(mainCode,7,1);
755        else
756                bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
757
758        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
759
760        for (n=5; n>=0; n--)
761                bitWrite(mainCode,n,bitRead(pitch,n)); //pitch 
762               
763        // CheckSum
764        for (n=0; n<=20; n=n+4)
765                checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
766
767        checkSum=checkSum & B1111; // get the last 4 bits of the sum
768        checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
769
770        mainCode= (mainCode << 5) | (checkSum << 1); // get the last 4 digit of CheckSum
771
772        bitWrite(mainCode,0,1);  // finish code
773       
774        return mainCode; // mainCode is a 29 bit binary number
775       
776} // formCode
777
778
779// ################################################################
780
781void setThrottle() {
782
783        char inByte=0;
784        int a=0;
785        int b=0;
786        int c=0;
787        int newThrottle=0;
788
789        while (Serial.available() == 0);
790        inByte = Serial.read() - '0';
791        a = inByte;
792
793        while (Serial.available() == 0);
794        inByte = Serial.read() - '0';
795        b = inByte;
796
797        while (Serial.available() == 0);
798        inByte = Serial.read() - '0';
799        c = inByte;
800
801        newThrottle = (a * 100) + (b * 10) + c;
802
803        if (newThrottle < 0)
804                newThrottle=0;
805
806        if (newThrottle > 100)
807                newThrottle=100;
808               
809        _throttle=newThrottle;
810
811        Serial.print("_throttle=");
812        Serial.println(int(_throttle));
813
814} // setThrottle
815
816
817// ################################################################
818
819void setYaw() {
820
821        char inByte=0;
822        int a=0;
823        int b=0;
824        int c=0;
825        int newYaw=0;
826
827        while (Serial.available() == 0);
828        inByte = Serial.read() - '0';
829        //Serial.println(inByte);
830        a = inByte;
831
832        while (Serial.available() == 0);
833        inByte = Serial.read() - '0';
834        //Serial.println(inByte);
835        b = inByte;
836
837        while (Serial.available() == 0);
838        inByte = Serial.read() - '0';
839        //Serial.println(inByte);
840        c = inByte;
841
842        newYaw = (a * 100) + (b * 10) + c;
843
844        if (newYaw < 0)
845                newYaw=0;
846
847        if (newYaw > 100)
848                newYaw=100;
849               
850        _yaw=newYaw;
851
852        Serial.print("_yaw=");
853        Serial.println(int(_yaw));
854
855} // setYaw
856
857
858// ################################################################
859
860void setPitch() {
861
862        char inByte=0;
863        int a=0;
864        int b=0;
865        int c=0;
866        int newPitch=0;
867
868        while (Serial.available() == 0);
869        inByte = Serial.read() - '0';
870        //Serial.println(inByte);
871        a = inByte;
872
873        while (Serial.available() == 0);
874        inByte = Serial.read() - '0';
875        //Serial.println(inByte);
876        b = inByte;
877
878        while (Serial.available() == 0);
879        inByte = Serial.read() - '0';
880        //Serial.println(inByte);
881        c = inByte;
882
883        newPitch = (a * 100) + (b * 10) + c;
884
885        if (newPitch < 0)
886                newPitch=0;
887
888        if (newPitch > 100)
889                newPitch=100;
890               
891        _pitch=newPitch;
892
893        Serial.print("_pitch=");
894        Serial.println(int(_pitch));
895
896} // setPitch
897
898
899// ################################################################
900
901void setColor() {
902       
903        char inByte=0;
904       
905        int position=0;
906       
907        int p1=0;
908        int p2=0;
909       
910        int red=0;
911        int blue=0;
912        int green=0;
913       
914        int r1=0;
915        int r2=0;
916        int r3=0;
917
918        int g1=0;
919        int g2=0;
920        int g3=0;
921
922        int b1=0;
923        int b2=0;
924        int b3=0;
925       
926       
927        while (Serial.available() == 0);
928        p1 = Serial.read() - '0';
929        while (Serial.available() == 0);
930        p2 = Serial.read() - '0';
931       
932        while (Serial.available() == 0);
933        r1 = Serial.read() - '0';
934        while (Serial.available() == 0);
935        r2 = Serial.read() - '0';
936        while (Serial.available() == 0);
937        r3 = Serial.read() - '0';
938       
939        while (Serial.available() == 0);
940        g1 = Serial.read() - '0';
941        while (Serial.available() == 0);
942        g2 = Serial.read() - '0';
943        while (Serial.available() == 0);
944        g3 = Serial.read() - '0';
945       
946        while (Serial.available() == 0);
947        b1 = Serial.read() - '0';
948        while (Serial.available() == 0);
949        b2 = Serial.read() - '0';
950        while (Serial.available() == 0);
951        b3 = Serial.read() - '0';
952       
953       
954        position = (p1 * 10) + p2;
955       
956        red = (r1 * 100) + (r2 * 10) + r3;
957        green = (g1 * 100) + (g2 * 10) + g3;
958        blue = (b1 * 100) + (b2 * 10) + b3;
959       
960       
961        RGB_Panel[position][0]=red;
962        RGB_Panel[position][1]=green;
963        RGB_Panel[position][2]=blue;
964       
965        sendFrame(0);
966       
967        #if DEBUG_OUTPUT
968                Serial.print("Color(");
969                Serial.print(red);
970                Serial.print(",");
971                Serial.print(green);
972                Serial.print(",");
973                Serial.print(blue);
974                Serial.print(") - Position[");
975                Serial.print(position);
976                Serial.println("]");
977        #endif
978
979
980} // setColor()
981
982
983// ################################################################
984
985void parseColorWheel() {
986       
987        char inByte=0;
988       
989        int red=0;
990        int blue=0;
991        int green=0;
992       
993        int r1=0;
994        int r2=0;
995        int r3=0;
996
997        int g1=0;
998        int g2=0;
999        int g3=0;
1000
1001        int b1=0;
1002        int b2=0;
1003        int b3=0;
1004       
1005       
1006        while (Serial.available() == 0);
1007        r1 = Serial.read() - '0';
1008        while (Serial.available() == 0);
1009        r2 = Serial.read() - '0';
1010        while (Serial.available() == 0);
1011        r3 = Serial.read() - '0';
1012       
1013        while (Serial.available() == 0);
1014        g1 = Serial.read() - '0';
1015        while (Serial.available() == 0);
1016        g2 = Serial.read() - '0';
1017        while (Serial.available() == 0);
1018        g3 = Serial.read() - '0';
1019       
1020        while (Serial.available() == 0);
1021        b1 = Serial.read() - '0';
1022        while (Serial.available() == 0);
1023        b2 = Serial.read() - '0';
1024        while (Serial.available() == 0);
1025        b3 = Serial.read() - '0';
1026       
1027       
1028        red = (r1 * 100) + (r2 * 10) + r3;
1029        green = (g1 * 100) + (g2 * 10) + g3;
1030        blue = (b1 * 100) + (b2 * 10) + b3;
1031       
1032       
1033        setColorWheel(red,green,blue);
1034       
1035       
1036        #if DEBUG_OUTPUT
1037                Serial.print("ColorWheel(");
1038                Serial.print(red);
1039                Serial.print(",");
1040                Serial.print(green);
1041                Serial.print(",");
1042                Serial.print(blue);
1043                Serial.println(")");
1044        #endif
1045
1046} // parseColorWheel()
1047
1048
1049// ################################################################
1050
1051void setColorWheel(int red, int green, int blue) {
1052       
1053        for (int hour=0; hour < 12; hour++) {
1054       
1055                RGB_Panel[hour][0]=red;
1056                RGB_Panel[hour][1]=green;
1057                RGB_Panel[hour][2]=blue;
1058       
1059        }
1060       
1061        sendFrame(0);
1062       
1063}
1064
1065
1066// ################################################################
1067
1068void setStartScreen() {
1069       
1070        // White
1071        RGB_Panel[11][0]=255;
1072        RGB_Panel[11][1]=255;
1073        RGB_Panel[11][2]=255;
1074       
1075        RGB_Panel[3][0]=255;
1076        RGB_Panel[3][1]=255;
1077        RGB_Panel[3][2]=255;
1078       
1079        RGB_Panel[7][0]=255;
1080        RGB_Panel[7][1]=255;
1081        RGB_Panel[7][2]=255;
1082       
1083        // Red
1084        for (int hour=0; hour < 3; hour++) {
1085                RGB_Panel[hour][0]=255;
1086                RGB_Panel[hour][1]=0;
1087                RGB_Panel[hour][2]=0;
1088        }
1089       
1090        // Green
1091        for (int hour=4; hour < 7; hour++) {
1092                RGB_Panel[hour][0]=0;
1093                RGB_Panel[hour][1]=255;
1094                RGB_Panel[hour][2]=0;
1095        }
1096       
1097        // Blue
1098        for (int hour=8; hour < 11; hour++) {
1099                RGB_Panel[hour][0]=0;
1100                RGB_Panel[hour][1]=0;
1101                RGB_Panel[hour][2]=255;
1102        }
1103       
1104        sendFrame(0);
1105       
1106}
1107
1108// ################################################################
1109
1110void setColorDirection(char direction) {
1111       
1112        int p1;
1113        int p2;
1114        int p3;
1115       
1116        // Set Color Pattern
1117       
1118        setColorWheel(0, 0, 0); // black
1119       
1120        if (direction == 'f') {
1121                #if DEBUG_OUTPUT
1122                        Serial.println("Direction: Forward");
1123                #endif
1124                p1 = 10;
1125                p2 = 11;
1126                p3 = 0;
1127        }
1128        else if (direction == 'b') {
1129                #if DEBUG_OUTPUT
1130                        Serial.println("Direction: Backwards");
1131                #endif
1132                p1 = 4;
1133                p2 = 5;
1134                p3 = 6;
1135        }
1136        else if (direction == 'l') {
1137                #if DEBUG_OUTPUT
1138                        Serial.println("Direction: Left");
1139                #endif
1140                p1 = 7;
1141                p2 = 8;
1142                p3 = 9;
1143        }
1144        else if (direction == 'r') {
1145                #if DEBUG_OUTPUT
1146                        Serial.println("Direction: Right");
1147                #endif
1148                p1 = 1;
1149                p2 = 2;
1150                p3 = 3;
1151        }
1152        else if (direction == 'u') {
1153                #if DEBUG_OUTPUT
1154                        Serial.println("Direction: Up");
1155                #endif
1156                p1 = 11;
1157                p2 = 3;
1158                p3 = 7;
1159        }
1160
1161       
1162       
1163        // White
1164        RGB_Panel[p1][0]=255;
1165        RGB_Panel[p1][1]=255;
1166        RGB_Panel[p1][2]=255;
1167       
1168        RGB_Panel[p2][0]=255;
1169        RGB_Panel[p2][1]=255;
1170        RGB_Panel[p2][2]=255;
1171       
1172        RGB_Panel[p3][0]=255;
1173        RGB_Panel[p3][1]=255;
1174        RGB_Panel[p3][2]=255;
1175       
1176       
1177        sendFrame(0);
1178
1179
1180} // setColorDirection
1181
1182
1183// ################################################################
1184
1185void setDefault() {
1186       
1187        _throttle = DEFAULT_THROTTLE;
1188        _yaw = DEFAULT_YAW;
1189        _pitch = DEFAULT_PITCH;
1190       
1191        displaySettings();
1192       
1193       
1194} // setDefault()
1195
1196
1197// ################################################################
1198
1199void displaySettings() {
1200       
1201        #if DEBUG_OUTPUT
1202                Serial.println("\nSettings:");
1203                Serial.print("Throttle: ");
1204                Serial.println(_throttle, DEC);
1205                Serial.print("Yaw: ");
1206                Serial.println(_yaw, DEC);
1207                Serial.print("Pitch: ");
1208                Serial.println(_pitch, DEC);
1209        #endif
1210       
1211       
1212} // displaySettings()
1213
1214
1215// ################################################################
1216
1217void manualSlide() {
1218       
1219        char inByte=0;
1220       
1221        char direction;
1222       
1223        int flightCounter = 0;
1224       
1225        int flightTarget;
1226        int flightStep;
1227        int throttleTarget;
1228        int throttleStep;
1229        int yawTarget;
1230        int yawStep;
1231        int pitchTarget;
1232        int pitchStep;
1233       
1234        char flightTarget1;
1235        char flightTarget2;
1236        char flightTarget3;
1237       
1238        char flightStep1;
1239        char flightStep2;
1240        char flightStep3;
1241       
1242        char throttleTarget1;
1243        char throttleTarget2;
1244        char throttleTarget3;
1245       
1246        char throttleStep1;
1247        char throttleStep2;
1248        char throttleStep3;
1249       
1250        char yawTarget1;
1251        char yawTarget2;
1252        char yawTarget3;
1253       
1254        char yawStep1;
1255        char yawStep2;
1256        char yawStep3;
1257       
1258        char pitchTarget1;
1259        char pitchTarget2;
1260        char pitchTarget3;
1261       
1262        char pitchStep1;
1263        char pitchStep2;
1264        char pitchStep3;
1265       
1266        while (Serial.available() == 0);
1267        direction = Serial.read();
1268       
1269        while (Serial.available() == 0); // f
1270        Serial.read();
1271        while (Serial.available() == 0);
1272        flightTarget1 = Serial.read() - '0';
1273        while (Serial.available() == 0);
1274        flightTarget2 = Serial.read() - '0';
1275        while (Serial.available() == 0);
1276        flightTarget3 = Serial.read() - '0';
1277       
1278        while (Serial.available() == 0);
1279        flightStep1 = Serial.read() - '0';
1280        while (Serial.available() == 0);
1281        flightStep2 = Serial.read() - '0';
1282        while (Serial.available() == 0);
1283        flightStep3 = Serial.read() - '0';
1284       
1285        while (Serial.available() == 0); // t
1286        Serial.read();
1287        while (Serial.available() == 0);
1288        throttleTarget1 = Serial.read() - '0';
1289        while (Serial.available() == 0);
1290        throttleTarget2 = Serial.read() - '0';
1291        while (Serial.available() == 0);
1292        throttleTarget3 = Serial.read() - '0';
1293       
1294        while (Serial.available() == 0);
1295        throttleStep1 = Serial.read() - '0';
1296        while (Serial.available() == 0);
1297        throttleStep2 = Serial.read() - '0';
1298        while (Serial.available() == 0);
1299        throttleStep3 = Serial.read() - '0';
1300       
1301        while (Serial.available() == 0); // y
1302        Serial.read();
1303        while (Serial.available() == 0);
1304        yawTarget1 = Serial.read() - '0';
1305        while (Serial.available() == 0);
1306        yawTarget2 = Serial.read() - '0';
1307        while (Serial.available() == 0);
1308        yawTarget3 = Serial.read() - '0';
1309       
1310        while (Serial.available() == 0);
1311        yawStep1 = Serial.read() - '0';
1312        while (Serial.available() == 0);
1313        yawStep2 = Serial.read() - '0';
1314        while (Serial.available() == 0);
1315        yawStep3 = Serial.read() - '0';
1316       
1317        while (Serial.available() == 0); // p
1318        Serial.read();
1319        while (Serial.available() == 0);
1320        pitchTarget1 = Serial.read() - '0';
1321        while (Serial.available() == 0);
1322        pitchTarget2 = Serial.read() - '0';
1323        while (Serial.available() == 0);
1324        pitchTarget3 = Serial.read() - '0';
1325       
1326        while (Serial.available() == 0);
1327        pitchStep1 = Serial.read() - '0';
1328        while (Serial.available() == 0);
1329        pitchStep2 = Serial.read() - '0';
1330        while (Serial.available() == 0);
1331        pitchStep3 = Serial.read() - '0';
1332       
1333        flightTarget = (flightTarget1 * 100) + (flightTarget2 * 10) + flightTarget3;
1334        flightStep = (flightStep1 * 100) + (flightStep2 * 10) + flightStep3;
1335        throttleTarget = (throttleTarget1 * 100) + (throttleTarget2 * 10) + throttleTarget3;
1336        throttleStep = (throttleStep1 * 100) + (throttleStep2 * 10) + throttleStep3;
1337        yawTarget = (yawTarget1 * 100) + (yawTarget2 * 10) + yawTarget3;
1338        yawStep = (yawStep1 * 100) + (yawStep2 * 10) + yawStep3;
1339        pitchTarget = (pitchTarget1 * 100) + (pitchTarget2 * 10) + pitchTarget3;
1340        pitchStep = (pitchStep1 * 100) + (pitchStep2 * 10) + pitchStep3;
1341       
1342       
1343        #if DEBUG_OUTPUT
1344                Serial.println("manualSlide:");
1345                Serial.print("Throttle: ");
1346                Serial.println(_throttle);
1347                Serial.print("Yaw: ");
1348                Serial.println(_yaw);
1349                Serial.print("Pitch: ");
1350                Serial.println(_pitch);
1351                Serial.println();
1352                Serial.print("flightTarget: ");
1353                Serial.println(flightTarget);
1354                Serial.print("flightStep: ");
1355                Serial.println(flightStep);
1356                Serial.print("throttleTarget: ");
1357                Serial.println(throttleTarget);
1358                Serial.print("throttleStep: ");
1359                Serial.println(throttleStep);
1360                Serial.print("yawTarget: ");
1361                Serial.println(yawTarget);
1362                Serial.print("yawStep: ");
1363                Serial.println(yawStep);
1364                Serial.print("pitchTarget: ");
1365                Serial.println(pitchTarget);
1366                Serial.print("pitchStep: ");
1367                Serial.println(pitchStep);
1368        #endif
1369       
1370       
1371        // Set Color Pattern
1372        setColorDirection(direction);
1373       
1374       
1375        // Begin Slide
1376                #if DEBUG_OUTPUT
1377                        Serial.println("Slide: Begin");
1378                #endif
1379       
1380                       
1381        while (flightCounter < flightTarget) {
1382               
1383                flightCounter = flightCounter + flightStep;
1384               
1385                _throttle = _throttle + throttleStep;
1386                if ((_throttle > throttleTarget) || (_throttle < THROTTLE_MIN))
1387                        _throttle = throttleTarget;
1388               
1389                if (direction == 'f') {
1390                        _pitch = _pitch + pitchStep;
1391                        if ((_pitch > pitchTarget) || (_pitch < PITCH_MIN))
1392                                _pitch = pitchTarget;
1393                }
1394               
1395                else if (direction == 'b') {
1396                        _pitch = _pitch - pitchStep;
1397                        if (_pitch < pitchTarget)
1398                                _pitch = pitchTarget;
1399                }
1400               
1401                else if (direction == 'l') {
1402                        _yaw = _yaw + yawStep;
1403                        if ((_yaw > yawTarget) || (_yaw < YAW_MIN))
1404                                _yaw = yawTarget;
1405                }
1406               
1407                else if (direction == 'r') {
1408                        _yaw = _yaw - yawStep;
1409                        if (_yaw < yawTarget)
1410                                _yaw = yawTarget;
1411                }
1412               
1413               
1414                #if DEBUG_OUTPUT
1415                        Serial.print("Sending: [");
1416                        Serial.print(flightCounter);
1417                        Serial.print("/");
1418                        Serial.print(flightTarget);
1419                        Serial.print("] (");
1420                        Serial.print(_throttle, DEC);
1421                        Serial.print(", ");
1422                        Serial.print(_yaw, DEC);
1423                        Serial.print(", ");
1424                        Serial.print(_pitch, DEC);
1425                        Serial.println(")");
1426                #endif
1427               
1428                sendCode(formCode(_throttle, _yaw, _pitch));
1429                delay(80); //cycle();
1430               
1431        } // while
1432
1433       
1434        #if DEBUG_OUTPUT
1435                Serial.println("Slide: Complete");
1436        #endif
1437       
1438       
1439        setColorWheel(255, 255, 0); // yellow
1440       
1441       
1442} // manualSlide
1443
1444
1445// ################################################################
1446
1447void parseHop() {
1448       
1449        char inByte=0;
1450       
1451        char direction;
1452       
1453        int p1;
1454        int p2;
1455        int p3;
1456       
1457        int hopThrottle = DEFAULT_THROTTLE;
1458        int hopYaw = DEFAULT_YAW;
1459        int hopPitch = DEFAULT_PITCH;
1460       
1461        while (Serial.available() == 0);
1462        direction = Serial.read();
1463       
1464        Serial.print("Serial.read(): ");
1465        Serial.println(direction);
1466       
1467        // Set Color Pattern
1468        setColorDirection(direction);
1469       
1470        manualHop(direction, hopThrottle, hopYaw, hopPitch);
1471       
1472        setColorWheel(255, 255, 0); // yellow
1473       
1474        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1475       
1476       
1477} // parseHop()
1478
1479
1480// ################################################################
1481
1482void manualHop(char direction, int hopThrottle, int hopYaw, int hopPitch) {
1483       
1484        // Begin Hop
1485                #if DEBUG_OUTPUT
1486                        Serial.println("Hop: (Ascending)");
1487                #endif
1488        while (hopThrottle < HOP_THROTTLE_MAX) {
1489               
1490                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1491                if (hopThrottle > HOP_THROTTLE_MAX)
1492                        hopThrottle = HOP_THROTTLE_MAX;
1493               
1494                if (direction == 'f') {
1495                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1496                        if (hopPitch > HOP_PITCH_MAX)
1497                                hopPitch = HOP_PITCH_MAX;
1498                }
1499               
1500                else if (direction == 'b') {
1501                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1502                        if (hopPitch < HOP_PITCH_MIN)
1503                                hopPitch = HOP_PITCH_MIN;
1504                }
1505               
1506                else if (direction == 'l') {
1507                        hopYaw = hopYaw + HOP_YAW_STEP;
1508                        if (hopYaw > HOP_YAW_MAX)
1509                                hopYaw = HOP_YAW_MAX;
1510                }
1511               
1512                else if (direction == 'r') {
1513                        hopYaw = hopYaw - HOP_YAW_STEP;
1514                        if (hopYaw < HOP_YAW_MIN)
1515                                hopYaw = HOP_YAW_MIN;
1516                }
1517               
1518                #if DEBUG_OUTPUT
1519                        Serial.print("Sending: (");
1520                        Serial.print(hopThrottle);
1521                        Serial.print(", ");
1522                        Serial.print(hopYaw);
1523                        Serial.print(", ");
1524                        Serial.print(hopPitch);
1525                        Serial.println(")");
1526                #endif
1527               
1528                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1529                delay(80); //cycle();
1530        }
1531       
1532        // Descend
1533                #if DEBUG_OUTPUT
1534                        Serial.println("Hop: (Descending)");
1535                #endif
1536        while (hopThrottle > DEFAULT_THROTTLE) {
1537               
1538                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1539                if (hopThrottle < DEFAULT_THROTTLE)
1540                        hopThrottle = DEFAULT_THROTTLE;
1541               
1542               
1543                #if DEBUG_OUTPUT
1544                        Serial.print("Sending: (");
1545                        Serial.print(hopThrottle);
1546                        Serial.print(", ");
1547                        Serial.print(hopYaw);
1548                        Serial.print(", ");
1549                        Serial.print(hopPitch);
1550                        Serial.println(")");
1551                #endif
1552
1553               
1554                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1555                delay(80); //cycle();
1556        }
1557       
1558       
1559} // manualHop
1560
1561
1562// ################################################################
1563
1564void executeHop() {
1565       
1566        char inByte=0;
1567       
1568        char direction;
1569       
1570        int p1;
1571        int p2;
1572        int p3;
1573       
1574        int hopThrottle = DEFAULT_THROTTLE;
1575        int hopYaw = DEFAULT_YAW;
1576        int hopPitch = DEFAULT_PITCH;
1577       
1578        while (Serial.available() == 0);
1579        direction = Serial.read();
1580       
1581        Serial.print("Serial.read(): ");
1582        Serial.println(direction);
1583       
1584       
1585        // Set Color Pattern
1586       
1587        setColorWheel(0, 0, 0); // black
1588       
1589        if (direction == 'f') {
1590                #if DEBUG_OUTPUT
1591                        Serial.println("Hop: Forward");
1592                #endif
1593                p1 = 10;
1594                p2 = 11;
1595                p3 = 0;
1596        }
1597        else if (direction == 'b') {
1598                #if DEBUG_OUTPUT
1599                        Serial.println("Hop: Backwards");
1600                #endif
1601                p1 = 4;
1602                p2 = 5;
1603                p3 = 6;
1604        }
1605        else if (direction == 'l') {
1606                #if DEBUG_OUTPUT
1607                        Serial.println("Hop: Left");
1608                #endif
1609                p1 = 7;
1610                p2 = 8;
1611                p3 = 9;
1612        }
1613        else if (direction == 'r') {
1614                #if DEBUG_OUTPUT
1615                        Serial.println("Hop: Right");
1616                #endif
1617                p1 = 1;
1618                p2 = 2;
1619                p3 = 3;
1620        }
1621        else if (direction == 'u') {
1622                #if DEBUG_OUTPUT
1623                        Serial.println("Hop: Up");
1624                #endif
1625                p1 = 11;
1626                p2 = 3;
1627                p3 = 7;
1628        }
1629
1630       
1631       
1632        // White
1633        RGB_Panel[p1][0]=255;
1634        RGB_Panel[p1][1]=255;
1635        RGB_Panel[p1][2]=255;
1636       
1637        RGB_Panel[p2][0]=255;
1638        RGB_Panel[p2][1]=255;
1639        RGB_Panel[p2][2]=255;
1640       
1641        RGB_Panel[p3][0]=255;
1642        RGB_Panel[p3][1]=255;
1643        RGB_Panel[p3][2]=255;
1644       
1645       
1646        sendFrame(0);
1647       
1648       
1649       
1650        // Begin Hop
1651                #if DEBUG_OUTPUT
1652                        Serial.println("Hop: (Ascending)");
1653                #endif
1654        while (hopThrottle < HOP_THROTTLE_MAX) {
1655               
1656                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1657                if (hopThrottle > HOP_THROTTLE_MAX)
1658                        hopThrottle = HOP_THROTTLE_MAX;
1659               
1660                if (direction == 'f') {
1661                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1662                        if (hopPitch > HOP_PITCH_MAX)
1663                                hopPitch = HOP_PITCH_MAX;
1664                }
1665               
1666                else if (direction == 'b') {
1667                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1668                        if (hopPitch < HOP_PITCH_MIN)
1669                                hopPitch = HOP_PITCH_MIN;
1670                }
1671               
1672                else if (direction == 'l') {
1673                        hopYaw = hopYaw + HOP_YAW_STEP;
1674                        if (hopYaw > HOP_YAW_MAX)
1675                                hopYaw = HOP_YAW_MAX;
1676                }
1677               
1678                else if (direction == 'r') {
1679                        hopYaw = hopYaw - HOP_YAW_STEP;
1680                        if (hopYaw < HOP_YAW_MIN)
1681                                hopYaw = HOP_YAW_MIN;
1682                }
1683               
1684                #if DEBUG_OUTPUT
1685                        Serial.print("Sending: (");
1686                        Serial.print(hopThrottle);
1687                        Serial.print(", ");
1688                        Serial.print(hopYaw);
1689                        Serial.print(", ");
1690                        Serial.print(hopPitch);
1691                        Serial.println(")");
1692                #endif
1693               
1694                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1695                delay(80); //cycle();
1696        }
1697       
1698        // Descend
1699                #if DEBUG_OUTPUT
1700                        Serial.println("Hop: (Descending)");
1701                #endif
1702        while (hopThrottle > DEFAULT_THROTTLE) {
1703               
1704                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1705                if (hopThrottle < DEFAULT_THROTTLE)
1706                        hopThrottle = DEFAULT_THROTTLE;
1707               
1708               
1709                #if DEBUG_OUTPUT
1710                        Serial.print("Sending: (");
1711                        Serial.print(hopThrottle);
1712                        Serial.print(", ");
1713                        Serial.print(hopYaw);
1714                        Serial.print(", ");
1715                        Serial.print(hopPitch);
1716                        Serial.println(")");
1717                #endif
1718
1719               
1720                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1721                delay(80); //cycle();
1722        }
1723       
1724       
1725        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1726        setColorWheel(255, 255, 0); // yellow
1727       
1728       
1729} // executeHop()
1730
1731
1732// ################################################################
1733
1734int calculateMeter(int value) {
1735
1736        int result = 0;
1737
1738        if (value == 0)
1739                result = 0;
1740        else if (value <= 20)
1741                result = 1;
1742        else if (value <= 40)
1743                result = 2;
1744        else if (value <= 60)
1745                result = 3;
1746        else if (value <= 80)
1747                result = 4;
1748        else if (value <= 100)
1749                result = 5;
1750
1751        return(result);
1752
1753} // calculateMeter
1754
1755
1756// ################################################################
1757
1758void updateFrameADK() {
1759
1760        #if DEBUG_OUTPUT
1761                Serial.print("eegSignal: ");
1762                Serial.print(eegSignal, DEC);
1763                Serial.print(" | eegAttention: ");
1764                Serial.print(eegAttention, DEC);
1765                Serial.print(" | eegMeditation: ");
1766                Serial.print(eegMeditation, DEC);
1767                Serial.print(" | eegPower: ");
1768                Serial.println(eegPower, DEC);
1769                Serial.println();
1770               
1771                Serial.print("Thottle: ");
1772                Serial.print(_throttle, DEC);
1773                Serial.print(" | Yaw: ");
1774                Serial.print(_yaw, DEC);
1775                Serial.print(" | Pitch: ");
1776                Serial.println(_pitch, DEC);
1777        #endif
1778       
1779//      setColorWheel(0, 0, 0); // black
1780       
1781       
1782        // if the signal is good enough, light 6:00 LED in green.
1783        if (eegSignal == 100) {
1784                RGB_Panel[5][0]=0;
1785                RGB_Panel[5][1]=255;
1786                RGB_Panel[5][2]=0;
1787        } else {
1788                RGB_Panel[5][0]=0;
1789                RGB_Panel[5][1]=63;
1790                RGB_Panel[5][2]=0;
1791                // The following two lines can optionally be used
1792                // to set all lights to red to indicate ADK mode
1793                // when the EEG signal quality is insufficient for processing
1794//              setColorWheel(255, 0, 0); // red
1795//              return;
1796        }
1797       
1798       
1799        int attention = calculateMeter(eegAttention);
1800        int meditation = calculateMeter(eegMeditation);
1801       
1802       
1803                //light up & dim red LED according to attention level
1804        for(int i=6; i<6+attention; i++) {
1805                RGB_Panel[i][0]=255;
1806                RGB_Panel[i][1]=0;
1807                RGB_Panel[i][2]=0;
1808        }
1809        for(int i=6+attention; i<11; i++) {
1810                RGB_Panel[i][0]=0;
1811                RGB_Panel[i][1]=0;
1812                RGB_Panel[i][2]=0;
1813        }
1814
1815        //light up & dim blue LED according to meditation level
1816        for(int i=4; i>4-meditation; i--) {
1817                RGB_Panel[i][0]=0;
1818                RGB_Panel[i][1]=0;
1819                RGB_Panel[i][2]=255;
1820        }
1821        for(int i=4-meditation; i>-1; i--) {
1822                RGB_Panel[i][0]=0;
1823                RGB_Panel[i][1]=0;
1824                RGB_Panel[i][2]=0;
1825        }
1826
1827
1828        if (eegPower > 0) {
1829                RGB_Panel[11][0]=255;
1830                RGB_Panel[11][1]=255;
1831                RGB_Panel[11][2]=255;
1832//              _throttle=_throttle_hover;
1833//              Serial.println("Throttle On");
1834        } else {
1835                RGB_Panel[11][0]=0;
1836                RGB_Panel[11][1]=0;
1837                RGB_Panel[11][2]=0;
1838//              _throttle=0;
1839//              Serial.println("Throttle Off");
1840        } // eegPower
1841       
1842       
1843        sendFrame(0);
1844
1845
1846} // updateFrameADK()
1847
1848
1849// ################################################################
1850
1851void parseADK() {
1852       
1853        // Android ADK
1854       
1855        if (acc.isConnected()) {
1856               
1857                int len = acc.read(msg, sizeof(msg), 1);
1858               
1859                if (! modeADK) {
1860                        modeADK = true;
1861                        modeBluetooth = false;
1862                        modeSerial = false;
1863                        Serial1.end();
1864                        Serial.println("INFO: parseADK connection detected, setting modeADK");
1865                        setColorWheel(255, 0, 0); // red
1866                }
1867               
1868                // Action taken by Arduino is tied to the message it receives from Android
1869               
1870                if (len > 0) {
1871                       
1872//                      Serial.println("INFO: ADK message received");
1873                       
1874                        if (msg[0] == 0x1) {
1875//                              Serial.println("0x1");
1876                                eegSignal = (int)msg[1];
1877//                              Serial.println(eegSignal);
1878                        }
1879                       
1880                        else if(msg[0] == 0x2) {
1881                                eegAttention = (int)msg[1];
1882                        }
1883                       
1884                        else if (msg[0] == 0x3) {
1885                                eegMeditation = (int)msg[1];
1886                        }
1887                       
1888                        else if (msg[0] == 0x4) {
1889                                eegPower = (int)msg[1];
1890                        }
1891                       
1892                        else if(msg[0] == 0x5) {
1893                                _throttle = (int)msg[1];
1894                        }
1895                       
1896                        else if (msg[0] == 0x6) {
1897                                _yaw = (int)msg[1];
1898                        }
1899                       
1900                        else if (msg[0] == 0x7) {
1901                                _pitch = (int)msg[1];
1902                        }
1903                       
1904                        else if (msg[0] == 0x8) {
1905                                if (msg[1] == 0x1)
1906                                        _channel = 'A';
1907                                else if (msg[1] == 0x2)
1908                                        _channel = 'B';
1909                                else if (msg[1] == 0x3)
1910                                        _channel = 'C';
1911                        }
1912                       
1913                       
1914                        // Orbit Hover
1915                        if (msg[0] == 0x9) {
1916                               
1917                                if (msg[1] == 0x1) {
1918                                        setColorWheel(255,255,255); // white
1919                                        _throttle = _throttle_hover;
1920                                        _yaw = DEFAULT_YAW;
1921                                        _pitch = DEFAULT_PITCH;
1922                                }
1923                                else if (msg[1] == 0x0) {
1924                                        setColorWheel(255,0,0); // red
1925                                        _throttle = DEFAULT_THROTTLE;
1926                                        _yaw = DEFAULT_YAW;
1927                                        _pitch = DEFAULT_PITCH;
1928                                }
1929                               
1930                        } else {
1931                                // Update the color wheel with all values if not forcing hover mode
1932                                updateFrameADK();
1933                        } // Hover
1934                       
1935               
1936                } // len
1937               
1938                sendCode(formCode(_throttle,_yaw,_pitch));
1939                delay(80); //cycle();
1940               
1941               
1942        } // if acc.isConnected()
1943
1944
1945} // parseADK
1946
1947
1948// ################################################################
1949
1950void parseSerialInput() {
1951       
1952        if (Serial.available() > 0)  {
1953               
1954                if (! modeSerial) {
1955                        Serial.println("INFO: Serial input command received, setting modeSerial");
1956                        setColorWheel(255, 255, 0);
1957                        modeSerial = true;
1958                       
1959                        Serial1.end();
1960                        modeBluetooth = false;
1961                        modeADK = false;
1962                }
1963               
1964                _command = Serial.read();
1965               
1966                #if DEBUG_OUTPUT
1967                        Serial.print("Serial.read(): ");
1968                        Serial.println(_command);
1969                #endif
1970               
1971                switch (_command) {
1972                       
1973                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1974                        case 'O': _throttle=0; setColorWheel(255,255,0); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1975                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1976                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1977                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1978                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1979                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1980                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1981                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
1982                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
1983                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
1984                        case 't':  setThrottle(); break;
1985                        case 'x':  setThrottle(); break;
1986                        case 'y':  setYaw(); break;
1987                        case 'p':  setPitch(); break;
1988                        case 'd':  setDefault(); break;
1989                        case 'v':  displaySettings(); break;
1990                        case 'c':  setColor(); break;
1991                        case 'Y':  setColorWheel(255, 255, 0); break;
1992                        case 'w':  parseColorWheel(); break;
1993                        case 's':  setStartScreen(); break;
1994                        case 'e':  executeHop(); break;
1995                        case 'h':  parseHop(); break;
1996                        case 'm':  manualSlide(); break;
1997                }
1998        }
1999
2000
2001} // parseSerialInput()
2002
Note: See TracBrowser for help on using the repository browser.