source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 94b751c

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