Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*************************************************
- * Public Constants
- *************************************************/
- #define NOTE_B0 31
- #define NOTE_C1 33
- #define NOTE_CS1 35
- #define NOTE_D1 37
- #define NOTE_DS1 39
- #define NOTE_E1 41
- #define NOTE_F1 44
- #define NOTE_FS1 46
- #define NOTE_G1 49
- #define NOTE_GS1 52
- #define NOTE_A1 55
- #define NOTE_AS1 58
- #define NOTE_B1 62
- #define NOTE_C2 65
- #define NOTE_CS2 69
- #define NOTE_D2 73
- #define NOTE_DS2 78
- #define NOTE_E2 82
- #define NOTE_F2 87
- #define NOTE_FS2 93
- #define NOTE_G2 98
- #define NOTE_GS2 104
- #define NOTE_A2 110
- #define NOTE_AS2 117
- #define NOTE_B2 123
- #define NOTE_C3 131
- #define NOTE_CS3 139
- #define NOTE_D3 147
- #define NOTE_DS3 156
- #define NOTE_E3 165
- #define NOTE_F3 175
- #define NOTE_FS3 185
- #define NOTE_G3 196
- #define NOTE_GS3 208
- #define NOTE_A3 220
- #define NOTE_AS3 233
- #define NOTE_B3 247
- #define NOTE_C4 262
- #define NOTE_CS4 277
- #define NOTE_D4 294
- #define NOTE_DS4 311
- #define NOTE_E4 330
- #define NOTE_F4 349
- #define NOTE_FS4 370
- #define NOTE_G4 392
- #define NOTE_GS4 415
- #define NOTE_A4 440
- #define NOTE_AS4 466
- #define NOTE_B4 494
- #define NOTE_C5 523
- #define NOTE_CS5 554
- #define NOTE_D5 587
- #define NOTE_DS5 622
- #define NOTE_E5 659
- #define NOTE_F5 698
- #define NOTE_FS5 740
- #define NOTE_G5 784
- #define NOTE_GS5 831
- #define NOTE_A5 880
- #define NOTE_AS5 932
- #define NOTE_B5 988
- #define NOTE_C6 1047
- #define NOTE_CS6 1109
- #define NOTE_D6 1175
- #define NOTE_DS6 1245
- #define NOTE_E6 1319
- #define NOTE_F6 1397
- #define NOTE_FS6 1480
- #define NOTE_G6 1568
- #define NOTE_GS6 1661
- #define NOTE_A6 1760
- #define NOTE_AS6 1865
- #define NOTE_B6 1976
- #define NOTE_C7 2093
- #define NOTE_CS7 2217
- #define NOTE_D7 2349
- #define NOTE_DS7 2489
- #define NOTE_E7 2637
- #define NOTE_F7 2794
- #define NOTE_FS7 2960
- #define NOTE_G7 3136
- #define NOTE_GS7 3322
- #define NOTE_A7 3520
- #define NOTE_AS7 3729
- #define NOTE_B7 3951
- #define NOTE_C8 4186
- #define NOTE_CS8 4435
- #define NOTE_D8 4699
- #define NOTE_DS8 4978
- #include <AFMotor.h> // Enables the Motor library
- #include <Servo.h> // Enables the Servo library
- //#include "pitches.h"
- // notes in the melody:
- int melody[] = {
- NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4
- };
- // note durations: 4 = quarter note, 8 = eighth note, etc.:
- int noteDurations[] = {
- 4, 8, 8, 4, 4, 4, 4, 4
- };
- long randNumber;
- //Servo PingServo;
- AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
- AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
- AF_DCMotor motor3(3); // Motor 3 is connected to the port 1 on the motor shield
- AF_DCMotor motor4(4); // Motor 4 is connected to the port 2 on the motor shield
- int minSafeDist = 15 ; // Minimum distance for ping sensor to know when to turn
- //int pingPin = 30; // Ping sensor is connected to port A0
- int trigPin = 30;
- int echoPin = 31;
- float pingTime;
- float targetDistance;
- float speedOfSound = 776.5;
- int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance
- long duration, inches, cm; // Define variables for Ping sensor
- int trigPinL = 34;
- int echoPinL = 35;
- float pingTimeL;
- float targetDistanceL;
- long inchesL;
- int beepPin = 36;
- void beep(unsigned char delayms){
- for (int thisNote = 0; thisNote < 8; thisNote++) {
- // to calculate the note duration, take one second
- // divided by the note type.
- //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
- int noteDuration = 1000 / noteDurations[thisNote];
- tone(beepPin, melody[thisNote], noteDuration);
- // to distinguish the notes, set a minimum time between them.
- // the note's duration + 30% seems to work well:
- int pauseBetweenNotes = noteDuration * 1.30;
- delay(pauseBetweenNotes);
- // stop the tone playing:
- noTone(beepPin);
- }
- }
- void setup() {
- //PingServo.attach(10); // Servo is attached to pin 10 in the motor shield
- //PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees)
- pinMode(trigPin, OUTPUT);
- pinMode(echoPin, INPUT);
- pinMode(beepPin, OUTPUT);
- 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.)
- 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.)
- 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.)
- motor4.setSpeed(200);
- Serial.begin(9600); // Enables Serial monitor for debugging purposes
- Serial.println("Serial test!"); // Test the Serial communication
- }
- void AllStop() {
- motor1.run(RELEASE); // Turns off motor 1
- motor2.run(RELEASE); // Turns off motor 2
- motor3.run(RELEASE); // Turns off motor 3
- motor4.run(RELEASE);
- }
- void AllForward() { // Makes the robot go forward
- motor1.run(FORWARD); // Motor 1 goes forward
- motor2.run(FORWARD); // Motor 2 goes forward
- //delay(100);
- //if(inches >= minSafeDist && inchesL >= minSafeDist){
- motor3.run(FORWARD); // Motor 3 goes forward
- motor4.run(FORWARD); // Motor 4 goes forward
- //}
- Serial.println("Going forward"); // Prints a line in the serial monitor
- }
- void turnRight() { // Makes the robot go right
- motor2.run(BACKWARD); // Turns off motor 2
- motor1.run(FORWARD); // Motor 1 goes forward
- delay(1600); // Time required to turn right (1.6 seconds)
- Serial.println("Motors going Right"); // Prints a line in the serial monitor
- }
- void GoBack(){ // Makes the robot go back
- motor2.run(BACKWARD); // Motor 2 goes back
- motor1.run(BACKWARD); // Motor 1 goes back
- delay(1600); // Time Required to go back (1.6 seconds)
- Serial.println("Backward"); // Prints a line in the serial monitor
- }
- void turnLeft() { // Makes the robot go Left
- motor2.run(FORWARD); // Motor 2 goes forward
- motor1.run(BACKWARD); // turns off motor 1
- delay(1600); //Time Required to turn left (1.6)Seconds
- Serial.println("Motors going Left");// Prints a line in the serial monitor
- }
- // Starts the loop to decide what to do
- void loop()
- {
- LookAhead();
- Serial.print(inches);
- Serial.println(" inches"); // Prints a line in the serial monitor
- Serial.println(inches);
- 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*/
- {
- AllForward(); // All wheels forward
- //delay(110); // Wait 0.11 seconds
- }else if (inches < minSafeDist) // If not:
- {
- Serial.print("Turn right");
- beep(200);
- AllStop();
- //randNumber = random(10, 20);
- //if(randNumber < 10.) {
- // turnLeft();
- // }
- //else {
- turnRight();
- //}
- //turnLeft();
- } /*else if (inchesL < minSafeDist) {
- Serial.print("Turn right");
- AllStop();
- turnRight();
- }*/
- /*{
- AllStop(); // Stop all motors
- LookAround(); // Check your surroundings for best route
- if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right
- {
- turnRight();
- }else if (leftDist > rightDist) // If the left distance is greater than the right distance , turn left
- {
- turnLeft();
- }else if (leftDist&&rightDist<minSafeDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back
- {
- GoBack();
- }
- }*/
- }
- unsigned long ping() {
- digitalWrite(trigPin, LOW);
- delayMicroseconds(2000);
- digitalWrite(trigPin, HIGH);
- delayMicroseconds(15);
- digitalWrite(trigPin, LOW);
- pingTime = pulseIn(echoPin, HIGH);
- pingTime = pingTime/1000000.;
- pingTime = pingTime/3600.;
- targetDistance = speedOfSound * pingTime;
- targetDistance = targetDistance/2;
- inches = targetDistance*63360; //Convert target distance to inches from mile;
- Serial.print("targetDistance");
- }
- unsigned long pingL() {
- digitalWrite(trigPinL, LOW);
- delayMicroseconds(2000);
- digitalWrite(trigPinL, HIGH);
- delayMicroseconds(15);
- digitalWrite(trigPinL, LOW);
- pingTimeL = pulseIn(echoPinL, HIGH);
- pingTimeL = pingTimeL/1000000.;
- pingTimeL = pingTimeL/3600.;
- targetDistanceL = speedOfSound * pingTimeL;
- targetDistanceL = targetDistanceL/2;
- inchesL = targetDistanceL*63360; //Convert target distance to inches from mile;
- Serial.print("targetDistanceL");
- Serial.print(inchesL);
- }
- long microsecondsToInches(long microseconds) // converts time to a distance
- {
- return microseconds / 74 / 2;
- }
- long microsecondsToCentimeters(long microseconds) // converts time to a distance
- {
- return microseconds / 29 / 2;
- }
- void LookAhead() {
- //PingServo.write(90);// angle to look forward
- delay(175); // wait 0.175 seconds
- ping();
- //if(inches < minSafeDist) {AllStop();}
- //pingL();
- //if(inchesL < minSafeDist) {AllStop();}
- }
- void LookAround(){
- //PingServo.write(180); // 180° angle
- //delay(320); // wait 0.32 seconds
- ping();
- pingL();
- //rightDist = inches; //get the right distance
- //PingServo.write(0); // look to the other side
- //delay(620); // wait 0.62 seconds
- //ping();
- //leftDist = inches; // get the left distance
- //PingServo.write(90); // 90° angle
- //delay(275); // wait 0.275 seconds
- // Prints a line in the serial monitor
- //Serial.print("RightDist: ");
- //Serial.println(rightDist);
- //Serial.print("LeftDist: ");
- //Serial.println(leftDist);
- //Serial.print("CenterDist: ");
- //Serial.println(centerDist);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement