Context Navigation

Puzzlebox_Pyramid.ino @ f5d44ec

Last change on this file since f5d44ec was f5d44ec, checked in by Steve Castellotti <sc@…>, 9 years ago
tighten up main loop
Property mode set to `100644`
File size: 22.7 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-15
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 0 // 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	//G lobal variables for controlling Orbit helicopter
61	// variables start with "_" means global variable
62	char _throttle=0; //32~127, default 0
63	char _pitch=31; //0~63, default 31
64	char _yaw=78; //16~127, default 78
65	char _channel='A';
66	char _command;
67	byte _attention_threshold = 67; //treshold for launching Orbit helicopter
68	int _throttle_hover=85;
69
70	// EEG
71	int eegSignal = 0;
72	int eegAttention = 0;
73	int eegMeditation = 0;
74
75	// Operation mode
76	boolean modeBluetooth = false;
77	boolean modeSerial = false;
78	boolean modeADK = false;
79
80	// MindWave Mobile Protocol
81	#define BAUDRATE 57600
82	#define DEBUGOUTPUT 0
83	#define powercontrol 10
84
85	// Checksum variables
86	byte generatedChecksum = 0;
87	byte checksum = 0;
88	int payloadLength = 0;
89	byte payloadData[64] = { 0 };
90	byte poorQuality = 0;
91	byte attention = 0;
92	byte att_buff = 0;
93	byte meditation = 0;
94
95	// System variables
96	long lastReceivedPacket = 0;
97	long lastLoop = 0;
98	long thisLoop = 0;
99	boolean bigPacket = false;
100
101	// Bluetooth Connection with MindWave Mobile
102	String retSymb = "+RTINQ="; //start symble when there's any return
103	String slaveName = ";MindWave Mobile"; // caution that ';'must be included, and make sure the slave name is right.
104	String connectCmd = "\r\n+CONN=";
105	int nameIndex = 0;
106	int addrIndex = 0;
107	String recvBuf;
108	String slaveAddr;
109
110	// RGB Panel
111	byte RGB_Panel[12][3] = {
112	{0,0,0},
113	{0,0,0},
114	{0,0,0},
115	{0,0,0},
116	{0,0,0},
117	{0,0,0},
118	{0,0,0},
119	{0,0,0},
120	{0,0,0},
121	{0,0,0},
122	{0,0,0},
123	{0,0,0},
124	};
125
126
127	// ################################################################
128
129	/////////
130	//SETUP//
131	/////////
132	void setup() {
133
134	pinMode(13, OUTPUT);
135	Serial.begin(115200);
136
137	// Android ADK
138	// delay(500);
139	// acc.powerOn();
140
141
142	////////Orbit Flight///////////
143	// initialize all timers except for 0, to save time keeping functions
144	InitTimersSafe();
145
146	// sets the IR_frequency for the specified pin
147	bool success = SetPinFrequencySafe(IR, IR_frequency);
148
149	// if the pin IR_frequency was set successfully, turn pin 13 on
150	if(success) {
151	Serial.println("INFO: PWM pin frequency set");
152	}
153
154	/////////////RGB////////////////
155	Wire.begin(); // join i2c bus as master (address optional for master)
156
157
158	setStartScreen();
159
160
161	// if (acc.isConnected()) {
162	// modeADK = true;
163	// Serial.println("INFO: ADK connection detected, setting modeADK");
164	// setColorWheel(255, 0, 0); // red
165	// }
166
167
168	//////////Bluetooth///////////
169
170	if (! modeADK) {
171
172	// delay(500);
173
174	setupBlueToothConnection();
175	// wait 1s and flush the serial buffer
176	delay(1000);
177	// Serial.flush(); //delete?
178	Serial1.flush();
179
180	}
181
182	selectInput(); // Determine method for Pyramid control
183
184
185	} // setup
186
187
188	// ################################################################
189
190	/////////////
191	//MAIN LOOP//
192	/////////////
193	void loop() {
194
195	if (modeBluetooth)
196	parseBluetooth();
197
198	else if (modeSerial)
199	parseSerialInput();
200
201	// else if (modeADK)
202	// parseADK();
203
204
205	// Read from button
206	if (digitalRead(BUTTON_PIN)){
207	RGB_Panel[11][0]=255;
208	RGB_Panel[11][1]=255;
209	RGB_Panel[11][2]=255;
210	digitalWrite(13,HIGH);
211	}
212	else {
213	RGB_Panel[11][0]=0;
214	RGB_Panel[11][1]=0;
215	RGB_Panel[11][2]=0;
216	digitalWrite(13,LOW);
217	}
218
219	// Send Flight Command through IR Emitter
220	if (millis()-lastLoop>_IR_period) {
221
222	// Add Settings for _yaw and _pitch here for custom flight path
223
224	sendCode(formCode(_throttle,_yaw,_pitch));
225
226	if (modeBluetooth)
227	Serial1.flush();
228
229	thisLoop = millis() - lastLoop;
230
231	#if DEBUG_OUTPUT
232	Serial.print(" Attention:");
233	Serial.print(att_buff);
234	Serial.print(" \| Time per loop:");
235	// Serial.println(millis() - lastLoop, DEC);
236	Serial.println(thisLoop);
237	#endif
238
239	lastLoop = millis();
240
241	} // if (millis()-lastLoop>_IR_period)
242
243	} // Main loop
244
245
246	// ################################################################
247
248	void selectInput() {
249
250	// Determine method for Pyramid control
251
252	while ( (! Serial.available()) & (! Serial1.available()) );
253
254	if (Serial.available()) {
255	modeSerial = true;
256	Serial.println("INFO: Serial input command received, setting modeSerial");
257	}
258
259	else if (Serial1.available()) {
260	modeBluetooth = true;
261	setColorWheel(0,0,255); // Blue
262	Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
263	}
264
265
266	// while ( (! Serial.available()) & (! Serial1.available()) & (! acc.isConnected()) );
267	//
268	// if (Serial.available()) {
269	// modeSerial = true;
270	// Serial.println("INFO: Serial input command received, setting modeSerial");
271	// }
272	//
273	// else if (acc.isConnected()) {
274	// modeADK = true;
275	// Serial1.end();
276	// Serial.println("INFO: ADK connection detected, setting modeADK");
277	// setColorWheel(255, 0, 0); // red
278	// }
279	//
280	// else if (Serial1.available()) {
281	// modeBluetooth = true;
282	// Serial.println("INFO: Bluetooth input received, setting modeBluetooth");
283	// }
284
285
286	} // selectInput
287
288
289	// ################################################################
290
291	void parseBluetooth() {
292
293	// MindWave Mobile Protocol
294
295	if(ReadOneByte() == 170) {
296
297	// if (! modeBluetooth)
298	// Serial.println("INFO: Bluetooth input command received, setting modeBluetooth");
299	// modeBluetooth = true;
300
301	if(ReadOneByte() == 170) {
302
303	payloadLength = ReadOneByte();
304	if(payloadLength > 169) //Payload length can not be greater than 169
305	return;
306
307	generatedChecksum = 0;
308	for(int i = 0; i < payloadLength; i++) {
309	payloadData[i] = ReadOneByte(); //Read payload into memory
310	generatedChecksum += payloadData[i];
311	}
312
313	checksum = ReadOneByte(); //Read checksum byte from stream
314	generatedChecksum = 255 - generatedChecksum; //Take one's compliment of generated checksum
315
316	if (checksum == generatedChecksum) {
317
318	poorQuality = 200;
319	attention = 0;
320	meditation = 0;
321
322	for(int i = 0; i < payloadLength; i++) { // Parse the payload
323	switch (payloadData[i]) {
324	case 2:
325	i++;
326	poorQuality = payloadData[i];
327	bigPacket = true;
328	break;
329	case 4:
330	i++;
331	attention = payloadData[i];
332	break;
333	case 5:
334	i++;
335	meditation = payloadData[i];
336	break;
337	case 0x80:
338	i = i + 3;
339	break;
340	case 0x83:
341	i = i + 25;
342	break;
343	default:
344	break;
345	} // switch
346	} // for loop
347
348
349	// Update RGB Panel Display
350
351	if(bigPacket) {
352	Serial.print("SignalQuality: ");
353	Serial.print(poorQuality, DEC);
354	Serial.print(" Attention: ");
355	Serial.print(attention, DEC);
356	Serial.print(" Meditation: ");
357	Serial.print(meditation, DEC);
358	Serial.print(" Time since last packet: ");
359	Serial.print(millis() - lastReceivedPacket, DEC);
360	lastReceivedPacket = millis();
361	Serial.print("\n");
362
363
364	if (poorQuality == 200)
365	setColorWheel(0,255,0); // Green
366
367
368	// Update RGB Panel when received data from MindWave Mobile
369	int att, med;
370	if(attention==0) att=0;
371	else att=(attention-1)/20+1;
372	if(meditation==0) med=0;
373	else med=(meditation-1)/20+1;
374
375	att_buff=attention;
376
377	if (att_buff>_attention_threshold) {
378	RGB_Panel[11][0]=255;
379	RGB_Panel[11][1]=255;
380	RGB_Panel[11][2]=128;
381	}
382	else{
383	RGB_Panel[11][0]=0;
384	RGB_Panel[11][1]=0;
385	RGB_Panel[11][2]=0;
386	}
387
388	updateFrame(att,med,poorQuality); // update buffer
389	sendFrame(0);// send current buffer to RGB Panel with no delay
390
391
392	if (att_buff>_attention_threshold) {
393
394	// If detected Attention level is above the target threshold
395	// then send the control command to fly the helicopter.
396	// Otherwise send no control frames, which cause the helicopter
397	// to land.
398
399	_throttle=_throttle_hover;
400
401
402	} else {
403
404	_throttle=0;
405
406	}
407
408
409	} // end if(bigPacket)
410
411	bigPacket = false;
412
413	} // if (checksum == generatedChecksum)
414
415	else {
416	#if DEBUG_OUTPUT
417	Serial.println("Checksum Error!");
418	// Checksum Error
419	#endif
420	} // end if else for checksum
421	} // end if read 0xAA byte (170)
422	} // end if read 0xAA byte (170)
423
424
425	} // parseBluetooth
426
427
428	// ################################################################
429
430	// Read data from Serial1 UART on ADK
431	byte ReadOneByte() {
432
433	int ByteRead;
434
435	while(!Serial1.available());
436
437	ByteRead = Serial1.read();
438
439	return ByteRead;
440
441	}
442
443
444	// ################################################################
445
446	void setupBlueToothConnection() {
447
448	// setColorWheel(0,0,255); // Blue
449
450	Serial1.begin(38400); // Set Bluetooth Module BaudRate (for communicating to ADK) to default baud rate 38400
451
452	// if you want to change baudrate, say to 115200;
453	// Serial1.print("\r\n+STBD=115200\r\n");//set bluetooth working baudrate
454	// delay(2000); // This delay is required.
455	// Serial1.begin(115200);
456
457	Serial1.print("\r\n+STWMOD=1\r\n");//set the bluetooth work in master mode
458	Serial1.print("\r\n+STNA=Puzzlebox Pyramid\r\n");//set the bluetooth name as "Puzzlebox Pyramid"
459	Serial1.print("\r\n+STAUTO=0\r\n");// Forbid Auto-connection
460	Serial1.print("\r\n+STOAUT=1\r\n");// Permit Pair the device
461	Serial1.print("\r\n+STPIN =0000\r\n");// Set Pincode to 0000
462	delay(2000); // This delay is required.
463	//Serial1.flush();
464
465
466	if(digitalRead(BUTTON_PIN)==0){ //if needs pair
467	//update RGB Panel, Red Indicates it's pairing new headset
468	RGB_Panel[5][0]=255;
469	RGB_Panel[5][1]=0;
470	RGB_Panel[5][2]=0;
471	sendFrame(0);
472
473	Serial1.print("\r\n+INQ=1\r\n");//make the master inquire
474	Serial.println("Pyramid is inquiring!");
475	delay(2000); // This delay is required.
476
477	//find the target slave, hence MindWave Mobile Headset
478
479	Serial.print("print recvChar:");
480	char recvChar;
481	while(1) {
482	if(Serial1.available()) {
483	recvChar = Serial1.read();
484	Serial.print(recvChar);
485	recvBuf += recvChar;
486	nameIndex = recvBuf.indexOf(slaveName);//get the position of slave name
487	//nameIndex -= 1;//decrease the ';' in front of the slave name, to get the position of the end of the slave address
488	if ( nameIndex != -1 ) {
489	//Serial.print(recvBuf);
490	addrIndex = (recvBuf.indexOf(retSymb,(nameIndex - retSymb.length()- 18) ) + retSymb.length());//get the start position of slave address
491	slaveAddr = recvBuf.substring(addrIndex, nameIndex);//get the string of slave address
492	break;
493	}
494	}
495	}
496
497	Serial.println();
498
499	//form the full connection command
500	connectCmd += slaveAddr;
501	connectCmd += "\r\n";
502	int connectOK = 0;
503	Serial.print("Connecting to slave:");
504	Serial.print(slaveAddr);
505	Serial.println(slaveName);
506	//connecting the slave till they are connected
507	do {
508	Serial1.print(connectCmd);//send connection command
509	recvBuf = "";
510	while(1) {
511	if(Serial1.available()) {
512	recvChar = Serial1.read();
513	recvBuf += recvChar;
514	if(recvBuf.indexOf("CONNECT:OK") != -1) {
515	connectOK = 1;
516	Serial.println("Connected!");
517	//Serial1.print("Connected!");
518	break;
519	} else if (recvBuf.indexOf("CONNECT:FAIL") != -1){
520	Serial.println("Connect again!");
521	break;
522	}
523	}
524	}
525	} while(0 == connectOK);
526
527	} //end if needs pair
528	else { //if auto connected
529	Serial1.print("\r\n+STAUTO=1\r\n");// Permit Auto-connection
530	//update bottom RGB LED to blue
531	RGB_Panel[5][0]=0;
532	RGB_Panel[5][1]=0;
533	RGB_Panel[5][2]=255;
534	sendFrame(0);
535	}
536
537	delay(3000);
538
539	//If Bluetooth connection is established, top LED blue.
540	// Otherwise top LED red.
541	if(digitalRead(5)){//D5 for Pyramid Shield, A1 for testing
542	RGB_Panel[11][0]=0;
543	RGB_Panel[11][1]=0;
544	RGB_Panel[11][2]=255;
545	sendFrame(0);
546	}
547	else{
548	RGB_Panel[11][0]=255;
549	RGB_Panel[11][1]=0;
550	RGB_Panel[11][2]=0;
551	sendFrame(0);
552	}
553
554	}
555	// End setupBluetoothConnection
556
557
558	// ################################################################
559
560	void updateFrame(byte attention, byte meditation, byte poorQuality) {
561	// update RGB_Panel[][] here to set next Frame you want to send
562	// For Example:
563	// RGB_Panel[0][0]=0;
564	// RGB_Panel[0][1]=0;
565	// RGB_Panel[0][2]=255;
566	// will update the LED on 1:00 position to be blue.
567
568	// This following code update RGB Panel based on data
569	// received from MindWave Mobile.
570
571	// if the signal is good enough, light 6:00 LED in green.
572	if (poorQuality <1) {
573	RGB_Panel[5][0]=0;
574	RGB_Panel[5][1]=255;
575	RGB_Panel[5][2]=0;
576	}
577	else {
578	RGB_Panel[5][0]=0;
579	RGB_Panel[5][1]=0;
580	RGB_Panel[5][2]=0;
581
582	//Make top LED green, indicating valid bluetooth connection
583	RGB_Panel[11][0]=0;
584	RGB_Panel[11][1]=255;
585	RGB_Panel[11][2]=0;
586	}
587
588
589	//light up & dim red LED according to attention level
590	for(int i=6; i<6+attention; i++) {
591	RGB_Panel[i][0]=255;
592	RGB_Panel[i][1]=0;
593	RGB_Panel[i][2]=0;
594	}
595	for(int i=6+attention; i<11; i++) {
596	RGB_Panel[i][0]=0;
597	RGB_Panel[i][1]=0;
598	RGB_Panel[i][2]=0;
599	}
600
601	//light up & dim blue LED according to meditation level
602	for(int i=4; i>4-meditation; i--) {
603	RGB_Panel[i][0]=0;
604	RGB_Panel[i][1]=0;
605	RGB_Panel[i][2]=255;
606	}
607	for(int i=4-meditation; i>-1; i--) {
608	RGB_Panel[i][0]=0;
609	RGB_Panel[i][1]=0;
610	RGB_Panel[i][2]=0;
611	}
612
613	}// end updateFrame
614
615
616	// ################################################################
617
618	void sendFrame(int delayTime) {
619	// Maximum bytes that can be send over I2C in one
620	// transmission is 32 bytes, since we need 36 bytes
621	// to update a full frame, we just split one frame
622	// to two frames.
623
624	//delayTime=delayTime/2;
625
626	Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
627	Wire.write(0);
628	for(int i=0;i<6;i++){
629	for(int j=0;j<3;j++)
630	Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 1~6
631	}
632	Wire.endTransmission(); // stop transmitting
633	//delay(delayTime);
634
635	Wire.beginTransmission(1); // transmit to device #1 (RGB Panel)
636	Wire.write(18);
637	for(int i=6;i<12;i++){
638	for(int j=0;j<3;j++)
639	Wire.write(RGB_Panel[i][j]);// sends 18 bytes of lights 7~12
640	}
641	Wire.endTransmission(); // stop transmitting
642	//delay(delayTime);
643
644	} // sendFrame
645
646
647	// ################################################################
648
649	//////PWM//////
650	//generate ON/OFF control signals, with starting and stopping PWM generator
651	void innerCycle(int onTime, int offTime) {
652	pwmWrite(IR, ON);
653	delayMicroseconds(onTime);
654	pwmWrite(IR, OFF);
655	delayMicroseconds(offTime);
656	} // innerCycle
657
658
659	// ################################################################
660
661	void emitCode(char BIT) {
662	//emitCode generate LOWs between HIGHs as same as the parameter.
663	if
664	(BIT) innerCycle(671,732); // 1
665	else
666	innerCycle(337,402); // 0
667
668
669	} // emitCode
670
671
672	// ################################################################
673
674	void sendCode(long code) {
675	char n;
676	//starting code, with special time period.
677	innerCycle(730,392); //(773 414)
678	innerCycle(730,392);
679
680	for (n=28;n>=0;n--)
681	emitCode((code >> n) & 1); //getting bits out from code
682
683	} // sendCode
684
685
686	// ################################################################
687
688	long formCode(char throttle,char yaw,char pitch) {
689	char n;
690	long mainCode=0;
691	int checkSum=0;
692
693	//throttle
694	for (n=6; n>=0; n--)
695	bitWrite(mainCode,17+n,bitRead(throttle,n)); //getting the first 7 digits to mainCode
696
697	bitWrite(mainCode,16,1); //meaning unclear, possibly left button.
698
699	//channel selection first half
700	if (_channel=='C')
701	bitWrite(mainCode,15,1);
702	else
703	bitWrite(mainCode,15,0); //this digit equals 0 in channel A or B
704
705	for (n=6; n>=0; n--) bitWrite(mainCode,8+n,bitRead(yaw,n));//yaw
706
707	//channel selection second half
708	if (_channel=='A')
709	bitWrite(mainCode,7,1);
710	else
711	bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
712
713	bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
714
715	for (n=5; n>=0; n--)
716	bitWrite(mainCode,n,bitRead(pitch,n)); //pitch
717
718	// CheckSum
719	for (n=0; n<=20; n=n+4)
720	checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
721
722	checkSum=checkSum & B1111; // get the last 4 bits of the sum
723	checkSum=(16-checkSum) & B1111;// 16-sum is the formula of this helicopter
724
725	mainCode= (mainCode << 5) \| (checkSum << 1); //get the last 4 digit of CheckSum
726
727	bitWrite(mainCode,0,1); // finish code
728	return mainCode; // mainCode is a 29 bit binary number
729	} // formCode
730
731
732	// ################################################################
733
734	void setThrottle() {
735
736	char inByte=0;
737	int a=0;
738	int b=0;
739	int c=0;
740	int newThrottle=0;
741
742	while (Serial.available() == 0);
743	inByte = Serial.read() - '0';
744	a = inByte;
745
746	while (Serial.available() == 0);
747	inByte = Serial.read() - '0';
748	b = inByte;
749
750	while (Serial.available() == 0);
751	inByte = Serial.read() - '0';
752	c = inByte;
753
754	newThrottle = (a * 100) + (b * 10) + c;
755
756	if (newThrottle < 0)
757	newThrottle=0;
758
759	if (newThrottle > 100)
760	newThrottle=100;
761
762	_throttle=newThrottle;
763
764	Serial.print("_throttle=");
765	Serial.println(int(_throttle));
766
767	} // setThrottle
768
769
770	// ################################################################
771
772	void setPitch() {
773
774	char inByte=0;
775	int a=0;
776	int b=0;
777	int c=0;
778	int newPitch=0;
779
780	while (Serial.available() == 0);
781	inByte = Serial.read() - '0';
782	//Serial.println(inByte);
783	a = inByte;
784
785	while (Serial.available() == 0);
786	inByte = Serial.read() - '0';
787	//Serial.println(inByte);
788	b = inByte;
789
790	while (Serial.available() == 0);
791	inByte = Serial.read() - '0';
792	//Serial.println(inByte);
793	c = inByte;
794
795	newPitch = (a * 100) + (b * 10) + c;
796
797	if (newPitch < 0)
798	newPitch=0;
799
800	if (newPitch > 100)
801	newPitch=100;
802
803	_pitch=newPitch;
804
805	Serial.print("_pitch=");
806	Serial.println(int(_pitch));
807
808	} // setPitch
809
810
811	// ################################################################
812
813	//void setThrottle() {
814	//
815	// char inByte=0;
816	// int a=0;
817	// int b=0;
818	// int c=0;
819	// int newThrottle=0;
820	//
821	// while (Serial.available() == 0);
822	// inByte = Serial.read() - '0';
823	// //Serial.println(inByte);
824	// a = inByte;
825	//
826	// while (Serial.available() == 0);
827	// inByte = Serial.read() - '0';
828	// //Serial.println(inByte);
829	// b = inByte;
830	//
831	// while (Serial.available() == 0);
832	// inByte = Serial.read() - '0';
833	// //Serial.println(inByte);
834	// c = inByte;
835	//
836	// newThrottle = (a * 100) + (b * 10) + c;
837	//
838	// if (newThrottle < 0)
839	// newThrottle=0;
840	//
841	// if (newThrottle > 100)
842	// newThrottle=100;
843	//
844	// _throttle=newThrottle;
845	//
846	// Serial.print("_throttle=");
847	// Serial.println(int(_throttle));
848	//
849	//} // setThrottle
850
851
852	// ################################################################
853
854	void setYaw() {
855
856	char inByte=0;
857	int a=0;
858	int b=0;
859	int c=0;
860	int newYaw=0;
861
862	while (Serial.available() == 0);
863	inByte = Serial.read() - '0';
864	//Serial.println(inByte);
865	a = inByte;
866
867	while (Serial.available() == 0);
868	inByte = Serial.read() - '0';
869	//Serial.println(inByte);
870	b = inByte;
871
872	while (Serial.available() == 0);
873	inByte = Serial.read() - '0';
874	//Serial.println(inByte);
875	c = inByte;
876
877	newYaw = (a * 100) + (b * 10) + c;
878
879	if (newYaw < 0)
880	newYaw=0;
881
882	if (newYaw > 100)
883	newYaw=100;
884
885	_yaw=newYaw;
886
887	Serial.print("_yaw=");
888	Serial.println(int(_yaw));
889
890	} // setYaw
891
892
893	// ################################################################
894
895	void setColorWheel(int red, int green, int blue) {
896
897	for (int hour=0; hour < 12; hour++) {
898
899	RGB_Panel[hour][0]=red;
900	RGB_Panel[hour][1]=green;
901	RGB_Panel[hour][2]=blue;
902
903	}
904
905	sendFrame(0);
906
907	}
908
909
910	// ################################################################
911
912	void setStartScreen() {
913
914	// White
915	RGB_Panel[11][0]=255;
916	RGB_Panel[11][1]=255;
917	RGB_Panel[11][2]=255;
918
919	RGB_Panel[3][0]=255;
920	RGB_Panel[3][1]=255;
921	RGB_Panel[3][2]=255;
922
923	RGB_Panel[7][0]=255;
924	RGB_Panel[7][1]=255;
925	RGB_Panel[7][2]=255;
926
927	// Red
928	for (int hour=0; hour < 3; hour++) {
929	RGB_Panel[hour][0]=255;
930	RGB_Panel[hour][1]=0;
931	RGB_Panel[hour][2]=0;
932	}
933
934	// Green
935	for (int hour=4; hour < 7; hour++) {
936	RGB_Panel[hour][0]=0;
937	RGB_Panel[hour][1]=255;
938	RGB_Panel[hour][2]=0;
939	}
940
941	// Blue
942	for (int hour=8; hour < 11; hour++) {
943	RGB_Panel[hour][0]=0;
944	RGB_Panel[hour][1]=0;
945	RGB_Panel[hour][2]=255;
946	}
947
948	sendFrame(0);
949
950	}
951
952
953	// ################################################################
954
955	void parseSerialInput() {
956
957	if (Serial.available() > 0) {
958
959	if (! modeSerial)
960	Serial.println("INFO: Serial input command received, setting modeSerial");
961	modeSerial = true;
962
963	Serial1.end();
964	modeBluetooth = false;
965	modeADK = false;
966
967
968	setColorWheel(255, 128, 0);
969
970
971	_command = Serial.read();
972
973	Serial.print("Serial.read(): ");
974	Serial.println(_command);
975
976	switch (_command) {
977
978	case 'P': _throttle=_throttle_hover; setColorWheel(255,255,255); Serial.print("_throttle="); Serial.println(int(_throttle)); break;
979	case 'O': _throttle=0; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
980	case 'U': _throttle+=1; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
981	case 'D': _throttle-=1; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
982	case 'L': _yaw+=15; Serial.print("_yaw="); Serial.println(int(_yaw)); break;
983	case 'R': _yaw-=15; Serial.print("_yaw="); Serial.println(int(_yaw)); break;
984	case 'F': _pitch+=5; Serial.print("_pitch="); Serial.println(int(_pitch)); break;
985	case 'B': _pitch-=5; Serial.print("_pitch="); Serial.println(int(_pitch)); break;
986	case '1': _channel='A'; Serial.println("_channel=A"); break;
987	case '2': _channel='B'; Serial.println("_channel=B"); break;
988	case '3': _channel='C'; Serial.println("_channel=C"); break;
989	case 'p': setPitch(); break;
990	case 't': setThrottle(); break;
991	case 'y': setYaw(); break;
992	case 'x': setThrottle(); break;
993	}
994	}
995
996	sendCode(formCode(_throttle,_yaw,_pitch));
997	delay(80); // originally 36, 51, 66, 80
998
999	} // parseSerialInput()
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013

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

Download in other formats:

Original Format