Vous êtes sur la page 1sur 2

int izqA = 5;

int izqB = 6;
int derA = 9;
int derB = 10;
int vel = 200; //
int vel2 = 30;
int vel3 = 30;
#define Ptrig 7
#define Pecho 8
long duracion, distancia;
void setup() {
Serial.begin (9600);
pinMode(derA, OUTPUT);
pinMode(derB, OUTPUT);
pinMode(izqA, OUTPUT);
pinMode(izqB, OUTPUT);
Serial.begin (9600);
pinMode(Pecho, INPUT); // define el pin 6 como entrada (echo)
pinMode(Ptrig, OUTPUT); // define el pin 7 como salida (triger)
delay(5000);
}
void loop() {
digitalWrite(Ptrig, LOW);
delayMicroseconds(2);
digitalWrite(Ptrig, HIGH); // genera el pulso de triger por 10ms
delayMicroseconds(10);
digitalWrite(Ptrig, LOW);
int SenFr = analogRead(A0);
int SenTr = analogRead(A1);

duracion = pulseIn(Pecho, HIGH);


distancia = (duracion/2) / 29;
float ValorSenF = (float)SenFr * 5.0 / 511.0;
float ValorSenT = (float)SenTr * 5.0 / 511.0;
if (distancia <= 80 && distancia >= 1){
avanzar();
Serial.print(distancia);
Serial.println("cm");
}
else
{
izquierda();

if (ValorSenF <= 9){


retroceder();
delay(1000);
izquierda();
delay(500);
}
if (ValorSenT <= 9){
avanzar2();
delay(100);
izquierda();
delay(500);
}
}
}
void retroceder()
{
analogWrite(derB, 0);
analogWrite(izqB, 0);
analogWrite(derA, vel2);
analogWrite(izqA, vel2);
}
void avanzar()
{
analogWrite(derB, vel);
analogWrite(izqB, vel);
analogWrite(derA, 0);
analogWrite(izqA, 0);
}
void derecha()
{
analogWrite(derA, vel2);
analogWrite(izqA, 0);
analogWrite(derB, 0);
analogWrite(izqB, vel2);
}
void izquierda()
{
analogWrite(derA, 0);
analogWrite(izqA, vel2);
analogWrite(derB, vel2);
analogWrite(izqB,0);
}
void avanzar2()
{
analogWrite(derB, 0);
analogWrite(izqB, 0);
analogWrite(derA, vel2);
analogWrite(izqA, vel2);
}

Vous aimerez peut-être aussi