Sweep Servo

/* Sweep
 by BARRAGAN <http://barraganstudio.com>
 This example code is in the public domain.

 modified 8 Nov 2013
 by Scott Fitzgerald
 http://www.arduino.cc/en/Tutorial/Sweep
*/

#include <Servo.h>
int una = 0;
Servo myservo;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int pos = 0;    // variable to store the servo position

void setup() {
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
}


void loop() {
  if (una <= 3) {
  for (pos = 0; pos <= 90; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  una = una +1 ;
  }
}

Auto Servo Sensor

// Setup the servo motor
    #include <Servo.h>
    Servo myservo;
    int servposnum = 0;
    int servpos = 0;
    // Setup Motor A (front and rear) pins
    int enableA = 1;
    int pinA1 = 3;
    int pinA2 = 2;
    // Setup Motor B (front and rear) pins
    int enableB = 6;
    int pinB1 = 5;
    int pinB2 = 4;
    int pinServo = 7;
    // Setup Ultrasonic Sensor pins
    #define trigPin 8
    #define echoPin 9

    int distanciasAngulos [5];
    intervaloMediciones = 500; 
    
    void setup() {
      // The setup code goes here and runs once only
      // Configure the pin modes for each drive motor
       pinMode (enableA, OUTPUT);
       pinMode (pinA1, OUTPUT);
       pinMode (pinA2, OUTPUT);
       pinMode (enableB, OUTPUT);
       pinMode (pinB1, OUTPUT);
       pinMode (pinB2, OUTPUT); 
       // Configure the pin modes for the Ultrasonic Sensor
       pinMode(trigPin, OUTPUT);
       pinMode(echoPin, INPUT);
       // Turn pin into servo driver. Calls pinMode. Returns 0 on failure.
       myservo.attach(pinServo);
       Serial.begin(9600);
    }
    void loop() {
      // Main code goes here and will run repeatedly:
         car(); // function keeps moving car forward while distance > 15cm
         avoid(); // function makes car go back, turn slightly right to move forward in new direction
         
    }
    // Create motor functions
    void motorAforward() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, LOW);
    }
    void motorBforward() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, HIGH);
    }
    void motorAbackward() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, HIGH);
    }
    void motorBbackward() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, LOW);
    }
    void motorAstop() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, HIGH);
    }
    void motorBstop() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, HIGH);
    }
    void motorAcoast() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, LOW);
    }
    void motorBcoast() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, LOW);
    }
    void motorAon() {
     digitalWrite (enableA, HIGH);
    }
    void motorBon() {
     digitalWrite (enableB, HIGH);
    }
    void motorAoff() {
     digitalWrite (enableA, LOW);
    }
    void motorBoff() {
     digitalWrite (enableB, LOW);
    }
    // Setup movement functions
    void forward (int duration) {
     motorAforward();
     motorBforward();
     delay (duration);
    }
    void backward (int duration) {
     motorAbackward();
     motorBbackward();
     delay (duration);
    }
    void right (int duration) {
     motorAbackward();
     motorBforward();
     delay (duration);
    }
    void left (int duration) {
     motorAforward();
     motorBbackward();
     delay (duration);
    }
    void coast (int duration) {
     motorAcoast();
     motorBcoast();
     delay (duration);
    }
    void breakRobot (int duration) {
     motorAstop();
     motorBstop();
     delay (duration);
    }
    void disableMotors() {
     motorAoff();
     motorBoff();
    }
    void enableMotors() {
     motorAon();
     motorBon();
    }
    // Setup Ultrasonic Sensor distance measuring
    int distance() {
      int duration, distance;
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;
      return distance;
    }
    // Setup the main car function
void car() {

    myservo.write(90);
    servposnum = 0;
    delay(100);
    distanciasAngulos [2] = distance();
      // Keep moving forward in a straight line while distance of objects > 15cm
      while(distanciasAngulos [2] > 15 || servposnum !=0)
      {
         // Keep moving servo motor back and forth to scan surroundings
         // This allows the ultrasonic sensor to see more to its left and right
         if(servposnum == 0)
         {
          myservo.write(0);
          servposnum = 1;
          delay(intervaloMediciones);  
          Serial.println("");   
         }
         else if(servposnum == 1)
         {
          myservo.write(45);
          servposnum = 2;
         delay(intervaloMediciones);
         }
         else if(servposnum  == 2)
         {
          myservo.write(90);
          servposnum = 3;
          delay(intervaloMediciones);
         }
         else if(servposnum == 3)
         {
          myservo.write(135);
          servposnum = 4;
          delay(intervaloMediciones);
         }
          else if(servposnum == 4)
         {
          myservo.write(180);
          servposnum = 0;
          delay(intervaloMediciones);
         }
         motorAon();
         motorBon();
         forward(1);    
         distanciasAngulos[servposnum] = distance();
         Serial.print(" ");
         Serial.print(servposnum);
         Serial.print(": ");
         Serial.print(distanciasAngulos[servposnum]);
         
      }
      breakRobot(0);
    }
    void avoid()
    { 
       Serial.println("");   
       Serial.println("Avoiding Obstacle ");   
       // Go back and turn slightly right to move car in new direction if object detected < 15cm away
       backward(500);
       if( distanciasAngulos [1] < 15 )
       {
          left(360);
          Serial.println("Moving left");   
       } 
       else if( distanciasAngulos [3] < 15 )
       {
          right(360);
          Serial.println("Moving right");   
       }        
        
    }

Codigo Auto Velocidad

// Setup Motor A (front and rear) pins
    int enableA = 4;
    int pinA1 = 5;
    int pinA2 = 6;
    // Setup Motor B (front and rear) pins
    int enableB = 8;
    int pinB1 = 9;
    int pinB2 = 10;
    // Setup Ultrasonic Sensor pins
    #define trigPin 2
    #define echoPin 3
    int distance_0;
    
    void setup() {
      // The setup code goes here and runs once only
      // Configure the pin modes for each drive motor
       pinMode (enableA, OUTPUT);
       pinMode (pinA1, OUTPUT);
       pinMode (pinA2, OUTPUT);
       pinMode (enableB, OUTPUT);
       pinMode (pinB1, OUTPUT);
       pinMode (pinB2, OUTPUT);
       // Configure the pin modes for the Ultrasonic Sensor
       pinMode(trigPin, OUTPUT);
       pinMode(echoPin, INPUT);
       
       Serial.begin(9600);
    }
    void loop() {
      // Main code goes here and will run repeatedly:
      car(); // function keeps moving car forward while distance of objects in front are > 15cm away
     //avoid(); // function makes car go back, turn slightly right to move forward in new direction
     //motorAon();
     //motorBon();
     //forward(1);
     //distance_0 = distance();
     Serial.println(distance_0);
    }
    // Create motor functions
    void motorAforward() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, LOW);
    }
    void motorBforward() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, HIGH);
    }
    
    void motorAforwardV(int v) {
     analogWrite (pinA1, v);
     digitalWrite (pinA2, LOW);
    }
    void motorBforwardV(int v) {
     digitalWrite (pinB1, LOW);
     analogWrite (pinB2, v);
    }

    
    void motorAbackward() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, HIGH);
    }
    void motorBbackward() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, LOW);
    }
    void motorAstop() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, HIGH);
    }
    void motorBstop() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, HIGH);
    }
    void motorAcoast() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, LOW);
    }
    void motorBcoast() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, LOW);
    }
    void motorAon() {
     digitalWrite (enableA, HIGH);
    }
    void motorBon() {
     digitalWrite (enableB, HIGH);
    }
    void motorAoff() {
     digitalWrite (enableA, LOW);
    }
    void motorBoff() {
     digitalWrite (enableB, LOW);
    }
    // Setup movement functions
    void forward (int duration) {
     motorAforward();
     motorBforward();
     delay (duration);
    }

    void velocidad (int duration, int v) {
     motorAforwardV(v);
     motorBforwardV(v);
     delay (duration);
    }
    void backward (int duration) {
     motorAbackward();
     motorBbackward();
     delay (duration);
    }
    void right (int duration) {
     motorAbackward();
     motorBforward();
     delay (duration);
    }
    void left (int duration) {
     motorAforward();
     motorBbackward();
     delay (duration);
    }
    void coast (int duration) {
     motorAcoast();
     motorBcoast();
     delay (duration);
    }
    void breakRobot (int duration) {
     motorAstop();
     motorBstop();
     delay (duration);
    }
    void disableMotors() {
     motorAoff();
     motorBoff();
    }
    void enableMotors() {
     motorAon();
     motorBon();
    }
    // Setup Ultrasonic Sensor distance measuring
    int distance() {
      int duration, distance;
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;
      return distance;
    }
    // Setup the main car function
    void car() {
    int distance_0;
    distance_0 = distance();
      // Keep moving forward in a straight line while distance of objects in front > 15cm away
      while(distance_0 < 255/3)
      {
         motorAon();
         motorBon();
         //forward(1);
         velocidad(1,255-distance_0*3);
         distance_0 = distance();
         Serial.println(distance_0);
      }
      breakRobot(0);
    }
    // Go back and turn slightly right to move car in new direction
    // This function only runs if an obstacle within 15cm is detected
    void avoid()
    {
        backward(500);
        right(360);
    }

Códigos Auto

Codigo Sensor y motores (Sin servo)

// Setup Motor A (front and rear) pins
    int enableA = 4;
    int pinA1 = 5;
    int pinA2 = 6;
    // Setup Motor B (front and rear) pins
    int enableB = 8;
    int pinB1 = 9;
    int pinB2 = 10;
    // Setup Ultrasonic Sensor pins
    #define trigPin 2
    #define echoPin 3
    int distance_0;
    
    void setup() {
      // The setup code goes here and runs once only
      // Configure the pin modes for each drive motor
       pinMode (enableA, OUTPUT);
       pinMode (pinA1, OUTPUT);
       pinMode (pinA2, OUTPUT);
       pinMode (enableB, OUTPUT);
       pinMode (pinB1, OUTPUT);
       pinMode (pinB2, OUTPUT);
       // Configure the pin modes for the Ultrasonic Sensor
       pinMode(trigPin, OUTPUT);
       pinMode(echoPin, INPUT);
       
       Serial.begin(9600);
    }
    void loop() {
      // Main code goes here and will run repeatedly:
     // car(); // function keeps moving car forward while distance of objects in front are > 15cm away
     // avoid(); // function makes car go back, turn slightly right to move forward in new direction
     motorAon();
     motorBon();
     forward(1);
     distance_0 = distance();
     Serial.println(distance_0);
    }
    // Create motor functions
    void motorAforward() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, LOW);
    }
    void motorBforward() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, HIGH);
    }
    void motorAbackward() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, HIGH);
    }
    void motorBbackward() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, LOW);
    }
    void motorAstop() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, HIGH);
    }
    void motorBstop() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, HIGH);
    }
    void motorAcoast() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, LOW);
    }
    void motorBcoast() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, LOW);
    }
    void motorAon() {
     digitalWrite (enableA, HIGH);
    }
    void motorBon() {
     digitalWrite (enableB, HIGH);
    }
    void motorAoff() {
     digitalWrite (enableA, LOW);
    }
    void motorBoff() {
     digitalWrite (enableB, LOW);
    }
    // Setup movement functions
    void forward (int duration) {
     motorAforward();
     motorBforward();
     delay (duration);
    }
    void backward (int duration) {
     motorAbackward();
     motorBbackward();
     delay (duration);
    }
    void right (int duration) {
     motorAbackward();
     motorBforward();
     delay (duration);
    }
    void left (int duration) {
     motorAforward();
     motorBbackward();
     delay (duration);
    }
    void coast (int duration) {
     motorAcoast();
     motorBcoast();
     delay (duration);
    }
    void breakRobot (int duration) {
     motorAstop();
     motorBstop();
     delay (duration);
    }
    void disableMotors() {
     motorAoff();
     motorBoff();
    }
    void enableMotors() {
     motorAon();
     motorBon();
    }
    // Setup Ultrasonic Sensor distance measuring
    int distance() {
      int duration, distance;
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;
      return distance;
    }
    // Setup the main car function
    void car() {
    int distance_0;
    distance_0 = distance();
      // Keep moving forward in a straight line while distance of objects in front > 15cm away
      while(distance_0 > 15)
      {
         motorAon();
         motorBon();
         forward(1);
         distance_0 = distance();
      }
      breakRobot(0);
    }
    // Go back and turn slightly right to move car in new direction
    // This function only runs if an obstacle within 15cm is detected
    void avoid()
    {
        backward(500);
        right(360);
    }

Codigo sensor servo y motores

    // Setup the servo motor
    #include <Servo.h>
    Servo myservo;
    int servposnum = 0;
    int servpos = 0;
    // Setup Motor A (front and rear) pins
    int enableA = 1;
    int pinA1 = 3;
    int pinA2 = 2;
    // Setup Motor B (front and rear) pins
    int enableB = 6;
    int pinB1 = 5;
    int pinB2 = 4;
    // Setup Ultrasonic Sensor pins
    #define trigPin 8
    #define echoPin 9
    void setup() {
      // The setup code goes here and runs once only
      // Configure the pin modes for each drive motor
       pinMode (enableA, OUTPUT);
       pinMode (pinA1, OUTPUT);
       pinMode (pinA2, OUTPUT);
       pinMode (enableB, OUTPUT);
       pinMode (pinB1, OUTPUT);
       pinMode (pinB2, OUTPUT); 
       // Configure the pin modes for the Ultrasonic Sensor
       pinMode(trigPin, OUTPUT);
       pinMode(echoPin, INPUT);
       // Turn pin into servo driver. Calls pinMode. Returns 0 on failure.
       myservo.attach(7);
    }
    void loop() {
      // Main code goes here and will run repeatedly:
         car(); // function keeps moving car forward while distance > 15cm
         avoid(); // function makes car go back, turn slightly right to move forward in new direction
         
    }
    // Create motor functions
    void motorAforward() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, LOW);
    }
    void motorBforward() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, HIGH);
    }
    void motorAbackward() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, HIGH);
    }
    void motorBbackward() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, LOW);
    }
    void motorAstop() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, HIGH);
    }
    void motorBstop() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, HIGH);
    }
    void motorAcoast() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, LOW);
    }
    void motorBcoast() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, LOW);
    }
    void motorAon() {
     digitalWrite (enableA, HIGH);
    }
    void motorBon() {
     digitalWrite (enableB, HIGH);
    }
    void motorAoff() {
     digitalWrite (enableA, LOW);
    }
    void motorBoff() {
     digitalWrite (enableB, LOW);
    }
    // Setup movement functions
    void forward (int duration) {
     motorAforward();
     motorBforward();
     delay (duration);
    }
    void backward (int duration) {
     motorAbackward();
     motorBbackward();
     delay (duration);
    }
    void right (int duration) {
     motorAbackward();
     motorBforward();
     delay (duration);
    }
    void left (int duration) {
     motorAforward();
     motorBbackward();
     delay (duration);
    }
    void coast (int duration) {
     motorAcoast();
     motorBcoast();
     delay (duration);
    }
    void breakRobot (int duration) {
     motorAstop();
     motorBstop();
     delay (duration);
    }
    void disableMotors() {
     motorAoff();
     motorBoff();
    }
    void enableMotors() {
     motorAon();
     motorBon();
    }
    // Setup Ultrasonic Sensor distance measuring
    int distance() {
      int duration, distance;
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;
      return distance;
    }
    // Setup the main car function
    void car() {
    int distance_0;
    distance_0 = distance();
      // Keep moving forward in a straight line while distance of objects > 15cm
      while(distance_0 > 15)
      {
         // Keep moving servo motor back and forth to scan surroundings
         // This allows the ultrasonic sensor to see more to its left and right
         if(servposnum == 0)
         {
          myservo.writeMicroseconds (1900);
          servposnum = 1;
          delay(100);     
         }
         else if(servposnum == 1)
         {
          myservo.writeMicroseconds (2200);
          servposnum = 2;
         delay(100);
         }
         else if(servposnum  == 2)
         {
          myservo.writeMicroseconds (1900);
          servposnum = 3;
          delay(100);
         }
         else if(servposnum == 3)
         {
          myservo.writeMicroseconds (1600);
          servposnum = 1;
          delay(100);
         }
         motorAon();
         motorBon();
         forward(1);    
         distance_0 = distance();
         
      }
      breakRobot(0);
    }
    void avoid()
    {
        // Go back and turn slightly right to move car in new direction if object detected < 15cm away
        backward(500);
        right(360);
    }

Código Sensor Ultrasónico

    /*
    * Ultrasonic Sensor HC-SR04 and Arduino Tutorial
    */
    // defines pins numbers
    const int trigPin = 9;
    const int echoPin = 10;
    // defines variables
    long duration;
    int distance;
    void setup() {
    pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
    pinMode(echoPin, INPUT); // Sets the echoPin as an Input
    Serial.begin(9600); // Starts the serial communication
    }
    void loop() {
    // Clears the trigPin
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    // Calculating the distance
    distance= duration*0.034/2;
    // Prints the distance on the Serial Monitor
    Serial.print("Distance: ");
    Serial.println(distance);
    }