Context Navigation

Puzzlebox_Pyramid.ino @ 7d2ea4b

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

Note: See TracBrowser for help on using the repository browser.

Download in other formats:

Original Format