SHOW:
|
|
- or go back to the newest paste.
1 | /************************************************* | |
2 | * Public Constants | |
3 | *************************************************/ | |
4 | ||
5 | #define NOTE_B0 31 | |
6 | #define NOTE_C1 33 | |
7 | #define NOTE_CS1 35 | |
8 | #define NOTE_D1 37 | |
9 | #define NOTE_DS1 39 | |
10 | #define NOTE_E1 41 | |
11 | #define NOTE_F1 44 | |
12 | #define NOTE_FS1 46 | |
13 | #define NOTE_G1 49 | |
14 | #define NOTE_GS1 52 | |
15 | #define NOTE_A1 55 | |
16 | #define NOTE_AS1 58 | |
17 | #define NOTE_B1 62 | |
18 | #define NOTE_C2 65 | |
19 | #define NOTE_CS2 69 | |
20 | #define NOTE_D2 73 | |
21 | #define NOTE_DS2 78 | |
22 | #define NOTE_E2 82 | |
23 | #define NOTE_F2 87 | |
24 | #define NOTE_FS2 93 | |
25 | #define NOTE_G2 98 | |
26 | #define NOTE_GS2 104 | |
27 | #define NOTE_A2 110 | |
28 | #define NOTE_AS2 117 | |
29 | #define NOTE_B2 123 | |
30 | #define NOTE_C3 131 | |
31 | #define NOTE_CS3 139 | |
32 | #define NOTE_D3 147 | |
33 | #define NOTE_DS3 156 | |
34 | #define NOTE_E3 165 | |
35 | #define NOTE_F3 175 | |
36 | #define NOTE_FS3 185 | |
37 | #define NOTE_G3 196 | |
38 | #define NOTE_GS3 208 | |
39 | #define NOTE_A3 220 | |
40 | #define NOTE_AS3 233 | |
41 | #define NOTE_B3 247 | |
42 | #define NOTE_C4 262 | |
43 | #define NOTE_CS4 277 | |
44 | #define NOTE_D4 294 | |
45 | #define NOTE_DS4 311 | |
46 | #define NOTE_E4 330 | |
47 | #define NOTE_F4 349 | |
48 | #define NOTE_FS4 370 | |
49 | #define NOTE_G4 392 | |
50 | #define NOTE_GS4 415 | |
51 | #define NOTE_A4 440 | |
52 | #define NOTE_AS4 466 | |
53 | #define NOTE_B4 494 | |
54 | #define NOTE_C5 523 | |
55 | #define NOTE_CS5 554 | |
56 | #define NOTE_D5 587 | |
57 | #define NOTE_DS5 622 | |
58 | #define NOTE_E5 659 | |
59 | #define NOTE_F5 698 | |
60 | #define NOTE_FS5 740 | |
61 | #define NOTE_G5 784 | |
62 | #define NOTE_GS5 831 | |
63 | #define NOTE_A5 880 | |
64 | #define NOTE_AS5 932 | |
65 | #define NOTE_B5 988 | |
66 | #define NOTE_C6 1047 | |
67 | #define NOTE_CS6 1109 | |
68 | #define NOTE_D6 1175 | |
69 | #define NOTE_DS6 1245 | |
70 | #define NOTE_E6 1319 | |
71 | #define NOTE_F6 1397 | |
72 | #define NOTE_FS6 1480 | |
73 | #define NOTE_G6 1568 | |
74 | #define NOTE_GS6 1661 | |
75 | #define NOTE_A6 1760 | |
76 | #define NOTE_AS6 1865 | |
77 | #define NOTE_B6 1976 | |
78 | #define NOTE_C7 2093 | |
79 | #define NOTE_CS7 2217 | |
80 | #define NOTE_D7 2349 | |
81 | #define NOTE_DS7 2489 | |
82 | #define NOTE_E7 2637 | |
83 | #define NOTE_F7 2794 | |
84 | #define NOTE_FS7 2960 | |
85 | #define NOTE_G7 3136 | |
86 | #define NOTE_GS7 3322 | |
87 | #define NOTE_A7 3520 | |
88 | #define NOTE_AS7 3729 | |
89 | #define NOTE_B7 3951 | |
90 | #define NOTE_C8 4186 | |
91 | #define NOTE_CS8 4435 | |
92 | #define NOTE_D8 4699 | |
93 | #define NOTE_DS8 4978 | |
94 | ||
95 | #include <AFMotor.h> // Enables the Motor library | |
96 | #include <Servo.h> // Enables the Servo library | |
97 | //#include "pitches.h" | |
98 | ||
99 | // notes in the melody: | |
100 | int melody[] = { | |
101 | NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4 | |
102 | }; | |
103 | ||
104 | // note durations: 4 = quarter note, 8 = eighth note, etc.: | |
105 | int noteDurations[] = { | |
106 | 4, 8, 8, 4, 4, 4, 4, 4 | |
107 | }; | |
108 | long randNumber; | |
109 | //Servo PingServo; | |
110 | AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield | |
111 | AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield | |
112 | AF_DCMotor motor3(3); // Motor 3 is connected to the port 1 on the motor shield | |
113 | AF_DCMotor motor4(4); // Motor 4 is connected to the port 2 on the motor shield | |
114 | int minSafeDist = 15 ; // Minimum distance for ping sensor to know when to turn | |
115 | //int pingPin = 30; // Ping sensor is connected to port A0 | |
116 | int trigPin = 30; | |
117 | int echoPin = 31; | |
118 | float pingTime; | |
119 | float targetDistance; | |
120 | float speedOfSound = 776.5; | |
121 | int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance | |
122 | long duration, inches, cm; // Define variables for Ping sensor | |
123 | ||
124 | int trigPinL = 34; | |
125 | int echoPinL = 35; | |
126 | float pingTimeL; | |
127 | float targetDistanceL; | |
128 | long inchesL; | |
129 | ||
130 | int beepPin = 36; | |
131 | ||
132 | void beep(unsigned char delayms){ | |
133 | for (int thisNote = 0; thisNote < 8; thisNote++) { | |
134 | ||
135 | // to calculate the note duration, take one second | |
136 | // divided by the note type. | |
137 | //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc. | |
138 | int noteDuration = 1000 / noteDurations[thisNote]; | |
139 | tone(beepPin, melody[thisNote], noteDuration); | |
140 | ||
141 | // to distinguish the notes, set a minimum time between them. | |
142 | // the note's duration + 30% seems to work well: | |
143 | int pauseBetweenNotes = noteDuration * 1.30; | |
144 | delay(pauseBetweenNotes); | |
145 | // stop the tone playing: | |
146 | noTone(beepPin); | |
147 | } | |
148 | } | |
149 | ||
150 | ||
151 | void setup() { | |
152 | //PingServo.attach(10); // Servo is attached to pin 10 in the motor shield | |
153 | //PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees) | |
154 | pinMode(trigPin, OUTPUT); | |
155 | pinMode(echoPin, INPUT); | |
156 | pinMode(beepPin, OUTPUT); | |
157 | motor1.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) | |
158 | motor2.setSpeed(200); // Sets the speed of the second motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) | |
159 | motor3.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) | |
160 | motor4.setSpeed(200); | |
161 | Serial.begin(9600); // Enables Serial monitor for debugging purposes | |
162 | Serial.println("Serial test!"); // Test the Serial communication | |
163 | ||
164 | } | |
165 | ||
166 | void AllStop() { | |
167 | motor1.run(RELEASE); // Turns off motor 1 | |
168 | motor2.run(RELEASE); // Turns off motor 2 | |
169 | motor3.run(RELEASE); // Turns off motor 3 | |
170 | motor4.run(RELEASE); | |
171 | } | |
172 | void AllForward() { // Makes the robot go forward | |
173 | motor1.run(FORWARD); // Motor 1 goes forward | |
174 | motor2.run(FORWARD); // Motor 2 goes forward | |
175 | //delay(100); | |
176 | //if(inches >= minSafeDist && inchesL >= minSafeDist){ | |
177 | motor3.run(FORWARD); // Motor 3 goes forward | |
178 | motor4.run(FORWARD); // Motor 4 goes forward | |
179 | //} | |
180 | Serial.println("Going forward"); // Prints a line in the serial monitor | |
181 | } | |
182 | void turnRight() { // Makes the robot go right | |
183 | motor2.run(BACKWARD); // Turns off motor 2 | |
184 | motor1.run(FORWARD); // Motor 1 goes forward | |
185 | delay(1600); // Time required to turn right (1.6 seconds) | |
186 | Serial.println("Motors going Right"); // Prints a line in the serial monitor | |
187 | } | |
188 | void GoBack(){ // Makes the robot go back | |
189 | motor2.run(BACKWARD); // Motor 2 goes back | |
190 | motor1.run(BACKWARD); // Motor 1 goes back | |
191 | delay(1600); // Time Required to go back (1.6 seconds) | |
192 | Serial.println("Backward"); // Prints a line in the serial monitor | |
193 | } | |
194 | void turnLeft() { // Makes the robot go Left | |
195 | motor2.run(FORWARD); // Motor 2 goes forward | |
196 | motor1.run(BACKWARD); // turns off motor 1 | |
197 | delay(1600); //Time Required to turn left (1.6)Seconds | |
198 | Serial.println("Motors going Left");// Prints a line in the serial monitor | |
199 | } | |
200 | ||
201 | // Starts the loop to decide what to do | |
202 | void loop() | |
203 | { | |
204 | LookAhead(); | |
205 | Serial.print(inches); | |
206 | Serial.println(" inches"); // Prints a line in the serial monitor | |
207 | Serial.println(inches); | |
208 | if(inches >= minSafeDist || inchesL >= minSafeDist) /* If the inches in front of an object is greater than or equal to the minimum safe distance (11 inches), react*/ | |
209 | { | |
210 | AllForward(); // All wheels forward | |
211 | //delay(110); // Wait 0.11 seconds | |
212 | }else if (inches < minSafeDist) // If not: | |
213 | { | |
214 | Serial.print("Turn right"); | |
215 | beep(200); | |
216 | AllStop(); | |
217 | //randNumber = random(10, 20); | |
218 | //if(randNumber < 10.) { | |
219 | // turnLeft(); | |
220 | // } | |
221 | //else { | |
222 | turnRight(); | |
223 | //} | |
224 | //turnLeft(); | |
225 | } /*else if (inchesL < minSafeDist) { | |
226 | Serial.print("Turn right"); | |
227 | AllStop(); | |
228 | turnRight(); | |
229 | }*/ | |
230 | /*{ | |
231 | AllStop(); // Stop all motors | |
232 | LookAround(); // Check your surroundings for best route | |
233 | if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right | |
234 | { | |
235 | turnRight(); | |
236 | }else if (leftDist > rightDist) // If the left distance is greater than the right distance , turn left | |
237 | { | |
238 | turnLeft(); | |
239 | }else if (leftDist&&rightDist<minSafeDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back | |
240 | { | |
241 | GoBack(); | |
242 | } | |
243 | }*/ | |
244 | } | |
245 | ||
246 | unsigned long ping() { | |
247 | digitalWrite(trigPin, LOW); | |
248 | delayMicroseconds(2000); | |
249 | digitalWrite(trigPin, HIGH); | |
250 | delayMicroseconds(15); | |
251 | digitalWrite(trigPin, LOW); | |
252 | pingTime = pulseIn(echoPin, HIGH); | |
253 | pingTime = pingTime/1000000.; | |
254 | pingTime = pingTime/3600.; | |
255 | targetDistance = speedOfSound * pingTime; | |
256 | targetDistance = targetDistance/2; | |
257 | inches = targetDistance*63360; //Convert target distance to inches from mile; | |
258 | Serial.print("targetDistance"); | |
259 | } | |
260 | ||
261 | unsigned long pingL() { | |
262 | digitalWrite(trigPinL, LOW); | |
263 | delayMicroseconds(2000); | |
264 | digitalWrite(trigPinL, HIGH); | |
265 | delayMicroseconds(15); | |
266 | digitalWrite(trigPinL, LOW); | |
267 | pingTimeL = pulseIn(echoPinL, HIGH); | |
268 | pingTimeL = pingTimeL/1000000.; | |
269 | pingTimeL = pingTimeL/3600.; | |
270 | targetDistanceL = speedOfSound * pingTimeL; | |
271 | targetDistanceL = targetDistanceL/2; | |
272 | inchesL = targetDistanceL*63360; //Convert target distance to inches from mile; | |
273 | Serial.print("targetDistanceL"); | |
274 | Serial.print(inchesL); | |
275 | } | |
276 | ||
277 | long microsecondsToInches(long microseconds) // converts time to a distance | |
278 | { | |
279 | return microseconds / 74 / 2; | |
280 | } | |
281 | long microsecondsToCentimeters(long microseconds) // converts time to a distance | |
282 | { | |
283 | return microseconds / 29 / 2; | |
284 | } | |
285 | ||
286 | void LookAhead() { | |
287 | //PingServo.write(90);// angle to look forward | |
288 | delay(175); // wait 0.175 seconds | |
289 | ping(); | |
290 | //if(inches < minSafeDist) {AllStop();} | |
291 | //pingL(); | |
292 | //if(inchesL < minSafeDist) {AllStop();} | |
293 | } | |
294 | ||
295 | void LookAround(){ | |
296 | //PingServo.write(180); // 180° angle | |
297 | //delay(320); // wait 0.32 seconds | |
298 | ping(); | |
299 | pingL(); | |
300 | //rightDist = inches; //get the right distance | |
301 | //PingServo.write(0); // look to the other side | |
302 | //delay(620); // wait 0.62 seconds | |
303 | //ping(); | |
304 | //leftDist = inches; // get the left distance | |
305 | //PingServo.write(90); // 90° angle | |
306 | //delay(275); // wait 0.275 seconds | |
307 | ||
308 | // Prints a line in the serial monitor | |
309 | //Serial.print("RightDist: "); | |
310 | //Serial.println(rightDist); | |
311 | //Serial.print("LeftDist: "); | |
312 | //Serial.println(leftDist); | |
313 | //Serial.print("CenterDist: "); | |
314 | //Serial.println(centerDist); | |
315 | } |