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

Last change on this file since dbfae54 was dbfae54, checked in by Steve Castellotti <sc@…>, 9 years ago
  • parseADK() re-enabled
  • Property mode set to 100644
File size: 28.5 KB
RevLine 
[fd3931e]1/*
[240e339]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
[0b675ba]28Original Author: Hao Zhang <hz@puzzlebox.info>
[240e339]29
[85e090e]30Last Modified 2013-10-15
[240e339]31by Steve Castellotti <sc@puzzlebox.info>
32
[fd3931e]33*/
[240e339]34
[fd3931e]35#include <Wire.h> //For I2C communication with RGB LED Panel
36#include <PWM.h> //For sending 38kHz Infrared to fly Orbit helicopter
37
[b62ab59]38
[f1b827c]39// ADK
[77a80d3]40#include <Max3421e.h>
41#include <Usb.h>
42#include <AndroidAccessory.h>
[240e339]43
[fd3931e]44
[f1b827c]45#define DEBUG_OUTPUT 0 // 1 for debug
[fd3931e]46
47
[f1b827c]48// Orbit Flight
[fd3931e]49#define IR 6   
50#define ON 128
51#define OFF 0
52
[f1b827c]53// Push Button
54#define BUTTON_PIN 4
55
[b62ab59]56//PWM IR_frequency (in Hz) required by IR
57#define IR_frequency 38400
[f1b827c]58byte _IR_period = 40;
[fd3931e]59
[b62ab59]60//G lobal variables for controlling Orbit helicopter
[f1b827c]61// variables start with "_" means global variable
[fd3931e]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;
[0b675ba]67byte _attention_threshold = 67; //treshold for launching Orbit helicopter
[b62ab59]68int _throttle_hover=85;
[fd3931e]69
[5202e8d]70// EEG
71int eegSignal = 0;
72int eegAttention = 0;
73int eegMeditation = 0;
74
[94b751c]75// Operation mode
76boolean modeBluetooth = false;
77boolean modeSerial = false;
78boolean modeADK = false;
79
[77a80d3]80// Android ADK
[dbfae54]81AndroidAccessory acc(
82        "puzzlebox",
83        "PuzzleboxPyramid",
84        "Puzzlebox Pyramid",
85        "1.2",
86        "http://pyramid.puzzlebox.info",
87        "0000000000000000"
88);
89
[77a80d3]90// AndroidAccessory acc(
91//      "puzzlebox",
[dbfae54]92//      "TriggerRemote",
93//      "DemoKit Arduino Board",
94//      "1.0",
95//      "http://www.android.com",
[77a80d3]96//      "0000000012345678"
97// );
98
99byte msg[2];
100byte led;
101
[f1b827c]102// MindWave Mobile Protocol
[fd3931e]103#define BAUDRATE 57600
104#define DEBUGOUTPUT 0
105#define powercontrol 10
106
[f1b827c]107// Checksum variables
[fd3931e]108byte generatedChecksum = 0;
109byte checksum = 0;
110int payloadLength = 0;
[f1b827c]111byte payloadData[64] = { 0 };
[fd3931e]112byte poorQuality = 0;
113byte attention = 0;
114byte att_buff = 0;
115byte meditation = 0;
116
[f1b827c]117// System variables
[fd3931e]118long lastReceivedPacket = 0;
119long lastLoop = 0;
[94b751c]120long thisLoop = 0;
[fd3931e]121boolean bigPacket = false;
122
[f1b827c]123// Bluetooth Connection with MindWave Mobile
[b62ab59]124String retSymb = "+RTINQ="; //start symble when there's any return
125String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
126String connectCmd = "\r\n+CONN=";
[fd3931e]127int nameIndex = 0;
128int addrIndex = 0;
129String recvBuf;
130String slaveAddr;
131
[f1b827c]132// RGB Panel
[0b675ba]133byte RGB_Panel[12][3] = {
[240e339]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        {0,0,0},
145        {0,0,0},
146};
147
148
149// ################################################################
150
[fd3931e]151/////////
152//SETUP//
153/////////
[0b675ba]154void setup() {
[f1b827c]155       
[240e339]156        pinMode(13, OUTPUT); 
157        Serial.begin(115200);
[77a80d3]158       
[f1b827c]159        // Android ADK
[dbfae54]160        delay(500);
161        acc.powerOn();
[77a80d3]162       
[b62ab59]163       
[240e339]164        ////////Orbit Flight///////////
[b62ab59]165        // initialize all timers except for 0, to save time keeping functions
[240e339]166        InitTimersSafe();
[77a80d3]167       
[b62ab59]168        // sets the IR_frequency for the specified pin
[240e339]169        bool success = SetPinFrequencySafe(IR, IR_frequency);
[77a80d3]170       
[b62ab59]171        // if the pin IR_frequency was set successfully, turn pin 13 on
172        if(success) {
[5202e8d]173                Serial.println("INFO: PWM pin frequency set"); 
[240e339]174        }
[77a80d3]175       
[240e339]176        /////////////RGB////////////////
177        Wire.begin(); // join i2c bus as master (address optional for master)
[77a80d3]178       
[b62ab59]179       
[5202e8d]180        setStartScreen();
[77a80d3]181        delay(1000); // Take a second after startup to receive any USB Serial input
[b62ab59]182       
183       
[dbfae54]184        if (acc.isConnected()) {
185                modeADK = true;
186                Serial.println("INFO: ADK connection detected, setting modeADK");
187                setColorWheel(255, 0, 0); // red
188        }
[b62ab59]189       
190       
[240e339]191        //////////Bluetooth///////////
[b62ab59]192       
[77a80d3]193        if ( (! modeADK) & (! Serial.available()) ) {
[5202e8d]194               
[b62ab59]195//              delay(500);
[77a80d3]196               
[b62ab59]197                setupBlueToothConnection();
[77a80d3]198               
199                // wait 1s and flush the Bluetooth serial buffer
[f1b827c]200                delay(1000);
201                Serial1.flush();
[94b751c]202       
[b62ab59]203        }
[94b751c]204       
205        selectInput(); // Determine method for Pyramid control
206       
207       
208} // setup
[240e339]209
210
211// ################################################################
[fd3931e]212
213/////////////
214//MAIN LOOP//
215/////////////
[240e339]216void loop() {
217       
[dbfae54]218//      if (acc.isConnected()) {
219//              modeADK = true;
220//              Serial1.end();
221//              Serial.println("INFO: [LOOP] ADK connection detected, setting modeADK");
222//              setColorWheel(255, 0, 0); // red
223//      }
224       
225//      if (modeADK)
226        if (acc.isConnected())
227                parseADK();
[94b751c]228       
229        else if (modeSerial)
230                parseSerialInput();
[0b675ba]231       
[dbfae54]232        else if (modeBluetooth)
233                parseBluetooth();
234       
[b62ab59]235//      else if (modeADK)
236//              parseADK();
[f1b827c]237       
[94b751c]238       
[b62ab59]239        // Read from button
[94b751c]240        if (digitalRead(BUTTON_PIN)){
241                RGB_Panel[11][0]=255;
242                RGB_Panel[11][1]=255;
243                RGB_Panel[11][2]=255;
244                digitalWrite(13,HIGH);
245        }
246        else {
[77a80d3]247//              RGB_Panel[11][0]=0;
248//              RGB_Panel[11][1]=0;
249//              RGB_Panel[11][2]=0;
[94b751c]250                digitalWrite(13,LOW);
251        }
[0b675ba]252       
[b62ab59]253        // Send Flight Command through IR Emitter
[0b675ba]254        if (millis()-lastLoop>_IR_period) {
[94b751c]255               
[f5d44ec]256                // Add Settings for _yaw and _pitch here for custom flight path
257               
258                sendCode(formCode(_throttle,_yaw,_pitch));
259               
260                if (modeBluetooth)
261                        Serial1.flush();
[0b675ba]262               
[94b751c]263                thisLoop = millis() - lastLoop;
264               
[0b675ba]265                #if DEBUG_OUTPUT
[94b751c]266                        Serial.print(" Attention:");
267                        Serial.print(att_buff);
268                        Serial.print(" | Time per loop:");
269                        // Serial.println(millis() - lastLoop, DEC);
270                        Serial.println(thisLoop);
[0b675ba]271                #endif
[b62ab59]272                       
[94b751c]273                lastLoop = millis();
[0b675ba]274       
275        } // if (millis()-lastLoop>_IR_period)
[240e339]276
[f5d44ec]277} // Main loop
[240e339]278
[0b675ba]279
280// ################################################################
281
[94b751c]282void selectInput() {
283       
284        // Determine method for Pyramid control
285       
[dbfae54]286//      while ( (! Serial.available()) & (! Serial1.available()) );
287//     
288//              if (Serial.available()) {
289//                      modeSerial = true;
290//                      Serial.println("INFO: Serial input command received, setting modeSerial");
291//              }
292//             
293//              else if (Serial1.available()) {
294//                      modeBluetooth = true;
295//                      setColorWheel(0,0,255); // Blue
296//                      Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
297//              }
[f1b827c]298       
299       
[dbfae54]300        while ( (! Serial.available()) & (! Serial1.available()) & (! acc.isConnected()) );
[5202e8d]301       
[dbfae54]302        if (Serial.available()) {
303                modeSerial = true;
304                Serial.println("INFO: Serial input command received, setting modeSerial");
305        }
306       
307        else if (acc.isConnected()) {
308                modeADK = true;
309                Serial1.end();
310                Serial.println("INFO: ADK connection detected, setting modeADK");
311                setColorWheel(255, 0, 0); // red
312        }
313       
314        else if (Serial1.available()) {
315                modeBluetooth = true;
316                Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
317        }
[94b751c]318       
319       
320} // selectInput
321
322
323// ################################################################
324
[0b675ba]325void parseBluetooth() {
326       
[b62ab59]327        // MindWave Mobile Protocol
[0b675ba]328       
329        if(ReadOneByte() == 170) {
330               
[f1b827c]331//              if (! modeBluetooth)
332//                      Serial.println("INFO: Bluetooth input command received, setting modeBluetooth");
333//                      modeBluetooth = true;
[94b751c]334               
[0b675ba]335                if(ReadOneByte() == 170) {
336                       
[240e339]337                        payloadLength = ReadOneByte();
338                        if(payloadLength > 169)                      //Payload length can not be greater than 169
339                                return;
340
341                        generatedChecksum = 0;       
342                        for(int i = 0; i < payloadLength; i++) { 
343                                payloadData[i] = ReadOneByte();            //Read payload into memory
344                                generatedChecksum += payloadData[i];
345                        }   
346
347                        checksum = ReadOneByte();                      //Read checksum byte from stream     
348                        generatedChecksum = 255 - generatedChecksum;   //Take one's compliment of generated checksum
349
[0b675ba]350                        if (checksum == generatedChecksum) {   
[240e339]351
352                                poorQuality = 200;
353                                attention = 0;
354                                meditation = 0;
355
356                                for(int i = 0; i < payloadLength; i++) {    // Parse the payload
357                                        switch (payloadData[i]) {
358                                        case 2:
359                                                i++;           
360                                                poorQuality = payloadData[i];
361                                                bigPacket = true;         
362                                                break;
363                                        case 4:
364                                                i++;
365                                                attention = payloadData[i];
366                                                break;
367                                        case 5:
368                                                i++;
369                                                meditation = payloadData[i];
370                                                break;
371                                        case 0x80:
372                                                i = i + 3;
373                                                break;
374                                        case 0x83:
375                                                i = i + 25;     
376                                                break;
377                                        default:
378                                                break;
379                                        } // switch
380                                } // for loop
381
[b62ab59]382                               
383                                // Update RGB Panel Display
[240e339]384
385                                if(bigPacket) {
[94b751c]386                                        Serial.print("SignalQuality: ");
[240e339]387                                        Serial.print(poorQuality, DEC);
388                                        Serial.print(" Attention: ");
389                                        Serial.print(attention, DEC);
390                                        Serial.print(" Meditation: ");
391                                        Serial.print(meditation, DEC);
392                                        Serial.print(" Time since last packet: ");
393                                        Serial.print(millis() - lastReceivedPacket, DEC);
394                                        lastReceivedPacket = millis();
395                                        Serial.print("\n");
[b62ab59]396                                       
[240e339]397                                       
[77a80d3]398//                                      if (poorQuality == 200)
399                                        if (poorQuality != 0)
[5202e8d]400                                                setColorWheel(0,255,0); // Green
[77a80d3]401//                                              return;
[b62ab59]402                                       
403                                       
404                                        // Update RGB Panel when received data from MindWave Mobile
[240e339]405                                        int att, med;
[77a80d3]406                                        if (attention==0)
407                                                att=0;
408                                        else
409                                                att=(attention-1)/20+1;
410                                       
411                                        if (meditation==0)
412                                                med=0;
413                                        else
414                                                med=(meditation-1)/20+1;
[240e339]415                                       
416                                        att_buff=attention;
417                                       
418                                        if (att_buff>_attention_threshold) {
419                                                RGB_Panel[11][0]=255;
420                                                RGB_Panel[11][1]=255;
421                                                RGB_Panel[11][2]=128;
422                                        }
[77a80d3]423                                        else {
424                                                if (poorQuality == 0) {
425                                                        RGB_Panel[11][0]=0;
426                                                        RGB_Panel[11][1]=0;
427                                                        RGB_Panel[11][2]=0;
428                                                }
[240e339]429                                        }
430                                       
[b62ab59]431                                        updateFrame(att,med,poorQuality); // update buffer
432                                        sendFrame(0);// send current buffer to RGB Panel with no delay
[240e339]433                                       
[0b675ba]434                                       
[f5d44ec]435                                        if (att_buff>_attention_threshold) {
436                                               
437                                                // If detected Attention level is above the target threshold
438                                                // then send the control command to fly the helicopter.
439                                                // Otherwise send no control frames, which cause the helicopter
440                                                // to land.
441                                               
442                                                _throttle=_throttle_hover;
443                                       
444                                       
445                                        } else {
446                                               
447                                                _throttle=0;
[0b675ba]448                                       
[f5d44ec]449                                        }
450                               
451                               
452                                } // end if(bigPacket)
453                               
[0b675ba]454                                bigPacket = false;       
[f5d44ec]455                               
[0b675ba]456                        } // if (checksum == generatedChecksum)
[f5d44ec]457                       
[0b675ba]458                        else {
[240e339]459                                #if DEBUG_OUTPUT
[94b751c]460                                        Serial.println("Checksum Error!");
461                                        // Checksum Error
[240e339]462                                #endif
463                        }  // end if else for checksum
464                } // end if read 0xAA byte (170)
[0b675ba]465        } // end if read 0xAA byte (170)
[f5d44ec]466       
467       
[0b675ba]468} // parseBluetooth
[240e339]469
470
471// ################################################################
[fd3931e]472
473// Read data from Serial1 UART on ADK
474byte ReadOneByte() {
[b62ab59]475       
[0b675ba]476        int ByteRead;
[b62ab59]477       
[0b675ba]478        while(!Serial1.available());
[b62ab59]479       
[0b675ba]480        ByteRead = Serial1.read();
[b62ab59]481       
[240e339]482        return ByteRead;
[b62ab59]483       
[fd3931e]484}
485
[240e339]486
487// ################################################################
488
489void setupBlueToothConnection() {
[5202e8d]490       
[f5d44ec]491//      setColorWheel(0,0,255); // Blue
[0b675ba]492
[b62ab59]493        Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
[240e339]494
495        // if you want to change baudrate, say to 115200;
496        //  Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
497        //  delay(2000); // This delay is required.
498        //  Serial1.begin(115200);
499
500        Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
501        Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
502        Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
503        Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
504        Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
505        delay(2000); // This delay is required.
506        //Serial1.flush();
507
508
[0b675ba]509        if(digitalRead(BUTTON_PIN)==0){ //if needs pair
[240e339]510                //update RGB Panel, Red Indicates it's pairing new headset
511                RGB_Panel[5][0]=255;
512                RGB_Panel[5][1]=0;
513                RGB_Panel[5][2]=0;
514                sendFrame(0);
515               
516                Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
517                Serial.println("Pyramid is inquiring!");
518                delay(2000); // This delay is required.
[0b675ba]519               
[240e339]520                //find the target slave, hence MindWave Mobile Headset
521               
522                Serial.print("print recvChar:");
523                char recvChar;
524                while(1) {
[0b675ba]525                        if(Serial1.available()) {
[240e339]526                                recvChar = Serial1.read();
527                                Serial.print(recvChar);
528                                recvBuf += recvChar;
529                                nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
530                                //nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
[0b675ba]531                                if ( nameIndex != -1 ) {
[240e339]532                                        //Serial.print(recvBuf);
533                                        addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address                 
534                                        slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
535                                        break;
536                                }
537                        }
[0b675ba]538                }
[240e339]539               
540                Serial.println();
[0b675ba]541               
[240e339]542                //form the full connection command
543                connectCmd += slaveAddr;
544                connectCmd += "\r\n";
545                int connectOK = 0;
546                Serial.print("Connecting to slave:");
547                Serial.print(slaveAddr);
548                Serial.println(slaveName);
549                //connecting the slave till they are connected
550                do {
551                        Serial1.print(connectCmd);//send connection command
552                        recvBuf = "";
[0b675ba]553                        while(1) {
554                                if(Serial1.available()) {
[240e339]555                                        recvChar = Serial1.read();
556                                        recvBuf += recvChar;
[0b675ba]557                                        if(recvBuf.indexOf("CONNECT:OK") != -1) {
[240e339]558                                                connectOK = 1;
559                                                Serial.println("Connected!");
560                                                //Serial1.print("Connected!");
561                                                break;
[0b675ba]562                                        } else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
[240e339]563                                                Serial.println("Connect again!");
564                                                break;
565                                        }
566                                }
567                        }
568                } while(0 == connectOK);
569               
[0b675ba]570        } //end if needs pair
571        else { //if auto connected
[240e339]572                Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
573                //update bottom RGB LED to blue
[77a80d3]574                setColorWheel(0,0,0); // Black
[240e339]575                RGB_Panel[5][0]=0;
576                RGB_Panel[5][1]=0;
577                RGB_Panel[5][2]=255;
578                sendFrame(0);
579        }
580
581        delay(3000);
582
583        //If Bluetooth connection is established, top LED blue.
584        // Otherwise top LED red.
[77a80d3]585//      if(digitalRead(5)){//D5 for Pyramid Shield, A1 for testing
586//              RGB_Panel[11][0]=0;
587//              RGB_Panel[11][1]=0;
588//              RGB_Panel[11][2]=255;
589//              sendFrame(0);
590//      }
591//      else{
592//              RGB_Panel[11][0]=255;
593//              RGB_Panel[11][1]=0;
594//              RGB_Panel[11][2]=0;
595//              sendFrame(0);
596//      }
[0b675ba]597       
598}
599// End setupBluetoothConnection
[240e339]600
601
602// ################################################################
603
604void updateFrame(byte attention, byte meditation, byte poorQuality) {
605        // update RGB_Panel[][] here to set next Frame you want to send
606        // For Example:
607        // RGB_Panel[0][0]=0;
608        // RGB_Panel[0][1]=0;
609        // RGB_Panel[0][2]=255;
610        // will update the LED on 1:00 position to be blue.
611
612        // This following code update RGB Panel based on data
613        // received from MindWave Mobile.
614
615        // if the signal is good enough, light 6:00 LED in green.
[0b675ba]616        if (poorQuality <1) {
[240e339]617                RGB_Panel[5][0]=0;
618                RGB_Panel[5][1]=255;
619                RGB_Panel[5][2]=0;
[77a80d3]620        } else
621                return;
622//      else {
623//              RGB_Panel[5][0]=0;
624//              RGB_Panel[5][1]=0;
625//              RGB_Panel[5][2]=0;
626//             
627//              //Make top LED green, indicating valid bluetooth connection
628//              RGB_Panel[11][0]=0;
629//              RGB_Panel[11][1]=255;
630//              RGB_Panel[11][2]=0;
631//      }
[0b675ba]632
[240e339]633
634        //light up & dim red LED according to attention level
[0b675ba]635        for(int i=6; i<6+attention; i++) {
[240e339]636                RGB_Panel[i][0]=255;
637                RGB_Panel[i][1]=0;
638                RGB_Panel[i][2]=0;
639        }
[0b675ba]640        for(int i=6+attention; i<11; i++) {
[240e339]641                RGB_Panel[i][0]=0;
642                RGB_Panel[i][1]=0;
643                RGB_Panel[i][2]=0;
644        }
645
646        //light up & dim blue LED according to meditation level
[0b675ba]647        for(int i=4; i>4-meditation; i--) {
[240e339]648                RGB_Panel[i][0]=0;
649                RGB_Panel[i][1]=0;
650                RGB_Panel[i][2]=255;
651        }
[0b675ba]652        for(int i=4-meditation; i>-1; i--) {
[240e339]653                RGB_Panel[i][0]=0;
654                RGB_Panel[i][1]=0;
655                RGB_Panel[i][2]=0;
656        }
657
[0b675ba]658}// end updateFrame
[240e339]659
660
661// ################################################################
662
663void sendFrame(int delayTime) {
664        // Maximum bytes that can be send over I2C in one
665        // transmission is 32 bytes, since we need 36 bytes
666        // to update a full frame, we just split one frame
667        // to two frames.
668
669        //delayTime=delayTime/2;
670
671        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
672        Wire.write(0);
673        for(int i=0;i<6;i++){
674                for(int j=0;j<3;j++)
675                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
676        }
677        Wire.endTransmission();    // stop transmitting
678        //delay(delayTime);
679
680        Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
681        Wire.write(18);
682        for(int i=6;i<12;i++){
683                for(int j=0;j<3;j++)
684                        Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
685        }
686        Wire.endTransmission();    // stop transmitting
687        //delay(delayTime);
[0b675ba]688       
689} // sendFrame
[240e339]690
691
692// ################################################################
693
[fd3931e]694//////PWM//////
695//generate ON/OFF control signals, with starting and stopping PWM generator
[240e339]696void innerCycle(int onTime, int offTime) {
697        pwmWrite(IR, ON);
698        delayMicroseconds(onTime);
699        pwmWrite(IR, OFF);
700        delayMicroseconds(offTime);
[0b675ba]701} // innerCycle
[240e339]702
703
704// ################################################################
705
706void emitCode(char BIT) {
707        //emitCode generate LOWs between HIGHs as same as the parameter.
[0b675ba]708        if
[b62ab59]709                (BIT) innerCycle(671,732); // 1
[240e339]710        else
[b62ab59]711                innerCycle(337,402); // 0
712       
713       
[0b675ba]714} // emitCode
[240e339]715
716
717// ################################################################
718
[0b675ba]719void sendCode(long code) {
[240e339]720        char n;
721        //starting code, with special time period.
722        innerCycle(730,392); //(773 414)
723        innerCycle(730,392);
724
[b62ab59]725        for (n=28;n>=0;n--)
726                emitCode((code >> n) & 1); //getting bits out from code
[0b675ba]727
[240e339]728} // sendCode
729
730
731// ################################################################
732
733long formCode(char throttle,char yaw,char pitch) {
734        char n;
735        long mainCode=0;
736        int checkSum=0;
737
738        //throttle
[b62ab59]739        for (n=6; n>=0; n--)
740                bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
[240e339]741
742        bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
743
744        //channel selection first half
[b62ab59]745        if (_channel=='C')
746                bitWrite(mainCode,15,1);
747        else
748                bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
[240e339]749
[77a80d3]750        for (n=6; n>=0; n--)
751                bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
[240e339]752
753        //channel selection second half
[b62ab59]754        if (_channel=='A')
755                bitWrite(mainCode,7,1);
756        else
757                bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
[240e339]758
[b62ab59]759        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
[240e339]760
[b62ab59]761        for (n=5; n>=0; n--)
762                bitWrite(mainCode,n,bitRead(pitch,n)); //pitch 
[240e339]763               
764        // CheckSum
[b62ab59]765        for (n=0; n<=20; n=n+4)
766                checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
[240e339]767
[b62ab59]768        checkSum=checkSum & B1111; // get the last 4 bits of the sum
769        checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
[240e339]770
771        mainCode= (mainCode << 5) | (checkSum << 1); //get the last 4 digit of CheckSum
772
[b62ab59]773        bitWrite(mainCode,0,1);  // finish code
[77a80d3]774       
[b62ab59]775        return mainCode; // mainCode is a 29 bit binary number
[77a80d3]776       
[0b675ba]777} // formCode
778
779
780// ################################################################
781
782void 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        a = inByte;
793
794        while (Serial.available() == 0);
795        inByte = Serial.read() - '0';
796        b = inByte;
[240e339]797
[0b675ba]798        while (Serial.available() == 0);
799        inByte = Serial.read() - '0';
800        c = inByte;
[240e339]801
[0b675ba]802        newThrottle = (a * 100) + (b * 10) + c;
[240e339]803
[0b675ba]804        if (newThrottle < 0)
805                newThrottle=0;
[fd3931e]806
[0b675ba]807        if (newThrottle > 100)
808                newThrottle=100;
809               
810        _throttle=newThrottle;
811
812        Serial.print("_throttle=");
813        Serial.println(int(_throttle));
814
815} // setThrottle
816
817
818// ################################################################
819
820void setPitch() {
[fd3931e]821
[0b675ba]822        char inByte=0;
823        int a=0;
824        int b=0;
825        int c=0;
826        int newPitch=0;
827
828        while (Serial.available() == 0);
829        inByte = Serial.read() - '0';
830        //Serial.println(inByte);
831        a = inByte;
832
833        while (Serial.available() == 0);
834        inByte = Serial.read() - '0';
835        //Serial.println(inByte);
836        b = inByte;
837
838        while (Serial.available() == 0);
839        inByte = Serial.read() - '0';
840        //Serial.println(inByte);
841        c = inByte;
842
843        newPitch = (a * 100) + (b * 10) + c;
[fd3931e]844
[0b675ba]845        if (newPitch < 0)
846                newPitch=0;
847
848        if (newPitch > 100)
849                newPitch=100;
850               
851        _pitch=newPitch;
852
853        Serial.print("_pitch=");
854        Serial.println(int(_pitch));
855
856} // setPitch
857
858
859// ################################################################
860
861//void setThrottle() {
862//
863//      char inByte=0;
864//      int a=0;
865//      int b=0;
866//      int c=0;
867//      int newThrottle=0;
868//
869//      while (Serial.available() == 0);
870//      inByte = Serial.read() - '0';
871//      //Serial.println(inByte);
872//      a = inByte;
873//
874//      while (Serial.available() == 0);
875//      inByte = Serial.read() - '0';
876//      //Serial.println(inByte);
877//      b = inByte;
878//
879//      while (Serial.available() == 0);
880//      inByte = Serial.read() - '0';
881//      //Serial.println(inByte);
882//      c = inByte;
883//
884//      newThrottle = (a * 100) + (b * 10) + c;
885//
886//      if (newThrottle < 0)
887//              newThrottle=0;
888//
889//      if (newThrottle > 100)
890//              newThrottle=100;
891//             
892//      _throttle=newThrottle;
893//
894//      Serial.print("_throttle=");
895//      Serial.println(int(_throttle));
896//
897//} // setThrottle
898
899
900// ################################################################
901
902void setYaw() {
903
904        char inByte=0;
905        int a=0;
906        int b=0;
907        int c=0;
908        int newYaw=0;
909
910        while (Serial.available() == 0);
911        inByte = Serial.read() - '0';
912        //Serial.println(inByte);
913        a = inByte;
914
915        while (Serial.available() == 0);
916        inByte = Serial.read() - '0';
917        //Serial.println(inByte);
918        b = inByte;
919
920        while (Serial.available() == 0);
921        inByte = Serial.read() - '0';
922        //Serial.println(inByte);
923        c = inByte;
924
925        newYaw = (a * 100) + (b * 10) + c;
926
927        if (newYaw < 0)
928                newYaw=0;
929
930        if (newYaw > 100)
931                newYaw=100;
932               
933        _yaw=newYaw;
934
935        Serial.print("_yaw=");
936        Serial.println(int(_yaw));
937
938} // setYaw
939
940
941// ################################################################
942
[f1b827c]943void setColorWheel(int red, int green, int blue) {
944       
945        for (int hour=0; hour < 12; hour++) {
946       
947                RGB_Panel[hour][0]=red;
948                RGB_Panel[hour][1]=green;
949                RGB_Panel[hour][2]=blue;
950       
951        }
952       
[5202e8d]953        sendFrame(0);
954       
955}
956
957
958// ################################################################
959
960void setStartScreen() {
961       
962        // White
963        RGB_Panel[11][0]=255;
964        RGB_Panel[11][1]=255;
965        RGB_Panel[11][2]=255;
966       
967        RGB_Panel[3][0]=255;
968        RGB_Panel[3][1]=255;
969        RGB_Panel[3][2]=255;
970       
971        RGB_Panel[7][0]=255;
972        RGB_Panel[7][1]=255;
973        RGB_Panel[7][2]=255;
974       
975        // Red
976        for (int hour=0; hour < 3; hour++) {
977                RGB_Panel[hour][0]=255;
978                RGB_Panel[hour][1]=0;
979                RGB_Panel[hour][2]=0;
980        }
981       
982        // Green
983        for (int hour=4; hour < 7; hour++) {
984                RGB_Panel[hour][0]=0;
985                RGB_Panel[hour][1]=255;
986                RGB_Panel[hour][2]=0;
987        }
988       
989        // Blue
990        for (int hour=8; hour < 11; hour++) {
991                RGB_Panel[hour][0]=0;
992                RGB_Panel[hour][1]=0;
993                RGB_Panel[hour][2]=255;
994        }
995       
996        sendFrame(0);
[f1b827c]997       
998}
999
1000
1001// ################################################################
1002
[0b675ba]1003void parseSerialInput() {
1004       
1005        if (Serial.available() > 0)  {
1006               
[b62ab59]1007                if (! modeSerial)
1008                        Serial.println("INFO: Serial input command received, setting modeSerial");
1009                        modeSerial = true;
1010                       
1011                        Serial1.end();
1012                        modeBluetooth = false;
1013                        modeADK = false;
[f1b827c]1014               
1015               
1016                setColorWheel(255, 128, 0);
1017               
[94b751c]1018               
[0b675ba]1019                _command = Serial.read();
1020               
1021                Serial.print("Serial.read(): ");
1022                Serial.println(_command);
1023               
1024                switch (_command) {
1025                       
[b62ab59]1026                        case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
[0b675ba]1027                        case 'O': _throttle=0; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1028                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1029                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
1030                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1031                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
1032                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1033                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
1034                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
1035                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
1036                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
1037                        case 'p':  setPitch(); break;
1038                        case 't':  setThrottle(); break;
1039                        case 'y':  setYaw(); break;
1040                        case 'x':  setThrottle(); break;
1041                }
1042        }
[94b751c]1043       
[77a80d3]1044//      sendCode(formCode(_throttle,_yaw,_pitch));
1045//      delay(80); // originally 36, 51, 66, 80
[fd3931e]1046
[0b675ba]1047} // parseSerialInput()
[fd3931e]1048
[94b751c]1049
[77a80d3]1050// ################################################################
1051
1052int calculateMeter(int value) {
1053 
1054        int result = 0;
1055
1056        if (value == 0)
1057                result = 0;
1058        else if (value <= 20)
1059                result = 1;
1060        else if (value <= 40)
1061                result = 2;
1062        else if (value <= 60)
1063                result = 3;
1064        else if (value <= 80)
1065                result = 4;
1066        else if (value <= 100)
1067                result = 5;
1068
1069        return(result);
1070 
1071} // calculateMeter
1072
1073
1074// ################################################################
1075
1076void updateFrameADK() {
1077
1078        // update RGB_Panel[][] here to set next Frame you want to send
1079        // For Example:
1080        // RGB_Panel[0][0]=0;
1081        // RGB_Panel[0][1]=0;
1082        // RGB_Panel[0][2]=255;
1083        // will update the LED on 1 o'clock position to be blue.
1084
1085        // This following code simulates when you wear a headset.
1086//      RGB_Panel[5][0]=0;
1087//      RGB_Panel[5][1]=255;
1088//      RGB_Panel[5][2]=0;
1089
1090//      int attention=random(0,7); //generates 0~6
1091//      int meditation=random(0,7);
1092
1093
1094        int attention = calculateMeter(eegAttention);
1095        int meditation = calculateMeter(eegMeditation);
1096
1097
1098        for(int i=6; i<=6+attention; i++){
1099                RGB_Panel[i][0]=255;
1100                RGB_Panel[i][1]=0;
1101                RGB_Panel[i][2]=0;
1102        }
1103
1104        if (meditation==6){
1105                RGB_Panel[11][0]=0;
1106                RGB_Panel[11][1]=0;
1107                RGB_Panel[11][2]=255;
1108        }
1109               
1110        for(int i=4; i>=4-meditation; i--){
1111                RGB_Panel[i][0]=0;
1112                RGB_Panel[i][1]=0;
1113                RGB_Panel[i][2]=255;
1114        }
1115
1116        if (attention==6)
1117                RGB_Panel[11][0]=255;
1118               
1119        for(int i=6+attention; i<=11; i++){
1120                RGB_Panel[i][0]=0;
1121                RGB_Panel[i][1]=0;
1122                RGB_Panel[i][2]=0;
1123        }
1124
1125        for(int i=4-meditation; i>=0; i--){
1126                RGB_Panel[i][0]=0;
1127                RGB_Panel[i][1]=0;
1128                RGB_Panel[i][2]=0;
1129        }
[85e090e]1130
1131
[77a80d3]1132} // updateFrame
[85e090e]1133
1134
[77a80d3]1135// ################################################################
[85e090e]1136
[77a80d3]1137void setOrbitThrottle(int throttle) {
1138       
1139        setColorWheel(255,255,0); // Yellow
1140       
1141        _throttle=throttle;
1142        setThrottle();
1143        sendCode(formCode(_throttle,_yaw,_pitch));
1144        delay(80); // originally 36, 51, 66, 80
1145} // setOrbitThrottle
[85e090e]1146
1147
[77a80d3]1148// ################################################################
[85e090e]1149
[77a80d3]1150void parseADK() {
1151       
1152        // Android ADK
1153       
1154        if (acc.isConnected()) {
1155               
1156                int len = acc.read(msg, sizeof(msg), 1);
1157               
1158                if (! modeADK) {
1159                        modeADK = true;
[dbfae54]1160                        Serial1.end();
1161                        Serial.println("INFO: parseADK connection detected, setting modeADK");
1162                        setColorWheel(255, 0, 0); // red
[77a80d3]1163                }
1164               
1165                // Action taken by Arduino is tied to the message it receives from Android
1166               
1167                if (len > 0) {
1168                       
1169                        setColorWheel(255,255,255); // White
1170                       
1171                        if (msg[0] == 0x1) {
1172                                // Reserved
1173                        }
1174                        else if(msg[0] == 0x2) {
1175                                // Reserved
1176                        }
1177                        else if (msg[0] == 0x3) {
1178                                // Reserved
1179                        }
1180                       
1181                        // EEG Signal
1182                        else if (msg[0] == 0x4) {
1183                                eegSignal = 0;
1184                                if (msg[1] == 0x1) {
1185                                        eegSignal = 10;
1186                                } else if (msg[1] == 0x2) {
1187                                        eegSignal = 20;
1188                                } else if (msg[1] == 0x3) {
1189                                        eegSignal = 30;
1190                                } else if (msg[1] == 0x4) {
1191                                        eegSignal = 40;
1192                                } else if (msg[1] == 0x5) {
1193                                        eegSignal = 50;
1194                                } else if (msg[1] == 0x6) {
1195                                        eegSignal = 60;
1196                                } else if (msg[1] == 0x7) {
1197                                        eegSignal = 70;
1198                                } else if (msg[1] == 0x8) {
1199                                        eegSignal = 80;
1200                                } else if (msg[1] == 0x9) {
1201                                        eegSignal = 90;
1202                                } else {
1203                                        eegSignal = 100;
1204                                }
1205                        }
1206                       
1207                       
1208                        // EEG Attention
1209                        else if (msg[0] == 0x5) {
1210                                eegAttention = 0;
1211                                if (msg[1] == 0x1) {
1212                                        eegAttention = 10;
1213                                } else if (msg[1] == 0x2) {
1214                                        eegAttention = 20;
1215                                } else if (msg[1] == 0x3) {
1216                                        eegAttention = 30;
1217                                } else if (msg[1] == 0x4) {
1218                                        eegAttention = 40;
1219                                } else if (msg[1] == 0x5) {
1220                                        eegAttention = 50;
1221                                } else if (msg[1] == 0x6) {
1222                                        eegAttention = 60;
1223                                } else if (msg[1] == 0x7) {
1224                                        eegAttention = 70;
1225                                } else if (msg[1] == 0x8) {
1226                                        eegAttention = 80;
1227                                } else if (msg[1] == 0x9) {
1228                                        eegAttention = 90;
1229                                } else {
1230                                        eegAttention = 100;
1231                                }
1232                        }
1233                       
1234                       
1235                        // EEG Meditation
1236                        else if (msg[0] == 0x6) {
1237                                eegMeditation = 0;
1238                                if (msg[1] == 0x1) {
1239                                        eegMeditation = 10;
1240                                } else if (msg[1] == 0x2) {
1241                                        eegMeditation = 20;
1242                                } else if (msg[1] == 0x3) {
1243                                        eegMeditation = 30;
1244                                } else if (msg[1] == 0x4) {
1245                                        eegMeditation = 40;
1246                                } else if (msg[1] == 0x5) {
1247                                        eegMeditation = 50;
1248                                } else if (msg[1] == 0x6) {
1249                                        eegMeditation = 60;
1250                                } else if (msg[1] == 0x7) {
1251                                        eegMeditation = 70;
1252                                } else if (msg[1] == 0x8) {
1253                                        eegMeditation = 80;
1254                                } else if (msg[1] == 0x9) {
1255                                        eegMeditation = 90;
1256                                } else {
1257                                        eegMeditation = 100;
1258                                }
1259                        }
1260                       
1261                       
1262//                      updateFrame(eegAttention, eegMeditation, eegSignal);
1263                        updateFrameADK();
1264                       
1265                       
1266                        // Orbit Throttle
1267                        if (msg[0] == 0x7) {
1268                               
1269                                Serial.println(" (msg[0] == 0x7) ");
1270                               
1271                                int throttle = 0;
1272                               
1273                                if (msg[1] == 0x1) {
1274                                        throttle = _throttle_hover;
1275                                        Serial.println(" (msg[1] == 0x1) ");
1276}
1277                                else if (msg[1] == 0x0)
1278                                        Serial.println(" (msg[1] == 0x0) ");
1279                               
1280                                setOrbitThrottle(throttle);
1281                       
1282                        } else {
1283                               
1284//                              sendFrame(1000);
1285                                sendFrame(0);
1286                                sendCode(formCode(_throttle,_yaw,_pitch));
1287                                delay(80); //cycle();
1288                        }
1289                       
1290                        setColorWheel(255,0,0); // Red
1291               
1292                } // len
1293                else {
1294//                      setColorWheel(255, 0, 0); // red
1295                        delay(80); //cycle();
1296                }
1297        } // if acc.isConnected()
[85e090e]1298
1299
[77a80d3]1300} // parseADK
[85e090e]1301
[f5d44ec]1302
[dbfae54]1303
1304
1305
1306
1307
1308
1309
1310
Note: See TracBrowser for help on using the repository browser.