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

Last change on this file since fd3931e was fd3931e, checked in by Steve Castellotti <sc@…>, 7 years ago

Initial commit

  • Property mode set to 100644
File size: 15.3 KB
Line 
1/*
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 
33*/
34 
35#include <Wire.h> //For I2C communication with RGB LED Panel
36#include <PWM.h> //For sending 38kHz Infrared to fly Orbit helicopter
37 
38
39#define DEBUG_OUTPUT 1 //1 for debug
40
41
42byte _IR_period = 40;
43
44
45#define BUTTON_PIN 4
46/////////////Orbit Flight/////////////
47#define IR 6   
48#define ON 128
49#define OFF 0
50
51//PWM IR_frequency (in Hz) required by IR
52#define IR_frequency 38400
53
54//Global variables for controlling Orbit helicopter
55//variables start with "_" means global variable
56char _throttle=0; //32~127, default 0
57char _pitch=31; //0~63, default 31
58char _yaw=78; //16~127, default 78
59char _channel='A';
60char _command;
61byte _attention_threshold = 60; //treshold for launching Orbit helicopter
62
63///////////////MindWave Mobile Protocol///////////
64#define BAUDRATE 57600
65#define DEBUGOUTPUT 0
66#define powercontrol 10
67
68// checksum variables
69byte generatedChecksum = 0;
70byte checksum = 0;
71int payloadLength = 0;
72byte payloadData[64] = {
73  0};
74byte poorQuality = 0;
75byte attention = 0;
76byte att_buff = 0;
77byte meditation = 0;
78
79// system variables
80long lastReceivedPacket = 0;
81long lastLoop = 0;
82boolean bigPacket = false;
83
84///////////////Bluetooth Connection with MindWave Mobile/////////////
85String retSymb = "+RTINQ=";//start symble when there's any return
86String slaveName = ";MindWave Mobile";// caution that ';'must be included, and make sure the slave name is right.
87int nameIndex = 0;
88int addrIndex = 0;
89
90String recvBuf;
91String slaveAddr;
92
93String connectCmd = "\r\n+CONN=";
94
95////////////RGB Panel//////////////
96byte 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 
111/////////
112//SETUP//
113/////////
114void 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();
142}
143 
144
145/////////////
146//MAIN LOOP//
147/////////////
148void 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
293
294// Read data from Serial1 UART on ADK
295byte ReadOneByte() {
296  int ByteRead;
297
298  while(!Serial1.available());
299  ByteRead = Serial1.read();
300 
301  return ByteRead;
302}
303 
304void 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)){ //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         
397      RGB_Panel[11][0]=0;
398      RGB_Panel[11][1]=0;
399      RGB_Panel[11][2]=255;
400    sendFrame(0);
401  }
402  else{
403          RGB_Panel[11][0]=255;
404      RGB_Panel[11][1]=0;
405      RGB_Panel[11][2]=0;
406      sendFrame(0);
407  }
408   
409}
410// End setupBluetoothConnection
411
412void updateFrame(byte attention, byte meditation, byte poorQuality)
413{
414// update RGB_Panel[][] here to set next Frame you want to send
415// For Example:
416// RGB_Panel[0][0]=0;
417// RGB_Panel[0][1]=0;
418// RGB_Panel[0][2]=255;
419// will update the LED on 1:00 position to be blue.
420
421// This following code update RGB Panel based on data
422// received from MindWave Mobile.
423
424// if the signal is good enough, light 6:00 LED in green.
425  if (poorQuality <1){
426      RGB_Panel[5][0]=0;
427      RGB_Panel[5][1]=255;
428      RGB_Panel[5][2]=0;
429  }
430  else {
431      RGB_Panel[5][0]=0;
432      RGB_Panel[5][1]=0;
433      RGB_Panel[5][2]=0;
434     
435      //Make top LED green, indicating valid bluetooth connection
436      RGB_Panel[11][0]=0;
437      RGB_Panel[11][1]=255;
438      RGB_Panel[11][2]=0;
439
440  }
441   
442 
443//light up & dim red LED according to attention level
444  for(int i=6; i<6+attention; i++){
445    RGB_Panel[i][0]=255;
446    RGB_Panel[i][1]=0;
447    RGB_Panel[i][2]=0;
448  }
449  for(int i=6+attention; i<11; i++){
450  RGB_Panel[i][0]=0;
451  RGB_Panel[i][1]=0;
452  RGB_Panel[i][2]=0;
453  }
454 
455//light up & dim blue LED according to meditation level
456  for(int i=4; i>4-meditation; i--){
457    RGB_Panel[i][0]=0;
458    RGB_Panel[i][1]=0;
459    RGB_Panel[i][2]=255;
460  }
461  for(int i=4-meditation; i>-1; i--){
462  RGB_Panel[i][0]=0;
463  RGB_Panel[i][1]=0;
464  RGB_Panel[i][2]=0;
465  }
466
467}// end updateFrame
468
469void sendFrame(int delayTime)
470{
471  // Maximum bytes that can be send over I2C in one
472  // transmission is 32 bytes, since we need 36 bytes
473  // to update a full frame, we just split one frame
474  // to two frames.
475 
476  //delayTime=delayTime/2;
477 
478  Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
479  Wire.write(0);
480  for(int i=0;i<6;i++){
481    for(int j=0;j<3;j++)
482      Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
483  }
484  Wire.endTransmission();    // stop transmitting
485  //delay(delayTime);
486 
487  Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
488  Wire.write(18);
489  for(int i=6;i<12;i++){
490    for(int j=0;j<3;j++)
491      Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
492  }
493  Wire.endTransmission();    // stop transmitting
494  //delay(delayTime);
495}
496
497//////PWM//////
498//generate ON/OFF control signals, with starting and stopping PWM generator
499void innerCycle(int onTime, int offTime)
500{
501  pwmWrite(IR, ON);
502  delayMicroseconds(onTime);
503  pwmWrite(IR, OFF);
504  delayMicroseconds(offTime);
505}
506
507void emitCode(char BIT)//emitCode generate LOWs between HIGHs as same as the parameter.
508{
509  if (BIT) innerCycle(671,732);// 1
510  else innerCycle(337,402);// 0
511}
512
513void sendCode(long code)
514{
515 char n;
516  //starting code, with special time period.
517  innerCycle(730,392); //(773 414)
518  innerCycle(730,392);
519 
520  for (n=28;n>=0;n--)  emitCode((code >> n) & 1); //getting bits out from code
521
522}
523
524long formCode(char throttle,char yaw,char pitch)
525{
526  char n;
527  long mainCode=0;
528  int checkSum=0;
529
530  //throttle
531  for (n=6; n>=0; n--)  bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
532 
533  bitWrite(mainCode,16,1);  //meaning unclear, possibly left button.
534 
535  //channel selection first half
536  if (_channel=='C') bitWrite(mainCode,15,1);
537  else bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
538 
539  for (n=6; n>=0; n--)  bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
540 
541  //channel selection second half
542  if (_channel=='A') bitWrite(mainCode,7,1);
543  else bitWrite(mainCode,7,0); //if channel B or C, this digit equals 0;
544 
545  bitWrite(mainCode,6,0);// meaning unclear, possibly right button.
546 
547  for (n=5; n>=0; n--)  bitWrite(mainCode,n,bitRead(pitch,n));//pitch 
548   
549  // CheckSum
550  for (n=0; n<=20; n=n+4)  checkSum += ((mainCode >> n) & B1111);//sum up every 4 digits in the code
551 
552  checkSum=checkSum & B1111; //get the last 4 bits of the sum
553  checkSum=(16-checkSum) & B1111;//16-sum is the formula of this helicopter
554 
555  mainCode= (mainCode << 5) | (checkSum << 1); //get the last 4 digit of CheckSum
556 
557  bitWrite(mainCode,0,1);  //finish code
558  return mainCode; //mainCode is a 29 bit binary number
559}
560 
561 void setThrottle() {
562 
563  char inByte=0;
564  int a=0;
565  int b=0;
566  int c=0;
567  int newThrottle=0;
568 
569  while (Serial.available() == 0);
570  inByte = Serial.read() - '0';
571  a = inByte;
572 
573  while (Serial.available() == 0);
574  inByte = Serial.read() - '0';
575  b = inByte;
576 
577  while (Serial.available() == 0);
578  inByte = Serial.read() - '0';
579  c = inByte;
580 
581  newThrottle = (a * 100) + (b * 10) + c;
582 
583  if (newThrottle < 0)
584    newThrottle=0;
585 
586  if (newThrottle > 100)
587    newThrottle=100;
588   
589  _throttle=newThrottle;
590 
591  Serial.print("_throttle=");
592  Serial.println(int(_throttle));
593 
594}
595
Note: See TracBrowser for help on using the repository browser.