Friday.ino

#include <Servo.h>
#include <Ultrasonic.h>

//Define os pinos para o trigger e echo

#define pino_trigger 7
#define pino_echo 6

int dir1PinA = 2;
int dir2PinA = 3;
int speedPinA = 9;

int dir1PinB = 4;
int dir2PinB = 5;
int speedPinB = 10;

Servo servo;

//Inicializa o sensor nos pinos definidos acima
Ultrasonic ultrasonic(pino_trigger, pino_echo);

int pos = 90;

void Motor(int status){
  if (status == 1)
   {
     Serial.println("Direita!!!\n");
     analogWrite(speedPinA, 255);
     digitalWrite(dir1PinA, HIGH);
     digitalWrite(dir2PinA, LOW);

     analogWrite(speedPinB, 255);
     digitalWrite(dir1PinB, LOW);
     digitalWrite(dir2PinB, HIGH);


   }
  if (status == 2)
  {
     Serial.println("Esquerda!!!\n");
     analogWrite(speedPinA, 255);//Sets speed variable via PWM
     digitalWrite(dir1PinA, LOW);
     digitalWrite(dir2PinA, HIGH);

     analogWrite(speedPinB, 255);
     digitalWrite(dir1PinB, HIGH);
     digitalWrite(dir2PinB, LOW);

  }

  if (status == 3)
  {
     Serial.println("Frente!!!\n");
     analogWrite(speedPinB, 255);
     digitalWrite(dir1PinB, LOW);
     digitalWrite(dir2PinB, HIGH);

     analogWrite(speedPinA, 255);//Sets speed variable via PWM
     digitalWrite(dir1PinA, LOW);
     digitalWrite(dir2PinA, HIGH);



  }
  if (status == 4)
  {
     Serial.println("Back!!!\n");
     analogWrite(speedPinA, 255);
     digitalWrite(dir1PinA, HIGH);
     digitalWrite(dir2PinA, LOW);

     analogWrite(speedPinB, 255);
     digitalWrite(dir1PinB, HIGH);
     digitalWrite(dir2PinB, LOW);

  }
  if (status == 5)
  {
     Serial.println("STOP!!!\n");
     analogWrite(speedPinA, 0);
     digitalWrite(dir1PinA, LOW);
     digitalWrite(dir2PinA, HIGH);

     analogWrite(speedPinB, 0);
     digitalWrite(dir1PinB, LOW);
     digitalWrite(dir2PinB, HIGH);

  }
}
void ServoMotor(int status){

  if (status == 1)
  {
     //Serial.println("Direita - 180 Graus\n");
     for (pos = 15; pos<= 165 ; pos++)
     {
      servo.write(pos);
      delay(30);
     }
  }
  if (status == 2)
  {
    //Serial.println("Esquerda - 0 Graus\n");
    for (pos = 165; pos > 15; pos--)
    {
         servo.write(pos);
         delay(30);
    }
  }
  if (status == 3)
  {
    //Serial.println("Frente - 90 Graus\n");
    servo.write(90);
    delay(30);
  }
}

int  leitura_Sensor(){
  //Serial.println("Lendo dados do sensor...");
  float cmMsec;
  long microsec = ultrasonic.timing();
  cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);
  //Serial.println(cmMsec);
  int sensor = cmMsec;
  return sensor;
}

int ServoAndSensor(){
   int distancia;
   int pos = 0;
     for (pos = 15; pos<= 165 ; pos++)
     {
      servo.write(pos);
      delay(30);
      distancia =  leitura_Sensor();
      Serial.println(distancia);
     }


    for (pos = 165; pos > 15; pos--)
    {
         servo.write(pos);
         delay(30);
         distancia =  leitura_Sensor();
         Serial.println(distancia);
    }

}




void setup() {
  Serial.begin(9600);
  servo.attach(11);
  servo.write(pos);

  pinMode(dir1PinA,OUTPUT);
  pinMode(dir2PinA,OUTPUT);
  pinMode(speedPinA,OUTPUT);

  pinMode(speedPinB,OUTPUT);
  pinMode(dir2PinB,OUTPUT);
  pinMode(speedPinB,OUTPUT);
}


void loop() {
  ServoAndSensor();

}