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

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