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

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