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

Last change on this file since b62ab59 was b62ab59, checked in by Steve Castellotti <sc@…>, 9 years ago
  • Rollback to Bluetooth/USB Serial input modes only
  • Property mode set to 100644
File size: 22.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 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                if (att_buff>_attention_threshold) {
223                       
224                        // If detected Attention level is above the target threshold
225                        // then send the control command to fly the helicopter.
226                        // Otherwise send no control frames, which cause the helicopter
227                        // to land.
228                       
229                        _throttle=_throttle_hover;
230                       
231                }
232                        // Add Settings for _yaw and _pitch here for custom flight path
233                       
234                        sendCode(formCode(_throttle,_yaw,_pitch));
235                       
236                        if (modeBluetooth)
237                                Serial1.flush();
238                       
239//              }
240               
241                thisLoop = millis() - lastLoop;
242               
243                #if DEBUG_OUTPUT
244                        Serial.print(" Attention:");
245                        Serial.print(att_buff);
246                        Serial.print(" | Time per loop:");
247                        // Serial.println(millis() - lastLoop, DEC);
248                        Serial.println(thisLoop);
249                #endif
250                       
251                lastLoop = millis();
252       
253        } // if (millis()-lastLoop>_IR_period)
254
255}// Main loop
256
257
258// ################################################################
259
260void selectInput() {
261       
262        // Determine method for Pyramid control
263       
264        while ( (! Serial.available()) & (! Serial1.available()) );
265       
266                if (Serial.available()) {
267                        modeSerial = true;
268                        Serial.println("INFO: Serial input command received, setting modeSerial");
269                }
270               
271                else if (Serial1.available()) {
272                        modeBluetooth = true;
273                        Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
274                }
275       
276       
277//      while ( (! Serial.available()) & (! Serial1.available()) & (! acc.isConnected()) );
278//     
279//      if (Serial.available()) {
280//              modeSerial = true;
281//              Serial.println("INFO: Serial input command received, setting modeSerial");
282//      }
283//     
284//      else if (acc.isConnected()) {
285//              modeADK = true;
286//              Serial1.end();
287//              Serial.println("INFO: ADK connection detected, setting modeADK");
288//              setColorWheel(255, 0, 0); // red
289//      }
290//     
291//      else if (Serial1.available()) {
292//              modeBluetooth = true;
293//              Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
294//      }
295       
296       
297} // selectInput
298
299
300// ################################################################
301
302void parseBluetooth() {
303       
304        // MindWave Mobile Protocol
305       
306        if(ReadOneByte() == 170) {
307               
308//              if (! modeBluetooth)
309//                      Serial.println("INFO: Bluetooth input command received, setting modeBluetooth");
310//                      modeBluetooth = true;
311               
312                if(ReadOneByte() == 170) {
313                       
314                        payloadLength = ReadOneByte();
315                        if(payloadLength > 169)                      //Payload length can not be greater than 169
316                                return;
317
318                        generatedChecksum = 0;       
319                        for(int i = 0; i < payloadLength; i++) { 
320                                payloadData[i] = ReadOneByte();            //Read payload into memory
321                                generatedChecksum += payloadData[i];
322                        }   
323
324                        checksum = ReadOneByte();                      //Read checksum byte from stream     
325                        generatedChecksum = 255 - generatedChecksum;   //Take one's compliment of generated checksum
326
327                        if (checksum == generatedChecksum) {   
328
329                                poorQuality = 200;
330                                attention = 0;
331                                meditation = 0;
332
333                                for(int i = 0; i < payloadLength; i++) {    // Parse the payload
334                                        switch (payloadData[i]) {
335                                        case 2:
336                                                i++;           
337                                                poorQuality = payloadData[i];
338                                                bigPacket = true;         
339                                                break;
340                                        case 4:
341                                                i++;
342                                                attention = payloadData[i];
343                                                break;
344                                        case 5:
345                                                i++;
346                                                meditation = payloadData[i];
347                                                break;
348                                        case 0x80:
349                                                i = i + 3;
350                                                break;
351                                        case 0x83:
352                                                i = i + 25;     
353                                                break;
354                                        default:
355                                                break;
356                                        } // switch
357                                } // for loop
358
359                               
360                                // Update RGB Panel Display
361
362                                if(bigPacket) {
363                                        Serial.print("SignalQuality: ");
364                                        Serial.print(poorQuality, DEC);
365                                        Serial.print(" Attention: ");
366                                        Serial.print(attention, DEC);
367                                        Serial.print(" Meditation: ");
368                                        Serial.print(meditation, DEC);
369                                        Serial.print(" Time since last packet: ");
370                                        Serial.print(millis() - lastReceivedPacket, DEC);
371                                        lastReceivedPacket = millis();
372                                        Serial.print("\n");
373                                       
374                                       
375                                        if (poorQuality == 200)
376                                                setColorWheel(0,255,0); // Green
377                                       
378                                       
379                                        // Update RGB Panel when received data from MindWave Mobile
380                                        int att, med;
381                                        if(attention==0) att=0;
382                                                else att=(attention-1)/20+1;
383                                        if(meditation==0) med=0;
384                                                else med=(meditation-1)/20+1;
385                                       
386                                        att_buff=attention;
387                                       
388                                        if (att_buff>_attention_threshold) {
389                                                RGB_Panel[11][0]=255;
390                                                RGB_Panel[11][1]=255;
391                                                RGB_Panel[11][2]=128;
392                                        }
393                                        else{
394                                                RGB_Panel[11][0]=0;
395                                                RGB_Panel[11][1]=0;
396                                                RGB_Panel[11][2]=0;
397                                        }
398                                       
399                                        updateFrame(att,med,poorQuality); // update buffer
400                                        sendFrame(0);// send current buffer to RGB Panel with no delay
401                                       
402                                       
403                                       
404                                } // end if(bigPacket)     
405                                bigPacket = false;       
406                        } // if (checksum == generatedChecksum)
407                        else {
408                                #if DEBUG_OUTPUT
409                                        Serial.println("Checksum Error!");
410                                        // Checksum Error
411                                #endif
412                        }  // end if else for checksum
413                } // end if read 0xAA byte (170)
414        } // end if read 0xAA byte (170)
415
416} // parseBluetooth
417
418
419// ################################################################
420
421// Read data from Serial1 UART on ADK
422byte ReadOneByte() {
423       
424        int ByteRead;
425       
426        while(!Serial1.available());
427       
428        ByteRead = Serial1.read();
429       
430        return ByteRead;
431       
432}
433
434
435// ################################################################
436
437void setupBlueToothConnection() {
438       
439        setColorWheel(0,0,255); // Blue
440
441        Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
442
443        // if you want to change baudrate, say to 115200;
444        //  Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
445        //  delay(2000); // This delay is required.
446        //  Serial1.begin(115200);
447
448        Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
449        Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
450        Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
451        Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
452        Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
453        delay(2000); // This delay is required.
454        //Serial1.flush();
455
456
457        if(digitalRead(BUTTON_PIN)==0){ //if needs pair
458                //update RGB Panel, Red Indicates it's pairing new headset
459                RGB_Panel[5][0]=255;
460                RGB_Panel[5][1]=0;
461                RGB_Panel[5][2]=0;
462                sendFrame(0);
463               
464                Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
465                Serial.println("Pyramid is inquiring!");
466                delay(2000); // This delay is required.
467               
468                //find the target slave, hence MindWave Mobile Headset
469               
470                Serial.print("print recvChar:");
471                char recvChar;
472                while(1) {
473                        if(Serial1.available()) {
474                                recvChar = Serial1.read();
475                                Serial.print(recvChar);
476                                recvBuf += recvChar;
477                                nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
478                                //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
479                                if ( nameIndex != -1 ) {
480                                        //Serial.print(recvBuf);
481                                        addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address                 
482                                        slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
483                                        break;
484                                }
485                        }
486                }
487               
488                Serial.println();
489               
490                //form the full connection command
491                connectCmd += slaveAddr;
492                connectCmd += "\r\n";
493                int connectOK = 0;
494                Serial.print("Connecting to slave:");
495                Serial.print(slaveAddr);
496                Serial.println(slaveName);
497                //connecting the slave till they are connected
498                do {
499                        Serial1.print(connectCmd);//send connection command
500                        recvBuf = "";
501                        while(1) {
502                                if(Serial1.available()) {
503                                        recvChar = Serial1.read();
504                                        recvBuf += recvChar;
505                                        if(recvBuf.indexOf("CONNECT:OK") != -1) {
506                                                connectOK = 1;
507                                                Serial.println("Connected!");
508                                                //Serial1.print("Connected!");
509                                                break;
510                                        } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
511                                                Serial.println("Connect again!");
512                                                break;
513                                        }
514                                }
515                        }
516                } while(0 == connectOK);
517               
518        } //end if needs pair
519        else { //if auto connected
520                Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
521                //update bottom RGB LED to blue
522                RGB_Panel[5][0]=0;
523                RGB_Panel[5][1]=0;
524                RGB_Panel[5][2]=255;
525                sendFrame(0);
526        }
527
528        delay(3000);
529
530        //If Bluetooth connection is established, top LED blue.
531        // Otherwise top LED red.
532        if(digitalRead(5)){//D5 for Pyramid Shield, A1 for testing
533                RGB_Panel[11][0]=0;
534                RGB_Panel[11][1]=0;
535                RGB_Panel[11][2]=255;
536                sendFrame(0);
537        }
538        else{
539                RGB_Panel[11][0]=255;
540                RGB_Panel[11][1]=0;
541                RGB_Panel[11][2]=0;
542                sendFrame(0);
543        }
544       
545}
546// End setupBluetoothConnection
547
548
549// ################################################################
550
551void updateFrame(byte attention, byte meditation, byte poorQuality) {
552        // update RGB_Panel[][] here to set next Frame you want to send
553        // For Example:
554        // RGB_Panel[0][0]=0;
555        // RGB_Panel[0][1]=0;
556        // RGB_Panel[0][2]=255;
557        // will update the LED on 1:00 position to be blue.
558
559        // This following code update RGB Panel based on data
560        // received from MindWave Mobile.
561
562        // if the signal is good enough, light 6:00 LED in green.
563        if (poorQuality <1) {
564                RGB_Panel[5][0]=0;
565                RGB_Panel[5][1]=255;
566                RGB_Panel[5][2]=0;
567        }
568        else {
569                RGB_Panel[5][0]=0;
570                RGB_Panel[5][1]=0;
571                RGB_Panel[5][2]=0;
572               
573                //Make top LED green, indicating valid bluetooth connection
574                RGB_Panel[11][0]=0;
575                RGB_Panel[11][1]=255;
576                RGB_Panel[11][2]=0;
577        }
578
579
580        //light up & dim red LED according to attention level
581        for(int i=6; i<6+attention; i++) {
582                RGB_Panel[i][0]=255;
583                RGB_Panel[i][1]=0;
584                RGB_Panel[i][2]=0;
585        }
586        for(int i=6+attention; i<11; i++) {
587                RGB_Panel[i][0]=0;
588                RGB_Panel[i][1]=0;
589                RGB_Panel[i][2]=0;
590        }
591
592        //light up & dim blue LED according to meditation level
593        for(int i=4; i>4-meditation; i--) {
594                RGB_Panel[i][0]=0;
595                RGB_Panel[i][1]=0;
596                RGB_Panel[i][2]=255;
597        }
598        for(int i=4-meditation; i>-1; i--) {
599                RGB_Panel[i][0]=0;
600                RGB_Panel[i][1]=0;
601                RGB_Panel[i][2]=0;
602        }
603
604}// end updateFrame
605
606
607// ################################################################
608
609void sendFrame(int delayTime) {
610        // Maximum bytes that can be send over I2C in one
611        // transmission is 32 bytes, since we need 36 bytes
612        // to update a full frame, we just split one frame
613        // to two frames.
614
615        //delayTime=delayTime/2;
616
617        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
618        Wire.write(0);
619        for(int i=0;i<6;i++){
620                for(int j=0;j<3;j++)
621                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
622        }
623        Wire.endTransmission();    // stop transmitting
624        //delay(delayTime);
625
626        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
627        Wire.write(18);
628        for(int i=6;i<12;i++){
629                for(int j=0;j<3;j++)
630                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
631        }
632        Wire.endTransmission();    // stop transmitting
633        //delay(delayTime);
634       
635} // sendFrame
636
637
638// ################################################################
639
640//////PWM//////
641//generate ON/OFF control signals, with starting and stopping PWM generator
642void innerCycle(int onTime, int offTime) {
643        pwmWrite(IR, ON);
644        delayMicroseconds(onTime);
645        pwmWrite(IR, OFF);
646        delayMicroseconds(offTime);
647} // innerCycle
648
649
650// ################################################################
651
652void emitCode(char BIT) {
653        //emitCode generate LOWs between HIGHs as same as the parameter.
654        if
655                (BIT) innerCycle(671,732); // 1
656        else
657                innerCycle(337,402); // 0
658       
659       
660} // emitCode
661
662
663// ################################################################
664
665void sendCode(long code) {
666        char n;
667        //starting code, with special time period.
668        innerCycle(730,392); //(773 414)
669        innerCycle(730,392);
670
671        for (n=28;n>=0;n--)
672                emitCode((code >> n) & 1); //getting bits out from code
673
674} // sendCode
675
676
677// ################################################################
678
679long formCode(char throttle,char yaw,char pitch) {
680        char n;
681        long mainCode=0;
682        int checkSum=0;
683
684        //throttle
685        for (n=6; n>=0; n--)
686                bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
687
688        bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
689
690        //channel selection first half
691        if (_channel=='C')
692                bitWrite(mainCode,15,1);
693        else
694                bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
695
696        for (n=6; n>=0; n--)  bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
697
698        //channel selection second half
699        if (_channel=='A')
700                bitWrite(mainCode,7,1);
701        else
702                bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
703
704        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
705
706        for (n=5; n>=0; n--)
707                bitWrite(mainCode,n,bitRead(pitch,n)); //pitch 
708               
709        // CheckSum
710        for (n=0; n<=20; n=n+4)
711                checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
712
713        checkSum=checkSum & B1111; // get the last 4 bits of the sum
714        checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
715
716        mainCode= (mainCode << 5) | (checkSum << 1); //get the last 4 digit of CheckSum
717
718        bitWrite(mainCode,0,1);  // finish code
719        return mainCode; // mainCode is a 29 bit binary number
720} // formCode
721
722
723// ################################################################
724
725void setThrottle() {
726
727        char inByte=0;
728        int a=0;
729        int b=0;
730        int c=0;
731        int newThrottle=0;
732
733        while (Serial.available() == 0);
734        inByte = Serial.read() - '0';
735        a = inByte;
736
737        while (Serial.available() == 0);
738        inByte = Serial.read() - '0';
739        b = inByte;
740
741        while (Serial.available() == 0);
742        inByte = Serial.read() - '0';
743        c = inByte;
744
745        newThrottle = (a * 100) + (b * 10) + c;
746
747        if (newThrottle < 0)
748                newThrottle=0;
749
750        if (newThrottle > 100)
751                newThrottle=100;
752               
753        _throttle=newThrottle;
754
755        Serial.print("_throttle=");
756        Serial.println(int(_throttle));
757
758} // setThrottle
759
760
761// ################################################################
762
763void setPitch() {
764
765        char inByte=0;
766        int a=0;
767        int b=0;
768        int c=0;
769        int newPitch=0;
770
771        while (Serial.available() == 0);
772        inByte = Serial.read() - '0';
773        //Serial.println(inByte);
774        a = inByte;
775
776        while (Serial.available() == 0);
777        inByte = Serial.read() - '0';
778        //Serial.println(inByte);
779        b = inByte;
780
781        while (Serial.available() == 0);
782        inByte = Serial.read() - '0';
783        //Serial.println(inByte);
784        c = inByte;
785
786        newPitch = (a * 100) + (b * 10) + c;
787
788        if (newPitch < 0)
789                newPitch=0;
790
791        if (newPitch > 100)
792                newPitch=100;
793               
794        _pitch=newPitch;
795
796        Serial.print("_pitch=");
797        Serial.println(int(_pitch));
798
799} // setPitch
800
801
802// ################################################################
803
804//void setThrottle() {
805//
806//      char inByte=0;
807//      int a=0;
808//      int b=0;
809//      int c=0;
810//      int newThrottle=0;
811//
812//      while (Serial.available() == 0);
813//      inByte = Serial.read() - '0';
814//      //Serial.println(inByte);
815//      a = inByte;
816//
817//      while (Serial.available() == 0);
818//      inByte = Serial.read() - '0';
819//      //Serial.println(inByte);
820//      b = inByte;
821//
822//      while (Serial.available() == 0);
823//      inByte = Serial.read() - '0';
824//      //Serial.println(inByte);
825//      c = inByte;
826//
827//      newThrottle = (a * 100) + (b * 10) + c;
828//
829//      if (newThrottle < 0)
830//              newThrottle=0;
831//
832//      if (newThrottle > 100)
833//              newThrottle=100;
834//             
835//      _throttle=newThrottle;
836//
837//      Serial.print("_throttle=");
838//      Serial.println(int(_throttle));
839//
840//} // setThrottle
841
842
843// ################################################################
844
845void setYaw() {
846
847        char inByte=0;
848        int a=0;
849        int b=0;
850        int c=0;
851        int newYaw=0;
852
853        while (Serial.available() == 0);
854        inByte = Serial.read() - '0';
855        //Serial.println(inByte);
856        a = inByte;
857
858        while (Serial.available() == 0);
859        inByte = Serial.read() - '0';
860        //Serial.println(inByte);
861        b = inByte;
862
863        while (Serial.available() == 0);
864        inByte = Serial.read() - '0';
865        //Serial.println(inByte);
866        c = inByte;
867
868        newYaw = (a * 100) + (b * 10) + c;
869
870        if (newYaw < 0)
871                newYaw=0;
872
873        if (newYaw > 100)
874                newYaw=100;
875               
876        _yaw=newYaw;
877
878        Serial.print("_yaw=");
879        Serial.println(int(_yaw));
880
881} // setYaw
882
883
884// ################################################################
885
886void setColorWheel(int red, int green, int blue) {
887       
888        for (int hour=0; hour < 12; hour++) {
889       
890                RGB_Panel[hour][0]=red;
891                RGB_Panel[hour][1]=green;
892                RGB_Panel[hour][2]=blue;
893       
894        }
895       
896        sendFrame(0);
897       
898}
899
900
901// ################################################################
902
903void setStartScreen() {
904       
905        // White
906        RGB_Panel[11][0]=255;
907        RGB_Panel[11][1]=255;
908        RGB_Panel[11][2]=255;
909       
910        RGB_Panel[3][0]=255;
911        RGB_Panel[3][1]=255;
912        RGB_Panel[3][2]=255;
913       
914        RGB_Panel[7][0]=255;
915        RGB_Panel[7][1]=255;
916        RGB_Panel[7][2]=255;
917       
918        // Red
919        for (int hour=0; hour < 3; hour++) {
920                RGB_Panel[hour][0]=255;
921                RGB_Panel[hour][1]=0;
922                RGB_Panel[hour][2]=0;
923        }
924       
925        // Green
926        for (int hour=4; hour < 7; hour++) {
927                RGB_Panel[hour][0]=0;
928                RGB_Panel[hour][1]=255;
929                RGB_Panel[hour][2]=0;
930        }
931       
932        // Blue
933        for (int hour=8; hour < 11; hour++) {
934                RGB_Panel[hour][0]=0;
935                RGB_Panel[hour][1]=0;
936                RGB_Panel[hour][2]=255;
937        }
938       
939        sendFrame(0);
940       
941}
942
943
944// ################################################################
945
946void parseSerialInput() {
947       
948        if (Serial.available() > 0)  {
949               
950                if (! modeSerial)
951                        Serial.println("INFO: Serial input command received, setting modeSerial");
952                        modeSerial = true;
953                       
954                        Serial1.end();
955                        modeBluetooth = false;
956                        modeADK = false;
957               
958               
959                setColorWheel(255, 128, 0);
960               
961               
962                _command = Serial.read();
963               
964                Serial.print("Serial.read(): ");
965                Serial.println(_command);
966               
967                switch (_command) {
968                       
969                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
970                        case 'O': _throttle=0; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
971                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
972                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
973                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
974                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
975                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
976                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
977                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
978                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
979                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
980                        case 'p':  setPitch(); break;
981                        case 't':  setThrottle(); break;
982                        case 'y':  setYaw(); break;
983                        case 'x':  setThrottle(); break;
984                }
985        }
986       
987        sendCode(formCode(_throttle,_yaw,_pitch));
988        delay(80); // originally 36, 51, 66, 80
989
990} // parseSerialInput()
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
Note: See TracBrowser for help on using the repository browser.