Ultrasonidos y esquivar obstaculo

International Español Software
1

buenas, necesitaba preguntar por ayuda ya que llevo poco programando y estoy atascado con un codigo que necesito sacar rapido.

es para un coche con dos motores, dos sensores CYN70 y un sensor de ultrasonidos, debe seguir una linea negra y cuando encuentre un obstaculo debe esquivarlo por la derecha siempre haciendo un circulo, el problema es que no soy capaz de que al detectar el obstaculo esquive el objeto.
// Definición de pines para motores

// Definición de pines para motores
int pinLB = 12;  // motor izquierdo - adelante
int pinLF = 3;   // motor izquierdo - atrás 
int pinRB = 13;  // motor derecho - adelante
int pinRF = 11;  // motor derecho - atrás 

int inputPin = 5;    // define pin sensor echo
int outputPin =6;    // define pin sensor trig
// Definición de pines para sensores CNY70
int leftSensorPin = A0;   // sensor izquierdo
int rightSensorPin = A1;  // sensor derecho


int cm


void avanzar();
void girarIzquierda();
void girarDerecha();
void rodearobstaculo();


  int leftSensorValue = analogRead(leftSensorPin);
  int rightSensorValue = analogRead(rightSensorPin);

void setup() {

  // ultrasonidos
  Serial.begin(9600);
  
  pinMode(triggerEmisor,OUTPUT); 
  pinMode(echoReceptor,INPUT);  


  // Configurar pines de los sensores
  pinMode(leftSensorPin, INPUT);
  pinMode(rightSensorPin, INPUT);

  pinMode(inputPin, INPUT);   
  pinMode(outputPin, OUTPUT);  

  // Configurar pines de los motores
  pinMode(pinLB, OUTPUT);
  pinMode(pinLF, OUTPUT);
  pinMode(pinRB, OUTPUT);
  pinMode(pinRF, OUTPUT); 

}

void loop() {

  // Leer los valores de los sensores
  int leftSensorValue = analogRead(leftSensorPin);
  int rightSensorValue = analogRead(rightSensorPin);

  //lectura ultrasonidos
   int cm = ping(TriggerPin, EchoPin);
  Serial.print("Distancia: ");
  Serial.println(cm);
  delay(1000);

  // Lógica de seguimiento de línea
if (leftSensorValue > 800 && rightSensorValue > 800) {
    avanzar();
  } else if (leftSensorValue < 700 && rightSensorValue > 500) {
    girarIzquierda();
  } else if (leftSensorValue > 500 && rightSensorValue < 700) {
    girarDerecha();
  }else if(distanceCm>5)
  rodearobstaculo();
}
  

void avanzar() {
  digitalWrite(pinLB, HIGH);
  analogWrite(pinLF, 255);  
  digitalWrite(pinRB, HIGH);
  analogWrite(pinRF, 255);  
} 

void girarIzquierda() {
  digitalWrite(pinRB, HIGH);
  analogWrite(pinRF, 150);  
  digitalWrite(pinLB, LOW);
  analogWrite(pinLB, 0);   
}

void girarDerecha() {
  digitalWrite(pinRB, LOW);
  analogWrite(pinRF, 0);   
  digitalWrite(pinLB, HIGH);
  analogWrite(pinLF, 150); 
}

void rodearobstaculo() {

  
 // Girar hacia la derecha
  digitalWrite(pinRB, LOW);
  analogWrite(pinRF, 0);
  digitalWrite(pinLB, HIGH);
  analogWrite(pinLF, 150); 
  delay(200);

  // Avanzar recto
  avanzar();
  delay(500);

  // Girar hacia la izquierda
  digitalWrite(pinRB, HIGH);
  analogWrite(pinRF, 150);  
  digitalWrite(pinLB, LOW);
  analogWrite(pinLB, 0);   

  if (leftSensorValue > 800 && rightSensorValue > 800) {
    avanzar();
  } else if (leftSensorValue < 700 && rightSensorValue > 500) {
    girarIzquierda();
  } else if (leftSensorValue > 500 && rightSensorValue < 700) {
    girarDerecha();
}
}

int ping(int TriggerPin, int EchoPin) {
  long duration, distanceCm;
  
  digitalWrite(TriggerPin, LOW);  //para generar un pulso limpio ponemos a LOW 4us
  delayMicroseconds(4);
  digitalWrite(TriggerPin, HIGH);  //generamos Trigger (disparo) de 10us
  delayMicroseconds(10);
  digitalWrite(TriggerPin, LOW);
  
  duration = pulseIn(EchoPin, HIGH);  //medimos el tiempo entre pulsos, en microsegundos
  
  distanceCm = duration * 10 / 292/ 2;   //convertimos a distancia, en cm
  return distanceCm;
}
2

Falta saber que hardware estas usando para poder responderte.
Esquema por favor.

3

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.