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

Last change on this file since f5d44ec was f5d44ec, checked in by Steve Castellotti <sc@…>, 9 years ago
  • tighten up main loop
  • Property mode set to 100644
File size: 22.7 KB
Line 
1/*
2
3Puzzlebox - Pyramid - Microcontroller
4
5Puzzlebox Pyramid microcontroller sketch. This sketch allows
6you to connect the Puzzlebox Pyramid to the NeuroSky MindWave
7Mobile headset over Bluetooth, then control the
8Puzzlebox Orbit helicopter using your brainwaves.
9Choose "Arduino Mega 2560 or Mega ADK" when building and
10uploading the compiled firmware.
11
12Copyright Puzzlebox Productions, LLC (2013)
13
14This code is released under the GNU Pulic License (GPL) version 2
15
16This software is distributed in the hope that it will be useful,
17but WITHOUT ANY WARRANTY; without even the implied warranty of
18MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
19
20You should have received a copy of the GNU General Public
21License along with this code; if not, write to the Free Software
22Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
23
24For more information about this licence please refer to http://www.gnu.org/copyleft/gpl.html
25
26For more details about the product please check http://puzzlebox.info
27
28Original Author: Hao Zhang <hz@puzzlebox.info>
29
30Last Modified 2013-10-15
31by Steve Castellotti <sc@puzzlebox.info>
32
33*/
34
35#include <Wire.h> //For I2C communication with RGB LED Panel
36#include <PWM.h> //For sending 38kHz Infrared to fly Orbit helicopter
37
38
39// ADK
40// #include <Max3421e.h>
41// #include <Usb.h>
42// #include <AndroidAccessory.h>
43
44
45#define DEBUG_OUTPUT 0 // 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//G lobal variables for controlling Orbit helicopter
61// variables start with "_" means global variable
62char _throttle=0; //32~127, default 0
63char _pitch=31; //0~63, default 31
64char _yaw=78; //16~127, default 78
65char _channel='A';
66char _command;
67byte _attention_threshold = 67; //treshold for launching Orbit helicopter
68int _throttle_hover=85;
69
70// EEG
71int eegSignal = 0;
72int eegAttention = 0;
73int eegMeditation = 0;
74
75// Operation mode
76boolean modeBluetooth = false;
77boolean modeSerial = false;
78boolean modeADK = false;
79
80// MindWave Mobile Protocol
81#define BAUDRATE 57600
82#define DEBUGOUTPUT 0
83#define powercontrol 10
84
85// Checksum variables
86byte generatedChecksum = 0;
87byte checksum = 0;
88int payloadLength = 0;
89byte payloadData[64] = { 0 };
90byte poorQuality = 0;
91byte attention = 0;
92byte att_buff = 0;
93byte meditation = 0;
94
95// System variables
96long lastReceivedPacket = 0;
97long lastLoop = 0;
98long thisLoop = 0;
99boolean bigPacket = false;
100
101// Bluetooth Connection with MindWave Mobile
102String retSymb = "+RTINQ="; //start symble when there's any return
103String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
104String connectCmd = "\r\n+CONN=";
105int nameIndex = 0;
106int addrIndex = 0;
107String recvBuf;
108String slaveAddr;
109
110// RGB Panel
111byte RGB_Panel[12][3] = {
112        {0,0,0},
113        {0,0,0},
114        {0,0,0},
115        {0,0,0},
116        {0,0,0},
117        {0,0,0},
118        {0,0,0},
119        {0,0,0},
120        {0,0,0},
121        {0,0,0},
122        {0,0,0},
123        {0,0,0},
124};
125
126
127// ################################################################
128
129/////////
130//SETUP//
131/////////
132void setup() {
133       
134        pinMode(13, OUTPUT); 
135        Serial.begin(115200);
136
137        // Android ADK
138//      delay(500);
139//      acc.powerOn();
140
141       
142        ////////Orbit Flight///////////
143        // initialize all timers except for 0, to save time keeping functions
144        InitTimersSafe();
145
146        // sets the IR_frequency for the specified pin
147        bool success = SetPinFrequencySafe(IR, IR_frequency);
148
149        // if the pin IR_frequency was set successfully, turn pin 13 on
150        if(success) {
151                Serial.println("INFO: PWM pin frequency set"); 
152        }
153
154        /////////////RGB////////////////
155        Wire.begin(); // join i2c bus as master (address optional for master)
156
157       
158        setStartScreen();
159       
160       
161//      if (acc.isConnected()) {
162//              modeADK = true;
163//              Serial.println("INFO: ADK connection detected, setting modeADK");
164//              setColorWheel(255, 0, 0); // red
165//      }
166       
167       
168        //////////Bluetooth///////////
169       
170        if (! modeADK) {
171               
172//              delay(500);
173       
174                setupBlueToothConnection();
175                // wait 1s and flush the serial buffer
176                delay(1000);
177//              Serial.flush(); //delete?
178                Serial1.flush();
179       
180        }
181       
182        selectInput(); // Determine method for Pyramid control
183       
184       
185} // setup
186
187
188// ################################################################
189
190/////////////
191//MAIN LOOP//
192/////////////
193void loop() {
194       
195        if (modeBluetooth)
196                parseBluetooth();
197       
198        else if (modeSerial)
199                parseSerialInput();
200       
201//      else if (modeADK)
202//              parseADK();
203       
204       
205        // Read from button
206        if (digitalRead(BUTTON_PIN)){
207                RGB_Panel[11][0]=255;
208                RGB_Panel[11][1]=255;
209                RGB_Panel[11][2]=255;
210                digitalWrite(13,HIGH);
211        }
212        else {
213                RGB_Panel[11][0]=0;
214                RGB_Panel[11][1]=0;
215                RGB_Panel[11][2]=0;
216                digitalWrite(13,LOW);
217        }
218       
219        // Send Flight Command through IR Emitter
220        if (millis()-lastLoop>_IR_period) {
221               
222                // Add Settings for _yaw and _pitch here for custom flight path
223               
224                sendCode(formCode(_throttle,_yaw,_pitch));
225               
226                if (modeBluetooth)
227                        Serial1.flush();
228               
229                thisLoop = millis() - lastLoop;
230               
231                #if DEBUG_OUTPUT
232                        Serial.print(" Attention:");
233                        Serial.print(att_buff);
234                        Serial.print(" | Time per loop:");
235                        // Serial.println(millis() - lastLoop, DEC);
236                        Serial.println(thisLoop);
237                #endif
238                       
239                lastLoop = millis();
240       
241        } // if (millis()-lastLoop>_IR_period)
242
243} // Main loop
244
245
246// ################################################################
247
248void selectInput() {
249       
250        // Determine method for Pyramid control
251       
252        while ( (! Serial.available()) & (! Serial1.available()) );
253       
254                if (Serial.available()) {
255                        modeSerial = true;
256                        Serial.println("INFO: Serial input command received, setting modeSerial");
257                }
258               
259                else if (Serial1.available()) {
260                        modeBluetooth = true;
261                        setColorWheel(0,0,255); // Blue
262                        Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
263                }
264       
265       
266//      while ( (! Serial.available()) & (! Serial1.available()) & (! acc.isConnected()) );
267//     
268//      if (Serial.available()) {
269//              modeSerial = true;
270//              Serial.println("INFO: Serial input command received, setting modeSerial");
271//      }
272//     
273//      else if (acc.isConnected()) {
274//              modeADK = true;
275//              Serial1.end();
276//              Serial.println("INFO: ADK connection detected, setting modeADK");
277//              setColorWheel(255, 0, 0); // red
278//      }
279//     
280//      else if (Serial1.available()) {
281//              modeBluetooth = true;
282//              Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
283//      }
284       
285       
286} // selectInput
287
288
289// ################################################################
290
291void parseBluetooth() {
292       
293        // MindWave Mobile Protocol
294       
295        if(ReadOneByte() == 170) {
296               
297//              if (! modeBluetooth)
298//                      Serial.println("INFO: Bluetooth input command received, setting modeBluetooth");
299//                      modeBluetooth = true;
300               
301                if(ReadOneByte() == 170) {
302                       
303                        payloadLength = ReadOneByte();
304                        if(payloadLength > 169)                      //Payload length can not be greater than 169
305                                return;
306
307                        generatedChecksum = 0;       
308                        for(int i = 0; i < payloadLength; i++) { 
309                                payloadData[i] = ReadOneByte();            //Read payload into memory
310                                generatedChecksum += payloadData[i];
311                        }   
312
313                        checksum = ReadOneByte();                      //Read checksum byte from stream     
314                        generatedChecksum = 255 - generatedChecksum;   //Take one's compliment of generated checksum
315
316                        if (checksum == generatedChecksum) {   
317
318                                poorQuality = 200;
319                                attention = 0;
320                                meditation = 0;
321
322                                for(int i = 0; i < payloadLength; i++) {    // Parse the payload
323                                        switch (payloadData[i]) {
324                                        case 2:
325                                                i++;           
326                                                poorQuality = payloadData[i];
327                                                bigPacket = true;         
328                                                break;
329                                        case 4:
330                                                i++;
331                                                attention = payloadData[i];
332                                                break;
333                                        case 5:
334                                                i++;
335                                                meditation = payloadData[i];
336                                                break;
337                                        case 0x80:
338                                                i = i + 3;
339                                                break;
340                                        case 0x83:
341                                                i = i + 25;     
342                                                break;
343                                        default:
344                                                break;
345                                        } // switch
346                                } // for loop
347
348                               
349                                // Update RGB Panel Display
350
351                                if(bigPacket) {
352                                        Serial.print("SignalQuality: ");
353                                        Serial.print(poorQuality, DEC);
354                                        Serial.print(" Attention: ");
355                                        Serial.print(attention, DEC);
356                                        Serial.print(" Meditation: ");
357                                        Serial.print(meditation, DEC);
358                                        Serial.print(" Time since last packet: ");
359                                        Serial.print(millis() - lastReceivedPacket, DEC);
360                                        lastReceivedPacket = millis();
361                                        Serial.print("\n");
362                                       
363                                       
364                                        if (poorQuality == 200)
365                                                setColorWheel(0,255,0); // Green
366                                       
367                                       
368                                        // Update RGB Panel when received data from MindWave Mobile
369                                        int att, med;
370                                        if(attention==0) att=0;
371                                                else att=(attention-1)/20+1;
372                                        if(meditation==0) med=0;
373                                                else med=(meditation-1)/20+1;
374                                       
375                                        att_buff=attention;
376                                       
377                                        if (att_buff>_attention_threshold) {
378                                                RGB_Panel[11][0]=255;
379                                                RGB_Panel[11][1]=255;
380                                                RGB_Panel[11][2]=128;
381                                        }
382                                        else{
383                                                RGB_Panel[11][0]=0;
384                                                RGB_Panel[11][1]=0;
385                                                RGB_Panel[11][2]=0;
386                                        }
387                                       
388                                        updateFrame(att,med,poorQuality); // update buffer
389                                        sendFrame(0);// send current buffer to RGB Panel with no delay
390                                       
391                                       
392                                        if (att_buff>_attention_threshold) {
393                                               
394                                                // If detected Attention level is above the target threshold
395                                                // then send the control command to fly the helicopter.
396                                                // Otherwise send no control frames, which cause the helicopter
397                                                // to land.
398                                               
399                                                _throttle=_throttle_hover;
400                                       
401                                       
402                                        } else {
403                                               
404                                                _throttle=0;
405                                       
406                                        }
407                               
408                               
409                                } // end if(bigPacket)
410                               
411                                bigPacket = false;       
412                               
413                        } // if (checksum == generatedChecksum)
414                       
415                        else {
416                                #if DEBUG_OUTPUT
417                                        Serial.println("Checksum Error!");
418                                        // Checksum Error
419                                #endif
420                        }  // end if else for checksum
421                } // end if read 0xAA byte (170)
422        } // end if read 0xAA byte (170)
423       
424       
425} // parseBluetooth
426
427
428// ################################################################
429
430// Read data from Serial1 UART on ADK
431byte ReadOneByte() {
432       
433        int ByteRead;
434       
435        while(!Serial1.available());
436       
437        ByteRead = Serial1.read();
438       
439        return ByteRead;
440       
441}
442
443
444// ################################################################
445
446void setupBlueToothConnection() {
447       
448//      setColorWheel(0,0,255); // Blue
449
450        Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
451
452        // if you want to change baudrate, say to 115200;
453        //  Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
454        //  delay(2000); // This delay is required.
455        //  Serial1.begin(115200);
456
457        Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
458        Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
459        Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
460        Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
461        Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
462        delay(2000); // This delay is required.
463        //Serial1.flush();
464
465
466        if(digitalRead(BUTTON_PIN)==0){ //if needs pair
467                //update RGB Panel, Red Indicates it's pairing new headset
468                RGB_Panel[5][0]=255;
469                RGB_Panel[5][1]=0;
470                RGB_Panel[5][2]=0;
471                sendFrame(0);
472               
473                Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
474                Serial.println("Pyramid is inquiring!");
475                delay(2000); // This delay is required.
476               
477                //find the target slave, hence MindWave Mobile Headset
478               
479                Serial.print("print recvChar:");
480                char recvChar;
481                while(1) {
482                        if(Serial1.available()) {
483                                recvChar = Serial1.read();
484                                Serial.print(recvChar);
485                                recvBuf += recvChar;
486                                nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
487                                //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
488                                if ( nameIndex != -1 ) {
489                                        //Serial.print(recvBuf);
490                                        addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address                 
491                                        slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
492                                        break;
493                                }
494                        }
495                }
496               
497                Serial.println();
498               
499                //form the full connection command
500                connectCmd += slaveAddr;
501                connectCmd += "\r\n";
502                int connectOK = 0;
503                Serial.print("Connecting to slave:");
504                Serial.print(slaveAddr);
505                Serial.println(slaveName);
506                //connecting the slave till they are connected
507                do {
508                        Serial1.print(connectCmd);//send connection command
509                        recvBuf = "";
510                        while(1) {
511                                if(Serial1.available()) {
512                                        recvChar = Serial1.read();
513                                        recvBuf += recvChar;
514                                        if(recvBuf.indexOf("CONNECT:OK") != -1) {
515                                                connectOK = 1;
516                                                Serial.println("Connected!");
517                                                //Serial1.print("Connected!");
518                                                break;
519                                        } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
520                                                Serial.println("Connect again!");
521                                                break;
522                                        }
523                                }
524                        }
525                } while(0 == connectOK);
526               
527        } //end if needs pair
528        else { //if auto connected
529                Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
530                //update bottom RGB LED to blue
531                RGB_Panel[5][0]=0;
532                RGB_Panel[5][1]=0;
533                RGB_Panel[5][2]=255;
534                sendFrame(0);
535        }
536
537        delay(3000);
538
539        //If Bluetooth connection is established, top LED blue.
540        // Otherwise top LED red.
541        if(digitalRead(5)){//D5 for Pyramid Shield, A1 for testing
542                RGB_Panel[11][0]=0;
543                RGB_Panel[11][1]=0;
544                RGB_Panel[11][2]=255;
545                sendFrame(0);
546        }
547        else{
548                RGB_Panel[11][0]=255;
549                RGB_Panel[11][1]=0;
550                RGB_Panel[11][2]=0;
551                sendFrame(0);
552        }
553       
554}
555// End setupBluetoothConnection
556
557
558// ################################################################
559
560void updateFrame(byte attention, byte meditation, byte poorQuality) {
561        // update RGB_Panel[][] here to set next Frame you want to send
562        // For Example:
563        // RGB_Panel[0][0]=0;
564        // RGB_Panel[0][1]=0;
565        // RGB_Panel[0][2]=255;
566        // will update the LED on 1:00 position to be blue.
567
568        // This following code update RGB Panel based on data
569        // received from MindWave Mobile.
570
571        // if the signal is good enough, light 6:00 LED in green.
572        if (poorQuality <1) {
573                RGB_Panel[5][0]=0;
574                RGB_Panel[5][1]=255;
575                RGB_Panel[5][2]=0;
576        }
577        else {
578                RGB_Panel[5][0]=0;
579                RGB_Panel[5][1]=0;
580                RGB_Panel[5][2]=0;
581               
582                //Make top LED green, indicating valid bluetooth connection
583                RGB_Panel[11][0]=0;
584                RGB_Panel[11][1]=255;
585                RGB_Panel[11][2]=0;
586        }
587
588
589        //light up & dim red LED according to attention level
590        for(int i=6; i<6+attention; i++) {
591                RGB_Panel[i][0]=255;
592                RGB_Panel[i][1]=0;
593                RGB_Panel[i][2]=0;
594        }
595        for(int i=6+attention; i<11; i++) {
596                RGB_Panel[i][0]=0;
597                RGB_Panel[i][1]=0;
598                RGB_Panel[i][2]=0;
599        }
600
601        //light up & dim blue LED according to meditation level
602        for(int i=4; i>4-meditation; i--) {
603                RGB_Panel[i][0]=0;
604                RGB_Panel[i][1]=0;
605                RGB_Panel[i][2]=255;
606        }
607        for(int i=4-meditation; i>-1; i--) {
608                RGB_Panel[i][0]=0;
609                RGB_Panel[i][1]=0;
610                RGB_Panel[i][2]=0;
611        }
612
613}// end updateFrame
614
615
616// ################################################################
617
618void sendFrame(int delayTime) {
619        // Maximum bytes that can be send over I2C in one
620        // transmission is 32 bytes, since we need 36 bytes
621        // to update a full frame, we just split one frame
622        // to two frames.
623
624        //delayTime=delayTime/2;
625
626        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
627        Wire.write(0);
628        for(int i=0;i<6;i++){
629                for(int j=0;j<3;j++)
630                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
631        }
632        Wire.endTransmission();    // stop transmitting
633        //delay(delayTime);
634
635        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
636        Wire.write(18);
637        for(int i=6;i<12;i++){
638                for(int j=0;j<3;j++)
639                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
640        }
641        Wire.endTransmission();    // stop transmitting
642        //delay(delayTime);
643       
644} // sendFrame
645
646
647// ################################################################
648
649//////PWM//////
650//generate ON/OFF control signals, with starting and stopping PWM generator
651void innerCycle(int onTime, int offTime) {
652        pwmWrite(IR, ON);
653        delayMicroseconds(onTime);
654        pwmWrite(IR, OFF);
655        delayMicroseconds(offTime);
656} // innerCycle
657
658
659// ################################################################
660
661void emitCode(char BIT) {
662        //emitCode generate LOWs between HIGHs as same as the parameter.
663        if
664                (BIT) innerCycle(671,732); // 1
665        else
666                innerCycle(337,402); // 0
667       
668       
669} // emitCode
670
671
672// ################################################################
673
674void sendCode(long code) {
675        char n;
676        //starting code, with special time period.
677        innerCycle(730,392); //(773 414)
678        innerCycle(730,392);
679
680        for (n=28;n>=0;n--)
681                emitCode((code >> n) & 1); //getting bits out from code
682
683} // sendCode
684
685
686// ################################################################
687
688long formCode(char throttle,char yaw,char pitch) {
689        char n;
690        long mainCode=0;
691        int checkSum=0;
692
693        //throttle
694        for (n=6; n>=0; n--)
695                bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
696
697        bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
698
699        //channel selection first half
700        if (_channel=='C')
701                bitWrite(mainCode,15,1);
702        else
703                bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
704
705        for (n=6; n>=0; n--)  bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
706
707        //channel selection second half
708        if (_channel=='A')
709                bitWrite(mainCode,7,1);
710        else
711                bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
712
713        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
714
715        for (n=5; n>=0; n--)
716                bitWrite(mainCode,n,bitRead(pitch,n)); //pitch 
717               
718        // CheckSum
719        for (n=0; n<=20; n=n+4)
720                checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
721
722        checkSum=checkSum & B1111; // get the last 4 bits of the sum
723        checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
724
725        mainCode= (mainCode << 5) | (checkSum << 1); //get the last 4 digit of CheckSum
726
727        bitWrite(mainCode,0,1);  // finish code
728        return mainCode; // mainCode is a 29 bit binary number
729} // formCode
730
731
732// ################################################################
733
734void setThrottle() {
735
736        char inByte=0;
737        int a=0;
738        int b=0;
739        int c=0;
740        int newThrottle=0;
741
742        while (Serial.available() == 0);
743        inByte = Serial.read() - '0';
744        a = inByte;
745
746        while (Serial.available() == 0);
747        inByte = Serial.read() - '0';
748        b = inByte;
749
750        while (Serial.available() == 0);
751        inByte = Serial.read() - '0';
752        c = inByte;
753
754        newThrottle = (a * 100) + (b * 10) + c;
755
756        if (newThrottle < 0)
757                newThrottle=0;
758
759        if (newThrottle > 100)
760                newThrottle=100;
761               
762        _throttle=newThrottle;
763
764        Serial.print("_throttle=");
765        Serial.println(int(_throttle));
766
767} // setThrottle
768
769
770// ################################################################
771
772void setPitch() {
773
774        char inByte=0;
775        int a=0;
776        int b=0;
777        int c=0;
778        int newPitch=0;
779
780        while (Serial.available() == 0);
781        inByte = Serial.read() - '0';
782        //Serial.println(inByte);
783        a = inByte;
784
785        while (Serial.available() == 0);
786        inByte = Serial.read() - '0';
787        //Serial.println(inByte);
788        b = inByte;
789
790        while (Serial.available() == 0);
791        inByte = Serial.read() - '0';
792        //Serial.println(inByte);
793        c = inByte;
794
795        newPitch = (a * 100) + (b * 10) + c;
796
797        if (newPitch < 0)
798                newPitch=0;
799
800        if (newPitch > 100)
801                newPitch=100;
802               
803        _pitch=newPitch;
804
805        Serial.print("_pitch=");
806        Serial.println(int(_pitch));
807
808} // setPitch
809
810
811// ################################################################
812
813//void setThrottle() {
814//
815//      char inByte=0;
816//      int a=0;
817//      int b=0;
818//      int c=0;
819//      int newThrottle=0;
820//
821//      while (Serial.available() == 0);
822//      inByte = Serial.read() - '0';
823//      //Serial.println(inByte);
824//      a = inByte;
825//
826//      while (Serial.available() == 0);
827//      inByte = Serial.read() - '0';
828//      //Serial.println(inByte);
829//      b = inByte;
830//
831//      while (Serial.available() == 0);
832//      inByte = Serial.read() - '0';
833//      //Serial.println(inByte);
834//      c = inByte;
835//
836//      newThrottle = (a * 100) + (b * 10) + c;
837//
838//      if (newThrottle < 0)
839//              newThrottle=0;
840//
841//      if (newThrottle > 100)
842//              newThrottle=100;
843//             
844//      _throttle=newThrottle;
845//
846//      Serial.print("_throttle=");
847//      Serial.println(int(_throttle));
848//
849//} // setThrottle
850
851
852// ################################################################
853
854void setYaw() {
855
856        char inByte=0;
857        int a=0;
858        int b=0;
859        int c=0;
860        int newYaw=0;
861
862        while (Serial.available() == 0);
863        inByte = Serial.read() - '0';
864        //Serial.println(inByte);
865        a = inByte;
866
867        while (Serial.available() == 0);
868        inByte = Serial.read() - '0';
869        //Serial.println(inByte);
870        b = inByte;
871
872        while (Serial.available() == 0);
873        inByte = Serial.read() - '0';
874        //Serial.println(inByte);
875        c = inByte;
876
877        newYaw = (a * 100) + (b * 10) + c;
878
879        if (newYaw < 0)
880                newYaw=0;
881
882        if (newYaw > 100)
883                newYaw=100;
884               
885        _yaw=newYaw;
886
887        Serial.print("_yaw=");
888        Serial.println(int(_yaw));
889
890} // setYaw
891
892
893// ################################################################
894
895void setColorWheel(int red, int green, int blue) {
896       
897        for (int hour=0; hour < 12; hour++) {
898       
899                RGB_Panel[hour][0]=red;
900                RGB_Panel[hour][1]=green;
901                RGB_Panel[hour][2]=blue;
902       
903        }
904       
905        sendFrame(0);
906       
907}
908
909
910// ################################################################
911
912void setStartScreen() {
913       
914        // White
915        RGB_Panel[11][0]=255;
916        RGB_Panel[11][1]=255;
917        RGB_Panel[11][2]=255;
918       
919        RGB_Panel[3][0]=255;
920        RGB_Panel[3][1]=255;
921        RGB_Panel[3][2]=255;
922       
923        RGB_Panel[7][0]=255;
924        RGB_Panel[7][1]=255;
925        RGB_Panel[7][2]=255;
926       
927        // Red
928        for (int hour=0; hour < 3; hour++) {
929                RGB_Panel[hour][0]=255;
930                RGB_Panel[hour][1]=0;
931                RGB_Panel[hour][2]=0;
932        }
933       
934        // Green
935        for (int hour=4; hour < 7; hour++) {
936                RGB_Panel[hour][0]=0;
937                RGB_Panel[hour][1]=255;
938                RGB_Panel[hour][2]=0;
939        }
940       
941        // Blue
942        for (int hour=8; hour < 11; hour++) {
943                RGB_Panel[hour][0]=0;
944                RGB_Panel[hour][1]=0;
945                RGB_Panel[hour][2]=255;
946        }
947       
948        sendFrame(0);
949       
950}
951
952
953// ################################################################
954
955void parseSerialInput() {
956       
957        if (Serial.available() > 0)  {
958               
959                if (! modeSerial)
960                        Serial.println("INFO: Serial input command received, setting modeSerial");
961                        modeSerial = true;
962                       
963                        Serial1.end();
964                        modeBluetooth = false;
965                        modeADK = false;
966               
967               
968                setColorWheel(255, 128, 0);
969               
970               
971                _command = Serial.read();
972               
973                Serial.print("Serial.read(): ");
974                Serial.println(_command);
975               
976                switch (_command) {
977                       
978                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
979                        case 'O': _throttle=0; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
980                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
981                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
982                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
983                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
984                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
985                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
986                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
987                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
988                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
989                        case 'p':  setPitch(); break;
990                        case 't':  setThrottle(); break;
991                        case 'y':  setYaw(); break;
992                        case 'x':  setThrottle(); break;
993                }
994        }
995       
996        sendCode(formCode(_throttle,_yaw,_pitch));
997        delay(80); // originally 36, 51, 66, 80
998
999} // parseSerialInput()
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
Note: See TracBrowser for help on using the repository browser.