Robot autonomo con Arduino

International Italiano Software
5

Ho cambiato un pò il funzionamento, anche dopo aver letto di altri robot online.
Ecco il nuovo sketch funzionante:

#include <Servo.h> 
#define motor1_pin1 2
#define motor1_pin2 3
#define motor2_pin1 4
#define motor2_pin2 5
#define ECHOPIN 6                            // Pin to receive echo pulse
#define TRIGPIN 7                            // Pin to send trigger pulse
#define fariled 8
int distance = 0;
int distanceleft = 0;
int distanceright = 0;
int object= 35;


Servo myservo;  

void setup ()
{
  Serial.begin(9600);
  myservo.attach(9);
  pinMode(ECHOPIN, INPUT);
  pinMode(TRIGPIN, OUTPUT);
  pinMode(fariled, OUTPUT);
  pinMode(motor1_pin1,OUTPUT);
  pinMode(motor1_pin2,OUTPUT);
  pinMode(motor2_pin1,OUTPUT);
  pinMode(motor2_pin2,OUTPUT);

}

void loop()
{
  myservo.write(90);
  delay(200);
  digitalWrite(TRIGPIN, LOW);                   // Set the trigger pin to low for 2uS
  delayMicroseconds(2);
  digitalWrite(TRIGPIN, HIGH);                  // Send a 10uS high to trigger ranging
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);                   // Send pin low again
  int distance = pulseIn(ECHOPIN, HIGH);        // Read in times pulse
  distance= distance/58;    // Calculate distance from time of pulse
  if (distance <0 or distance >500) distance=0; 
  
  if (distance > object) {
    avanti();
  }
  if (distance <= object) {
    trovastrada();
  }
}

void avanti() {
  digitalWrite(fariled,HIGH);
  digitalWrite(motor1_pin1,HIGH);
  digitalWrite(motor1_pin2,LOW);
  digitalWrite(motor2_pin1,HIGH);
  digitalWrite(motor2_pin2,LOW); 
  digitalWrite(fariled,LOW);
  return;
}

void trovastrada() {
  fermati();
  retro();
  guardasinistra();
  guardadestra();
  if( distanceleft < distanceright)  {
    girasinistra();
  }
  else                               {
    giradestra();
  }
}
   
void retro() {
  digitalWrite(motor1_pin1,LOW);
  digitalWrite(motor1_pin2,HIGH);
  digitalWrite(motor2_pin1,LOW);
  digitalWrite(motor2_pin2,HIGH); 
  delay(500);
  fermati();
  return;
}

void fermati() {
  digitalWrite(motor1_pin1,LOW);
  digitalWrite(motor1_pin2,LOW);
  digitalWrite(motor2_pin1,LOW);
  digitalWrite(motor2_pin2,LOW);
  delay(500); 
  return;
}

void ultrasuoni() {
  digitalWrite(TRIGPIN, LOW);                   // Set the trigger pin to low for 2uS
  delayMicroseconds(2);
  digitalWrite(TRIGPIN, HIGH);                  // Send a 10uS high to trigger ranging
  delayMicroseconds(10);
  digitalWrite(TRIGPIN, LOW);                   // Send pin low again
  distance = pulseIn(ECHOPIN, HIGH);        // Read in times pulse
  distance= distance/58;    // Calculate distance from time of pulse
  if (distance <0 or distance >500) distance=0;
}

void guardasinistra(){
  myservo.write(30);
  delay(500);
  ultrasuoni();
  distanceleft = distance;
  Serial.print(distanceleft);
  Serial.print("  ");
  myservo.write(90);
  delay(500);
  return;  
}

void guardadestra(){
  myservo.write(150);
  delay(500);
  ultrasuoni();
  distanceright = distance;
  Serial.print(distanceright);
  Serial.print("  ");
  myservo.write(90);
  delay(500);
  return;  
  
}
void girasinistra() {
  digitalWrite(motor1_pin1,LOW);
  digitalWrite(motor1_pin2,HIGH);
  digitalWrite(motor2_pin1,HIGH);
  digitalWrite(motor2_pin2,LOW);
  delay(500);
  fermati();
  return;
}

void giradestra() {
  digitalWrite(motor1_pin1,HIGH);
  digitalWrite(motor1_pin2,LOW);
  digitalWrite(motor2_pin1,LOW);
  digitalWrite(motor2_pin2,HIGH);
  delay(500);
  fermati();
  return;
  
}

Se qualcuno ha altri suggerimenti, li accetterò voltentieri.

Simone