Changeset 240e339 in pyramid


Ignore:
Timestamp:
10/09/13 13:56:13 (6 years ago)
Author:
Steve Castellotti <sc@…>
Branches:
master
Children:
0b675ba
Parents:
a96666e
Message:
  • added parseInput() routine to accept commands from serial input
File:
1 edited

Legend:

Unmodified
Added
Removed
  • firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino

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