source: pyramid/firmware/Puzzlebox_Pyramid/Puzzlebox_Pyramid.ino @ 17625c3

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

Puzzlebox Pyramid:

  • Production release
  • Property mode set to 100644
File size: 15.4 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)==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
413void 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
470void 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}
497
498//////PWM//////
499//generate ON/OFF control signals, with starting and stopping PWM generator
500void innerCycle(int onTime, int offTime)
501{
502  pwmWrite(IR, ON);
503  delayMicroseconds(onTime);
504  pwmWrite(IR, OFF);
505  delayMicroseconds(offTime);
506}
507
508void 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
514void 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
525long 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
Note: See TracBrowser for help on using the repository browser.