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

Last change on this file since ba86b3f was ba86b3f, checked in by Steve Castellotti <sc@…>, 7 years ago
  • manualSlide() added
  • setDefault() added
  • displaySettings() added
  • 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 PTICH_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.2",
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)
1387                        _throttle = throttleTarget;
1388               
1389                if (direction == 'f') {
1390                        _pitch = _pitch + pitchStep;
1391                        if (_pitch > pitchTarget)
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)
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//      sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1440        setColorWheel(255, 255, 0); // yellow
1441
1442       
1443} // manualSlide
1444
1445
1446// ################################################################
1447
1448void parseHop() {
1449       
1450        char inByte=0;
1451       
1452        char direction;
1453       
1454        int p1;
1455        int p2;
1456        int p3;
1457       
1458        int hopThrottle = DEFAULT_THROTTLE;
1459        int hopYaw = DEFAULT_YAW;
1460        int hopPitch = DEFAULT_PITCH;
1461       
1462        while (Serial.available() == 0);
1463        direction = Serial.read();
1464       
1465        Serial.print("Serial.read(): ");
1466        Serial.println(direction);
1467       
1468        // Set Color Pattern
1469        setColorDirection(direction);
1470       
1471        manualHop(direction, hopThrottle, hopYaw, hopPitch);
1472       
1473        setColorWheel(255, 255, 0); // yellow
1474       
1475        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1476       
1477       
1478} // parseHop()
1479
1480
1481// ################################################################
1482
1483void manualHop(char direction, int hopThrottle, int hopYaw, int hopPitch) {
1484       
1485        // Begin Hop
1486                #if DEBUG_OUTPUT
1487                        Serial.println("Hop: (Ascending)");
1488                #endif
1489        while (hopThrottle < HOP_THROTTLE_MAX) {
1490               
1491                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1492                if (hopThrottle > HOP_THROTTLE_MAX)
1493                        hopThrottle = HOP_THROTTLE_MAX;
1494               
1495                if (direction == 'f') {
1496                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1497                        if (hopPitch > HOP_PITCH_MAX)
1498                                hopPitch = HOP_PITCH_MAX;
1499                }
1500               
1501                else if (direction == 'b') {
1502                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1503                        if (hopPitch < HOP_PITCH_MIN)
1504                                hopPitch = HOP_PITCH_MIN;
1505                }
1506               
1507                else if (direction == 'l') {
1508                        hopYaw = hopYaw + HOP_YAW_STEP;
1509                        if (hopYaw > HOP_YAW_MAX)
1510                                hopYaw = HOP_YAW_MAX;
1511                }
1512               
1513                else if (direction == 'r') {
1514                        hopYaw = hopYaw - HOP_YAW_STEP;
1515                        if (hopYaw < HOP_YAW_MIN)
1516                                hopYaw = HOP_YAW_MIN;
1517                }
1518               
1519                #if DEBUG_OUTPUT
1520                        Serial.print("Sending: (");
1521                        Serial.print(hopThrottle);
1522                        Serial.print(", ");
1523                        Serial.print(hopYaw);
1524                        Serial.print(", ");
1525                        Serial.print(hopPitch);
1526                        Serial.println(")");
1527                #endif
1528               
1529                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1530                delay(80); //cycle();
1531        }
1532       
1533        // Descend
1534                #if DEBUG_OUTPUT
1535                        Serial.println("Hop: (Descending)");
1536                #endif
1537        while (hopThrottle > DEFAULT_THROTTLE) {
1538               
1539                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1540                if (hopThrottle < DEFAULT_THROTTLE)
1541                        hopThrottle = DEFAULT_THROTTLE;
1542               
1543               
1544                #if DEBUG_OUTPUT
1545                        Serial.print("Sending: (");
1546                        Serial.print(hopThrottle);
1547                        Serial.print(", ");
1548                        Serial.print(hopYaw);
1549                        Serial.print(", ");
1550                        Serial.print(hopPitch);
1551                        Serial.println(")");
1552                #endif
1553
1554               
1555                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1556                delay(80); //cycle();
1557        }
1558       
1559       
1560} // manualHop
1561
1562
1563// ################################################################
1564
1565void executeHop() {
1566       
1567        char inByte=0;
1568       
1569        char direction;
1570       
1571        int p1;
1572        int p2;
1573        int p3;
1574       
1575        int hopThrottle = DEFAULT_THROTTLE;
1576        int hopYaw = DEFAULT_YAW;
1577        int hopPitch = DEFAULT_PITCH;
1578       
1579        while (Serial.available() == 0);
1580        direction = Serial.read();
1581       
1582        Serial.print("Serial.read(): ");
1583        Serial.println(direction);
1584       
1585       
1586        // Set Color Pattern
1587       
1588        setColorWheel(0, 0, 0); // black
1589       
1590        if (direction == 'f') {
1591                #if DEBUG_OUTPUT
1592                        Serial.println("Hop: Forward");
1593                #endif
1594                p1 = 10;
1595                p2 = 11;
1596                p3 = 0;
1597        }
1598        else if (direction == 'b') {
1599                #if DEBUG_OUTPUT
1600                        Serial.println("Hop: Backwards");
1601                #endif
1602                p1 = 4;
1603                p2 = 5;
1604                p3 = 6;
1605        }
1606        else if (direction == 'l') {
1607                #if DEBUG_OUTPUT
1608                        Serial.println("Hop: Left");
1609                #endif
1610                p1 = 7;
1611                p2 = 8;
1612                p3 = 9;
1613        }
1614        else if (direction == 'r') {
1615                #if DEBUG_OUTPUT
1616                        Serial.println("Hop: Right");
1617                #endif
1618                p1 = 1;
1619                p2 = 2;
1620                p3 = 3;
1621        }
1622        else if (direction == 'u') {
1623                #if DEBUG_OUTPUT
1624                        Serial.println("Hop: Up");
1625                #endif
1626                p1 = 11;
1627                p2 = 3;
1628                p3 = 7;
1629        }
1630
1631       
1632       
1633        // White
1634        RGB_Panel[p1][0]=255;
1635        RGB_Panel[p1][1]=255;
1636        RGB_Panel[p1][2]=255;
1637       
1638        RGB_Panel[p2][0]=255;
1639        RGB_Panel[p2][1]=255;
1640        RGB_Panel[p2][2]=255;
1641       
1642        RGB_Panel[p3][0]=255;
1643        RGB_Panel[p3][1]=255;
1644        RGB_Panel[p3][2]=255;
1645       
1646       
1647        sendFrame(0);
1648       
1649       
1650       
1651        // Begin Hop
1652                #if DEBUG_OUTPUT
1653                        Serial.println("Hop: (Ascending)");
1654                #endif
1655        while (hopThrottle < HOP_THROTTLE_MAX) {
1656               
1657                hopThrottle = hopThrottle + HOP_THROTTLE_STEP;
1658                if (hopThrottle > HOP_THROTTLE_MAX)
1659                        hopThrottle = HOP_THROTTLE_MAX;
1660               
1661                if (direction == 'f') {
1662                        hopPitch = hopPitch + HOP_PITCH_STEP_FORWARD;
1663                        if (hopPitch > HOP_PITCH_MAX)
1664                                hopPitch = HOP_PITCH_MAX;
1665                }
1666               
1667                else if (direction == 'b') {
1668                        hopPitch = hopPitch - HOP_PITCH_STEP_BACKWARD;
1669                        if (hopPitch < HOP_PITCH_MIN)
1670                                hopPitch = HOP_PITCH_MIN;
1671                }
1672               
1673                else if (direction == 'l') {
1674                        hopYaw = hopYaw + HOP_YAW_STEP;
1675                        if (hopYaw > HOP_YAW_MAX)
1676                                hopYaw = HOP_YAW_MAX;
1677                }
1678               
1679                else if (direction == 'r') {
1680                        hopYaw = hopYaw - HOP_YAW_STEP;
1681                        if (hopYaw < HOP_YAW_MIN)
1682                                hopYaw = HOP_YAW_MIN;
1683                }
1684               
1685                #if DEBUG_OUTPUT
1686                        Serial.print("Sending: (");
1687                        Serial.print(hopThrottle);
1688                        Serial.print(", ");
1689                        Serial.print(hopYaw);
1690                        Serial.print(", ");
1691                        Serial.print(hopPitch);
1692                        Serial.println(")");
1693                #endif
1694               
1695                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1696                delay(80); //cycle();
1697        }
1698       
1699        // Descend
1700                #if DEBUG_OUTPUT
1701                        Serial.println("Hop: (Descending)");
1702                #endif
1703        while (hopThrottle > DEFAULT_THROTTLE) {
1704               
1705                hopThrottle = hopThrottle - HOP_THROTTLE_STEP;
1706                if (hopThrottle < DEFAULT_THROTTLE)
1707                        hopThrottle = DEFAULT_THROTTLE;
1708               
1709               
1710                #if DEBUG_OUTPUT
1711                        Serial.print("Sending: (");
1712                        Serial.print(hopThrottle);
1713                        Serial.print(", ");
1714                        Serial.print(hopYaw);
1715                        Serial.print(", ");
1716                        Serial.print(hopPitch);
1717                        Serial.println(")");
1718                #endif
1719
1720               
1721                sendCode(formCode(hopThrottle, hopYaw, hopPitch));
1722                delay(80); //cycle();
1723        }
1724       
1725       
1726        sendCode(formCode(DEFAULT_THROTTLE, DEFAULT_YAW, DEFAULT_PITCH));
1727        setColorWheel(255, 255, 0); // yellow
1728       
1729       
1730} // executeHop()
1731
1732
1733// ################################################################
1734
1735int calculateMeter(int value) {
1736
1737        int result = 0;
1738
1739        if (value == 0)
1740                result = 0;
1741        else if (value <= 20)
1742                result = 1;
1743        else if (value <= 40)
1744                result = 2;
1745        else if (value <= 60)
1746                result = 3;
1747        else if (value <= 80)
1748                result = 4;
1749        else if (value <= 100)
1750                result = 5;
1751
1752        return(result);
1753
1754} // calculateMeter
1755
1756
1757// ################################################################
1758
1759void updateFrameADK() {
1760
1761        #if DEBUG_OUTPUT
1762                Serial.print("eegSignal: ");
1763                Serial.print(eegSignal, DEC);
1764                Serial.print(" | eegAttention: ");
1765                Serial.print(eegAttention, DEC);
1766                Serial.print(" | eegMeditation: ");
1767                Serial.print(eegMeditation, DEC);
1768                Serial.print(" | eegPower: ");
1769                Serial.println(eegPower, DEC);
1770                Serial.println();
1771               
1772                Serial.print("Thottle: ");
1773                Serial.print(_throttle, DEC);
1774                Serial.print(" | Yaw: ");
1775                Serial.print(_yaw, DEC);
1776                Serial.print(" | Pitch: ");
1777                Serial.println(_pitch, DEC);
1778        #endif
1779       
1780//      setColorWheel(0, 0, 0); // black
1781       
1782       
1783        // if the signal is good enough, light 6:00 LED in green.
1784        if (eegSignal == 100) {
1785                RGB_Panel[5][0]=0;
1786                RGB_Panel[5][1]=255;
1787                RGB_Panel[5][2]=0;
1788        } else {
1789                RGB_Panel[5][0]=0;
1790                RGB_Panel[5][1]=63;
1791                RGB_Panel[5][2]=0;
1792                // The following two lines can optionally be used
1793                // to set all lights to red to indicate ADK mode
1794                // when the EEG signal quality is insufficient for processing
1795//              setColorWheel(255, 0, 0); // red
1796//              return;
1797        }
1798       
1799       
1800        int attention = calculateMeter(eegAttention);
1801        int meditation = calculateMeter(eegMeditation);
1802       
1803       
1804                //light up & dim red LED according to attention level
1805        for(int i=6; i<6+attention; i++) {
1806                RGB_Panel[i][0]=255;
1807                RGB_Panel[i][1]=0;
1808                RGB_Panel[i][2]=0;
1809        }
1810        for(int i=6+attention; i<11; i++) {
1811                RGB_Panel[i][0]=0;
1812                RGB_Panel[i][1]=0;
1813                RGB_Panel[i][2]=0;
1814        }
1815
1816        //light up & dim blue LED according to meditation level
1817        for(int i=4; i>4-meditation; i--) {
1818                RGB_Panel[i][0]=0;
1819                RGB_Panel[i][1]=0;
1820                RGB_Panel[i][2]=255;
1821        }
1822        for(int i=4-meditation; i>-1; i--) {
1823                RGB_Panel[i][0]=0;
1824                RGB_Panel[i][1]=0;
1825                RGB_Panel[i][2]=0;
1826        }
1827
1828
1829        if (eegPower > 0) {
1830                RGB_Panel[11][0]=255;
1831                RGB_Panel[11][1]=255;
1832                RGB_Panel[11][2]=255;
1833//              _throttle=_throttle_hover;
1834//              Serial.println("Throttle On");
1835        } else {
1836                RGB_Panel[11][0]=0;
1837                RGB_Panel[11][1]=0;
1838                RGB_Panel[11][2]=0;
1839//              _throttle=0;
1840//              Serial.println("Throttle Off");
1841        } // eegPower
1842       
1843       
1844        sendFrame(0);
1845
1846
1847} // updateFrameADK()
1848
1849
1850// ################################################################
1851
1852void parseADK() {
1853       
1854        // Android ADK
1855       
1856        if (acc.isConnected()) {
1857               
1858                int len = acc.read(msg, sizeof(msg), 1);
1859               
1860                if (! modeADK) {
1861                        modeADK = true;
1862                        modeBluetooth = false;
1863                        modeSerial = false;
1864                        Serial1.end();
1865                        Serial.println("INFO: parseADK connection detected, setting modeADK");
1866                        setColorWheel(255, 0, 0); // red
1867                }
1868               
1869                // Action taken by Arduino is tied to the message it receives from Android
1870               
1871                if (len > 0) {
1872                       
1873//                      Serial.println("INFO: ADK message received");
1874                       
1875                        if (msg[0] == 0x1) {
1876//                              Serial.println("0x1");
1877                                eegSignal = (int)msg[1];
1878//                              Serial.println(eegSignal);
1879                        }
1880                       
1881                        else if(msg[0] == 0x2) {
1882                                eegAttention = (int)msg[1];
1883                        }
1884                       
1885                        else if (msg[0] == 0x3) {
1886                                eegMeditation = (int)msg[1];
1887                        }
1888                       
1889                        else if (msg[0] == 0x4) {
1890                                eegPower = (int)msg[1];
1891                        }
1892                       
1893                        else if(msg[0] == 0x5) {
1894                                _throttle = (int)msg[1];
1895                        }
1896                       
1897                        else if (msg[0] == 0x6) {
1898                                _yaw = (int)msg[1];
1899                        }
1900                       
1901                        else if (msg[0] == 0x7) {
1902                                _pitch = (int)msg[1];
1903                        }
1904                       
1905                        else if (msg[0] == 0x8) {
1906                                if (msg[1] == 0x1)
1907                                        _channel = 'A';
1908                                else if (msg[1] == 0x2)
1909                                        _channel = 'B';
1910                                else if (msg[1] == 0x3)
1911                                        _channel = 'C';
1912                        }
1913                       
1914                       
1915                        // Orbit Hover
1916                        if (msg[0] == 0x9) {
1917                               
1918                                if (msg[1] == 0x1) {
1919                                        setColorWheel(255,255,255); // white
1920                                        _throttle = _throttle_hover;
1921                                        _yaw = DEFAULT_YAW;
1922                                        _pitch = DEFAULT_PITCH;
1923                                }
1924                                else if (msg[1] == 0x0) {
1925                                        setColorWheel(255,0,0); // red
1926                                        _throttle = DEFAULT_THROTTLE;
1927                                        _yaw = DEFAULT_YAW;
1928                                        _pitch = DEFAULT_PITCH;
1929                                }
1930                               
1931                        } else {
1932                                // Update the color wheel with all values if not forcing hover mode
1933                                updateFrameADK();
1934                        } // Hover
1935                       
1936               
1937                } // len
1938               
1939                sendCode(formCode(_throttle,_yaw,_pitch));
1940                delay(80); //cycle();
1941               
1942               
1943        } // if acc.isConnected()
1944
1945
1946} // parseADK
1947
1948
1949// ################################################################
1950
1951void parseSerialInput() {
1952       
1953        if (Serial.available() > 0)  {
1954               
1955                if (! modeSerial) {
1956                        Serial.println("INFO: Serial input command received, setting modeSerial");
1957                        setColorWheel(255, 255, 0);
1958                        modeSerial = true;
1959                       
1960                        Serial1.end();
1961                        modeBluetooth = false;
1962                        modeADK = false;
1963                }
1964               
1965                _command = Serial.read();
1966               
1967                #if DEBUG_OUTPUT
1968                        Serial.print("Serial.read(): ");
1969                        Serial.println(_command);
1970                #endif
1971               
1972                switch (_command) {
1973                       
1974                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1975                        case 'O': _throttle=0; setColorWheel(255,255,0); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1976                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1977                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1978                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1979                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1980                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1981                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1982                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
1983                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
1984                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
1985                        case 't':  setThrottle(); break;
1986                        case 'x':  setThrottle(); break;
1987                        case 'y':  setYaw(); break;
1988                        case 'p':  setPitch(); break;
1989                        case 'd':  setDefault(); break;
1990                        case 'v':  displaySettings(); break;
1991                        case 'c':  setColor(); break;
1992                        case 'Y':  setColorWheel(255, 255, 0); break;
1993                        case 'w':  parseColorWheel(); break;
1994                        case 's':  setStartScreen(); break;
1995                        case 'e':  executeHop(); break;
1996                        case 'h':  parseHop(); break;
1997                        case 'm':  manualSlide(); break;
1998                }
1999        }
2000
2001
2002} // parseSerialInput()
2003
2004
2005
Note: See TracBrowser for help on using the repository browser.