Misguided 1.0 by by NickC (FIU ENG Olympic Team)
Commented by NickC

1.	//Global Variables
2.	//////////////////////////////////////////////////////////////////////////////////
3.	//Input tracking variables
4.	int sens1, sens2, sens3 = 0;
5.	//AI conditional variables
6.	boolean left, right, nearFront, farFront = false;
7.	//Runtime conditional variables
8.	int lastTurn = 0;
9.	boolean avoidingObstacle, tested, goAuto, controlled = false;
10.	//Constant variables
11.	int farLimit = 100;
12.	int nearLimit = 400;
13.	int turnTime = 3;
14.	int driveCurrent = 0;
15.	//Intro Music
16.	char introNotes[] = "cdefgCC";
17.	int introBeats[] = { 
18.	1, 1, 1, 1, 2, 2, 2};
19.	int introTempo = 100;
20.	//Auto Mode Music
21.	char autoNotes[] = "ccCC";
22.	int autoBeats[] = { 
23.	1, 1, 2, 2};
24.	int autoTempo = 100;
25.	//Controlled Mode Music
26.	char controlNotes[] = "CCcc";
27.	int controlBeats[] = { 
28.	1, 1, 2, 2};
29.	int controlTempo = 100;
30.	//Methods
31.	//////////////////////////////////////////////////////////////////////////////////
32.	//Setup Method
33.	void setup() {
34.	//Serial Communication
35.	Serial.begin(9600);
36.	//Pin Modes
37.	pinMode(2, OUTPUT);
38.	pinMode(3, INPUT);
39.	pinMode(4, OUTPUT);
40.	pinMode(7, OUTPUT);
41.	pinMode(11, OUTPUT);
42.	pinMode(12, OUTPUT);
43.	pinMode(13, OUTPUT);
44.	//Animation of LEDS
45.	animateLEDS(100, 4);
46.	//Initiates Control Switch
47.	digitalWrite(2, LOW);
48.	digitalWrite(4, HIGH);
49.	//Plays Intro music
50.	playMusic(introNotes, introBeats, introTempo, sizeof(introNotes));
51.	delay(1000);
52.	//Reads Control Switch
53.	if(digitalRead(3) == LOW) {
54.	//Plays Auto Music
55.	playMusic(autoNotes, autoBeats, autoTempo, sizeof(autoNotes));
56.	}
57.	}
58.	//Runtime Method
59.	void loop() {
60.	//Tests if car switch is on controlled mode and runtime mode has allowed for auto mode
61.	if(digitalRead(3) == HIGH && goAuto) {
62.	//Tests for the auto mode command on serial port (will disable auto mode)
63.	if(Serial.read() == 115) {
64.	//Run respective command and animates leds
65.	serialCommands(115);
66.	animateLEDS(100, 2);
67.	} 
68.	//Else run AI methods
69.	else {
70.	//updates sensor and position tracking variables and uses AI method to decide correct move
71.	readSensors();
72.	decideMove();
73.	}
74.	}
75.	//If runtime mode has disabled auto mode, it turns control over to the serial control methods when in controlled mode
76.	else if(digitalRead(3) == HIGH) {
77.	serialControl();
78.	} 
79.	//Switch must be on auto mode
80.	//AI methods override all serial control and runtime variables are ignored
81.	else {
82.	//Overriding the runtime auto mode variable
83.	goAuto = false;
84.	//Playing music and setting auto mode switch tracking
85.	if(controlled) {
86.	playMusic(autoNotes, autoBeats, autoTempo, sizeof(autoNotes));
87.	}
88.	controlled = false;
89.	//updates sensor and position tracking variables and uses AI method to decide correct move
90.	readSensors();
91.	decideMove();
92.	}
93.	}
94.	//Artificial Intelligence Methods
95.	void decideMove() {
96.	//Tests if car is currently avoiding an obstacle in order to override twitchy operation
97.	if(!avoidingObstacle) {
98.	//Driving forward
99.	Forward();
100.	//Test front sensor tracking variable for the near limit
101.	if(nearFront) {
102.	//Stops car in order to avoid execution delay of reverse function
103.	Stop();
104.	//Avoid obstacle and sets runtime variable
105.	avoidingObstacle = true;
106.	avoidObstacle();
107.	}
108.	//Else test sensor conditonals to decide turns
109.	else {
110.	//Tests if there is something in front of side sensors
111.	if(left || right) {
112.	//Tests if there is something in front of both side sensors
113.	if(left && right) {
114.	//Evaluates which side has an object closer to it, makes the turn, and sets the runtime variable to reflect the last turn taken
115.	if(sens2 > sens3) {
116.	Right();
117.	lastTurn = 1;
118.	} 
119.	else if(sens3 > sens2) {
120.	Left();
121.	lastTurn = 0;
122.	} 
123.	//If both sensors have equal values, drive forward to avoid both obstacles and save power
124.	else {
125.	Front();
126.	}
127.	}
128.	//If there is something in front of one of the two sensors, take respective turn, and set the runtime variable
129.	else if(left) {
130.	Right();
131.	lastTurn = 1;
132.	} 
133.	else if(right) {
134.	Left();
135.	lastTurn = 0;
136.	} 
137.	}
138.	//If there are no side obstacles, drive forward to save power
139.	else {
140.	Front();
141.	}
142.	} 
143.	}
144.	//Car must be avoiding an obstacle so turn control over to the avoidance AI method 
145.	else {
146.	avoidObstacle();
147.	}
148.	}
149.	void avoidObstacle() {
150.	//Test front sensor tracking variable for the far limit
151.	if(farFront) {
152.	//Drive in reverse
153.	Reverse();
154.	//Makes the opposite of the last forward turn to avoid future obstacles in that direction
155.	if(lastTurn == 0) {
156.	Right();
157.	} 
158.	else {
159.	Left();
160.	}
161.	}
162.	//Fully avoided front obstacle and beginning forward avoidance 
163.	else {
164.	//Drive forward
165.	Forward();
166.	//Makes the last forward turn to avoid future obstacles in that direction
167.	if(lastTurn == 0) {
168.	Left();
169.	} 
170.	else {
171.	Right();
172.	}
173.	//Sets the runtime variable and delays execution of runtime to avoid glitchy operation
174.	avoidingObstacle = false;
175.	delay(200);
176.	}
177.	}
178.	//Basic Read and Write methods
179.	void readSensors() {
180.	//Sets sensor tracking variables to respective input values
181.	sens1 = analogRead(0);
182.	sens2 = analogRead(1);
183.	sens3 = analogRead(2);
184.	//Sets conditional variables by comparing tracking values to sensor limits
185.	nearFront = (sens1 >= nearLimit) ? true:false;
186.	farFront = (sens1 >= farFront) ? true:false;
187.	left = (sens2 >= farLimit) ? true:false;
188.	right = (sens3 >= farLimit) ? true:false;
189.	}
190.	void animateLEDS(int animSpeed, int animCycles) {
191.	//Turn off and on leds using the passed delay
192.	digitalWrite(11, HIGH);
193.	digitalWrite(12, LOW);
194.	digitalWrite(13, LOW);
195.	delay(animSpeed);
196.	digitalWrite(11, LOW);
197.	digitalWrite(12, HIGH);
198.	digitalWrite(13, LOW);
199.	delay(animSpeed);
200.	digitalWrite(11, LOW);
201.	digitalWrite(12, LOW);
202.	digitalWrite(13, HIGH);
203.	delay(animSpeed);
204.	//Repeats method until cycles meet 0, where it turns off all leds
205.	if(animCycles > 1) {
206.	animateLEDS(animSpeed, --animCycles);
207.	} 
208.	else {
209.	digitalWrite(11, LOW);
210.	digitalWrite(12, LOW);
211.	digitalWrite(13, LOW);
212.	}
213.	}
214.	void blinkLED(int blinkingLED, int blinkingSpeed, int blinkingCycles) {
215.	//turns off leds and delays by interval
216.	digitalWrite(11, LOW);
217.	digitalWrite(12, LOW);
218.	digitalWrite(13, LOW);
219.	delay(blinkingSpeed);
220.	//turns on chosen led and delays by interval
221.	digitalWrite(blinkingLED, HIGH);
222.	delay(blinkingSpeed);
223.	//Repeats method until cycles meet 0, where it turns off all leds
224.	if(blinkingCycles > 1) {
225.	blinkLED(blinkingLED, blinkingSpeed, --blinkingCycles);
226.	} 
227.	else {
228.	digitalWrite(11, LOW);
229.	digitalWrite(12, LOW);
230.	digitalWrite(13, LOW);
231.	}
232.	}
233.	//Basic motor control methods using a variable current to increase or decrease speed
234.	//Includes stop and front methods to avoid glitchy operation
235.	void Front() {
236.	analogWrite(5, 255);
237.	analogWrite(6, 255);
238.	digitalWrite(11, LOW);
239.	}
240.	void Left() {
241.	analogWrite(5, driveCurrent);
242.	analogWrite(6, 255);
243.	digitalWrite(11, HIGH);
244.	}
245.	void Right() {
246.	driveCurrent = 50;
247.	analogWrite(5, 255);
248.	analogWrite(6, driveCurrent);
249.	digitalWrite(11, HIGH);
250.	}
251.	void Stop() {
252.	analogWrite(9, 255);
253.	analogWrite(10, 255);
254.	digitalWrite(12, LOW);
255.	digitalWrite(13, LOW);
256.	}
257.	void Forward() {
258.	analogWrite(9, driveCurrent);
259.	analogWrite(10, 255);
260.	digitalWrite(12, LOW);
261.	digitalWrite(13, HIGH);
262.	}
263.	void Reverse() {
264.	analogWrite(9, 255);
265.	analogWrite(10, driveCurrent);
266.	digitalWrite(12, HIGH);
267.	digitalWrite(13, LOW);
268.	}
269.	//Musical methods found in Arduino website
270.	//However, methods have been slightly modified
271.	//for an indepth explanation on how we used these scripts, visit http://arduino.cc/en/Tutorial/Melody
272.	void playTone(int tone, int duration) {
273.	for (long i = 0; i < duration * 1000L; i += tone * 2) {
274.	digitalWrite(7, HIGH);
275.	delayMicroseconds(tone);
276.	digitalWrite(7, LOW);
277.	delayMicroseconds(tone);
278.	}
279.	}
280.	boolean playNote(char note, int duration) {
281.	char names[] = { 
282.	'c', 'd', 'e', 'f', 'g', 'a', 'b', 'C'       };
283.	int tones[] = { 
284.	1915, 1700, 1519, 1432, 1275, 1136, 1014, 956       };
285.	for (int i = 0; i < 8; i++) {
286.	if (names[i] == note) {
287.	playTone(tones[i], duration);
288.	return true;
289.	}
290.	}
291.	return false;
292.	}
293.	void playMusic(char* notes, int* beats, int tempo, int length) {
294.	for (int i = 0; i < length; i++) {
295.	if (notes[i] == ' ') {
296.	delay(beats[i] * tempo);
297.	} 
298.	else {
299.	playNote(notes[i], beats[i] * tempo);
300.	}
301.	delay(tempo / 2); 
302.	}
303.	}
304.	//Serial control methods for demonstration and testing
305.	//Main method to decode serial commands
306.	void serialCommands(int command) {
307.	//Commands are decoded using the ASCII Chart available at http://arduino.cc/en/Reference/ASCIIchart
308.	if(command == 119) {
309.	Forward();
310.	delay(200);
311.	}
312.	else if(command == 97) {
313.	Left();
314.	delay(200);
315.	}
316.	else if(command == 113) {
317.	Front();
318.	delay(200);
319.	}
320.	else if(command == 115) {
321.	Stop();
322.	Front();
323.	goAuto = false;
324.	delay(200);
325.	}
326.	else if(command == 100) {
327.	Right();
328.	delay(200);
329.	}
330.	else if(command == 120) {
331.	Reverse();
332.	delay(200);
333.	}
334.	else if(command == 105) {
335.	//this method uses the runtime conditional to override car control and set it to auto mode
336.	animateLEDS(100, 2);
337.	goAuto = true;
338.	}
339.	else if(command == 114) {
340.	//This method uses the basic method to read the sensor values and then prints the tracking results
341.	readSensors();
342.	Serial.println("Sensor Status:\tFront\tLeft\tRight");
343.	Serial.print("\t\t");
344.	Serial.print(sens1);
345.	Serial.print("/");
346.	Serial.print(570/sens1);
347.	Serial.print('"');
348.	Serial.print("\t");
349.	Serial.print(sens2);
350.	Serial.print("/");
351.	Serial.print(570/sens2);
352.	Serial.print('"');
353.	Serial.print("\t");
354.	Serial.print(sens3);
355.	Serial.print("/");
356.	Serial.print(570/sens3);
357.	Serial.print('"');
358.	Serial.println();
359.	}
360.	else if(command == 108) {
361.	animateLEDS(100, 10);
362.	}
363.	else if(command == 116) {
364.	//uses multiple delays, buffers the serial cache and plays all notes until the stream ends
365.	//also uses a modified musical method to validate note in order to avoid extraneous sounds and processing errors
366.	delay(10);
367.	char note = Serial.read();
368.	delay(10);
369.	int beats = Serial.read()-48;
370.	if(note != ' ') {
371.	if(playNote(note, (100*beats)+50)) {
372.	if(Serial.available() > 0) {
373.	serialCommands(116);
374.	}
375.	} 
376.	else {
377.	Serial.println("Bad Note or Format");
378.	Serial.flush();
379.	}
380.	} 
381.	else {
382.	delay(100*beats);
383.	if(Serial.available() > 0) {
384.	serialCommands(116);
385.	}
386.	}
387.	}
388.	else if(command == 99) {
389.	//uses multiple delays, buffers the serial cache and modifies the sensor limits
390.	//also allows for the resetting of the values in order to avoid repetitive bootups
391.	//also uses a conditional statement to avoid extraneous data and processing errors
392.	readSensors();
393.	delay(10);
394.	char mode = Serial.read();
395.	if(mode == 'f') {
396.	Serial.print("Changed Sensor Far Limit from ");
397.	Serial.print(farLimit);
398.	Serial.print(" to ");
399.	Serial.print(sens1);
400.	Serial.println();
401.	farLimit = sens1;
402.	} 
403.	else if(mode == 'n') {
404.	Serial.print("Changed Sensor Near Limit from ");
405.	Serial.print(nearLimit);
406.	Serial.print(" to ");
407.	Serial.print(sens1);
408.	Serial.println();
409.	nearLimit = sens1;
410.	} 
411.	else if(mode == 'r') {
412.	Serial.print("Reset Sensor Far and Near Limits to 100 and 400 respectively");
413.	nearLimit = 400;
414.	farLimit = 100;
415.	}
416.	else {
417.	Serial.println("Bad Sensor Change Mode");
418.	Serial.flush();
419.	}
420.	}
421.	else if(command == 98) {
422.	//user delay to buffer serial cache and then blinks passed led
423.	//also uses a conditional statement to avoid extraneous data and processing errors
424.	delay(10);
425.	char led = Serial.read();
426.	if(led == 'r') {
427.	blinkLED(12, 100, 5);
428.	} 
429.	else if(led == 'g') {
430.	blinkLED(13, 100, 5);
431.	} 
432.	else if(led == 'y') {
433.	blinkLED(11, 100, 5);
434.	} 
435.	else {
436.	Serial.println("Bad LED Color");
437.	Serial.flush();
438.	}
439.	}
440.	else {
441.	Serial.println("Bad Command");
442.	Serial.flush();
443.	}
444.	}
445.	void serialControl() {
446.	//Main serial control mode method to initiate controlled mode, stop auto mode, and send command instructions
447.	//also reads serial cache and uses the previous method to decode input
448.	if(!controlled) {
449.	Stop();
450.	Front();
451.	controlled = true;
452.	playMusic(controlNotes, controlBeats, controlTempo, sizeof(controlNotes));
453.	}
454.	if(!tested) {
455.	Stop();
456.	Front();
457.	Serial.println("Misguided 1.0 - The Olympic Team");
458.	Serial.println("---------------------------------");
459.	Serial.println("BASIC MODES");
460.	Serial.println();
461.	Serial.println("w - Forward");
462.	Serial.println("x - Reverse");
463.	Serial.println("s - Stop");
464.	Serial.println();
465.	Serial.println("a - Left");
466.	Serial.println("d - Right");
467.	Serial.println("q - Front");
468.	Serial.println();
469.	Serial.println("i - Automatic");
470.	Serial.println();
471.	Serial.println("---------------------------------");
472.	Serial.println("ADVANCED FUNCTION");
473.	Serial.println();
474.	Serial.println("r - Get Current Sensor Readings");
475.	Serial.println("cf - Change Sensor Far Limit");
476.	Serial.println("cn - Change Sensor Near Limit");
477.	Serial.println("cr - Reset Sensor Limits");
478.	Serial.println();
479.	Serial.println("l - Animate LEDS");
480.	Serial.println("b - Blink LED - y, r, g");
481.	Serial.println("t - Play Tune - c, d, e, f, g, a, b, C, space");
482.	Serial.println();
483.	Serial.println("Waiting on command...");
484.	Serial.println();
485.	tested = true;
486.	delay(100);
487.	} 
488.	else {
489.	if(Serial.available() > 0) {
490.	serialCommands(Serial.read());
491.	}
492.	}
493.	}