Obstacle avoiding autonomous LEGO Technic car

1#include <Stepper.h>
2
3//For stepper. 64 step for internal motor (before reduction gear) seems to be correct for this model
4#define STEPS 64
5Stepper stepper(STEPS, 8, 10, 9, 11); // swap 9 and 10 if stepper does not run counterclockwise
6//int previousPos = 0; // we do not need to store the previous pos since we always turn the wheels in an absolute fashion
7
8const int LED = 13; // Pin for the LED
9
10//Pins for ultrasound sensor
11const int trigPin = 7;  
12const int echoPin = 4; 
13
14// Control pins for motor. PWM pins. M1 to 255 and M2 to 0 gives full speed forward. Opposite gives full reverse.
15// One pin must always be 0, other pin must be, for this motor in this config, above approx 70-80 to drive.
16// Set one pin to 0 before setting other pin to value above 0.
17const int M1 = 3;
18const int M2 = 5;
19const int stopDistance = 16; //stop and avoid at 20cm
20
21float duration, distance;  
22float prevDistance = 0;
23int distanceAdds = 0;
24int averageDistance = 0;
25int cycleCount = 0;
26int DistanceToPMWValue;
27int valForStepper = 0;
28int randomDelay;
29int stepperDir = 999;
30int turningAmount = 460; //by testing, how much the stepper can turn for front wheels to turn max.
31
32void setup() {
33
34pinMode(LED, OUTPUT); // LED is as an OUTPUT
35
36// Set up ultrasound sensor
37pinMode(trigPin, OUTPUT);  
38pinMode(echoPin, INPUT);  
39
40//stepper speed. By testing, slower gives slower turns, higher might not work reliably
41stepper.setSpeed(500);
42
43//Go forward full speed
44analogWrite(M2, 0);
45analogWrite(M1, 255);
46
47Serial.begin(9600);  
48}
49
50void loop() {
51	
52	digitalWrite(LED, HIGH); // turn the LED on
53
54	// Send and check ultrsound
55	distance = readDistance();
56
57	// we add the distance measurments from each loop iteration. Each iteration has a 50ms delay so 1 second is 20 iterations
58	// we will take the average of 1 seconds worth (20 iterations) of deistance measurements and compare it to the latest distance
59	// measurements. If it is close to the same as the average we assume a stuck car so we trigger a small reverse drive plus
60	// an avoidanceManuever. It is not a perfect algorithm but it works, and it is simple.
61
62	distanceAdds = distance + distanceAdds;
63	if (cycleCount == 20){ // we check if we are stuck once per approx 1 second
64
65		averageDistance = distanceAdds/20;
66
67		distanceAdds = 0; //reset the adding of distances 
68
69		if ((distance < (averageDistance + 5)) && (distance > (averageDistance - 5))) {
70
71			//stop motor
72		analogWrite(M1, 0);
73		analogWrite(M2, 0);
74		//chill a bit
75		delay(100);  
76
77		digitalWrite(LED, LOW);
78		//reverse motor for 200 ms
79		analogWrite(M1, 0);
80		analogWrite(M2, 180);
81		delay(200);  
82
83		avoidanceManuever();
84		digitalWrite(LED, LOW); // turn the LED off, we get one blink per cycle
85		
86		}
87	}
88
89	Serial.print("Distance: ");  
90	Serial.println(distance);  
91
92	Serial.print("Average: ");  
93	Serial.println(averageDistance);  
94
95	DistanceToPMWValue = mapDistanceToPMW(distance);
96
97	// drive forward, adjusting the speed a bit depending on (safe) distance to object
98	analogWrite(M2, 0);
99	analogWrite(M1, DistanceToPMWValue);
100
101	// If close to object (less than 16cm), try to avoid
102	if (distance < stopDistance) {
103	avoidanceManuever();
104	}
105	
106	delay(50);  
107
108	cycleCount++;
109	if (cycleCount == 21) {
110		cycleCount = 0;
111	}
112}
113
114int mapDistanceToPMW (int x) {
115	int output = 0;
116	// if distance over 100cm drive full speed. If less, slow down
117	if (x > 100) {
118		output = 255;
119	} else {
120		output = map(x, stopDistance, 100, 90, 255);
121	}
122	return output;
123}
124
125int readDistance() {
126	int output = 0;
127	digitalWrite(trigPin, LOW);  
128	delayMicroseconds(2);  
129	digitalWrite(trigPin, HIGH);  
130	delayMicroseconds(10);  
131	digitalWrite(trigPin, LOW);  
132	//Calculate distance to obstacle based on ultrasound sensor
133	duration = pulseIn(echoPin, HIGH); 
134	output = (duration*.0343)/2; // calculates the distance in cm 
135	return output;
136}
137
138void avoidanceManuever() {
139
140		//if we do an avoidance manuever we reset the check for being stuck.
141		cycleCount = 0;
142
143		//stop motor and turn wheels one way (program stops until wheels have turned)
144		analogWrite(M1, 0);
145		analogWrite(M2, 0);
146
147		//chill a bit
148		delay(100);  
149
150		digitalWrite(LED, LOW); // turn the LED off, we get one blink per cycle
151
152		//reverse motor and drive (bit slow) for between 0,5 to 2 seconds 
153		analogWrite(M1, 0);
154		analogWrite(M2, 180);
155		// while turning the wheels either one way or the other
156		stepperDir = random(0, 999);
157		if (stepperDir > 500){
158			stepper.step(-turningAmount);
159		} else {
160			stepper.step(turningAmount);
161		}
162		randomDelay = random(500, 2000);
163		delay(randomDelay);
164
165		//stop motor
166		analogWrite(M1, 0);
167		analogWrite(M2, 0);
168
169		// and turn wheels straight (program stops until wheels have turned)
170		if (stepperDir > 500){
171			stepper.step(turningAmount);
172		} else {
173			stepper.step(-turningAmount);
174		}
175		//deactivate stepper controller (it consumes power otherwise)
176			digitalWrite(8, LOW);
177			analogWrite(9, 0);
178			analogWrite(10, 0);
179			analogWrite(11, 0);
180
181}