Arduino (IoT) Video Serie: Kit de Carro Parte 6 (Auto-Dirigido)

Finalmente veremos como hacer un carro realmente inteligente.  Claro que hay mucho mas por hacer con un proyecto como este, pero espero que con este material tengan suficiente para poder arrancar.  Vamos a ver como agregar un sensor que le diga al carro que tan lejos esta de un obstáculo, hacer algunos cálculos para determinar cual es la mejor ruta a seguir y continuar.  Hay miles de maneras de hacer esto pero solo vamos a explorar una en detalle aqui y les dejare otra opción para su investigación [por ejemplo, agregar navegación GPS!].

Veamos las conexiones:

Si han llegado hasta este punto, lo mas dificil ya está hecho.  Solo falta desconectar los 2 cables que usábamos en la modalidad BT e IR para Rx/Tx datos porque ya no hay señales que transmitir.  Y el ultimo es quitar el jumper amarillo de la L293D.

El Jumper

Este jumper cuando esta interrumpido significa que la L293D usará la potencia de la fuente externa (las 4xAA) para los motores y debemos suplir una fuente alterna para la Arduino UNO.  Anteriormente solo usábamos 2 motores por lo cual no era necesario mas potencia pero ahora que usaremos 4 motores si es necesario.  Es decir, los 4 motores se usaran de manera mas continua debido a que el Arduino se continuara moviendo en distintas direcciones mientras decide cual es el mejor camino para avanzar.

Una vez removido el jumper, podemos conectar ambas fuentes de poder y listo!

Analicemos el código final:

Option 4WD

#include <AFMotor.h>
#include <NewPing.h>
#include <Servo.h>
// Los 2 pines del ultrasonico
#define TRIG_PIN A4 
#define ECHO_PIN A5 
#define MAX_DISTANCE 100
#define MAX_SPEED 180 // sets speed of DC motors
#define MAX_SPEED_OFFSET 20
// Distancias y velocidades maximas
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
// Crear objetos para motores
AF_DCMotor motor1(1, MOTOR12_1KHZ); 
AF_DCMotor motor2(2, MOTOR12_1KHZ);
AF_DCMotor motor3(3, MOTOR12_1KHZ); 
AF_DCMotor motor4(4, MOTOR12_1KHZ);
//Iniciamos el objeto para servo
Servo myservo;
//Variable de estado de inicio falso
boolean goesForward=false;
int distance = 100;
int speedSet = 0;

void setup() {
//Iniciamos servo y tomamos 4 lecturas al frente
myservo.attach(9); 
 myservo.write(115); 
 delay(2000);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
 distance = readPing();
 delay(100);
}

void loop() {
 int distanceR = 0;
 int distanceL = 0;
 delay(40);
 //Si distancia al frente<30, alto, retroceder, alto, medir derecha e izquierda
 if(distance<=30){
 moveStop();
 delay(100);
 moveBackward();
 delay(300);
 moveStop();
 delay(200);
 distanceR = lookRight();
 delay(200);
 distanceL = lookLeft();
 delay(200);
//Si distanciaR es mayor, girar derecha...
if(distanceR>=distanceL){
 turnRight();
 moveStop();
 }else{
 turnLeft();
 moveStop();
 }
 }else{
 moveForward();//Sino continuar...
 }
 distance = readPing();
}
//Mirar derecha
int lookRight(){
 myservo.write(50); 
 delay(500);
 int distance = readPing();
 delay(100);
 myservo.write(115); 
 return distance;
}
//Mirar izquierda
int lookLeft(){
 myservo.write(170); 
 delay(500);
 int distance = readPing();
 delay(100);
 myservo.write(115); 
 return distance;
 delay(100);
}
//Medir distancia
int readPing() { 
 delay(70);
 int cm = sonar.ping_cm();
 if(cm==0)
 {
 cm = 250;
 }
 return cm;
}
//Alto
void moveStop() {
 motor1.run(RELEASE); 
 motor2.run(RELEASE);
 motor3.run(RELEASE); 
 motor4.run(RELEASE);
 } 
//Mover al frente
void moveForward() {
if(!goesForward){
 goesForward=true;
 motor1.run(FORWARD); 
 motor2.run(FORWARD); 
 motor3.run(FORWARD); 
 motor4.run(FORWARD); 
 for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // Acelerar lentamente
 {
 motor1.setSpeed(speedSet);
 motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
 motor3.setSpeed(speedSet);
 motor4.setSpeed(speedSet+MAX_SPEED_OFFSET);
 delay(5);
 }
 }
}
//Retroceder
void moveBackward() {
 goesForward=false;
 motor1.run(BACKWARD); 
 motor2.run(BACKWARD); 
 motor3.run(BACKWARD); 
 motor4.run(BACKWARD); 
 for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // Acelerar lentamente
 {
 motor1.setSpeed(speedSet);
 motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
 motor3.setSpeed(speedSet);
 motor4.setSpeed(speedSet+MAX_SPEED_OFFSET);
 delay(5);
 }
}
//Girar derecha
void turnRight() {
 motor1.run(BACKWARD);
 motor2.run(BACKWARD); 
 motor3.run(FORWARD);
 motor4.run(FORWARD); 
 delay(300);
 motor1.run(BACKWARD);
 motor2.run(BACKWARD); 
 motor3.run(FORWARD);
 motor4.run(FORWARD); 
 delay(300);
 motor1.run(BACKWARD);
 motor2.run(BACKWARD); 
 motor3.run(FORWARD);
 motor4.run(FORWARD); 
 delay(300);
 motor1.run(FORWARD); 
 motor2.run(FORWARD); 
 motor3.run(FORWARD); 
 motor4.run(FORWARD); 
} 
//Girar Izquierda
void turnLeft() {
 motor1.run(FORWARD); 
 motor2.run(FORWARD); 
 motor3.run(BACKWARD);
 motor4.run(BACKWARD); 
 delay(300);
 motor1.run(FORWARD); 
 motor2.run(FORWARD); 
 motor3.run(BACKWARD);
 motor4.run(BACKWARD); 
 delay(300);
 motor1.run(FORWARD); 
 motor2.run(FORWARD); 
 motor3.run(BACKWARD);
 motor4.run(BACKWARD); 
 delay(300);
 motor1.run(FORWARD); 
 motor2.run(FORWARD);
 motor3.run(FORWARD); 
 motor4.run(FORWARD);
}

Segunda opción para 4WD (re-definir motores)

#include //add Adafruit Motor Shield library
#include //add Servo Motor library            
#include //add Ultrasonic sensor library

#define TRIG_PIN A0 // Pin A0 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A1 // Pin A1 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 300 // sets maximum useable sensor measuring distance to 300cm
#define MAX_SPEED 160 // sets speed of DC traction motors to 150/250 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 40 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 30 // sets distance at which robot stops and reverses to 30cm
#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

AF_DCMotor leftMotor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor leftMotor2(2, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor2(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency
Servo myservo;  // create servo object to control a servo 

int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet = "";
int speedSet = 0;

//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() {
  myservo.attach(10);  // attaches the servo on pin 10 (SERVO_1 on the Motor Drive Shield to the servo object 
  myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
  delay(1000); // delay for one seconds
 }
//------------------------------------------------------------------------------------------------------------------------------------

//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
  myservo.write(90);  // move eyes forward
  delay(90);
  curDist = readPing();   // read distance
  if (curDist < COLL_DIST) {changePath();} // if forward is blocked change direction moveForward(); // move forward delay(500); } //------------------------------------------------------------------------------------------------------------------------------------- void changePath() { moveStop(); // stop forward movement myservo.write(36); // check distance to the right delay(500); rightDistance = readPing(); //set right distance delay(500); myservo.write(144); // check distace to the left delay(700); leftDistance = readPing(); //set left distance delay(500); myservo.write(90); //return to center delay(100); compareDistance(); } void compareDistance() // find the longest distance { if (leftDistance>rightDistance) //if left is less obstructed 
  {
    turnLeft();
  }
  else if (rightDistance>leftDistance) //if right is less obstructed
  {
    turnRight();
  }
   else //if they are equally obstructed
  {
    turnAround();
  }
}


//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() { // read the ultrasonic sensor distance
  delay(70);   
  unsigned int uS = sonar.ping();
  int cm = uS/US_ROUNDTRIP_CM;
  return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {leftMotor1.run(RELEASE); leftMotor2.run(RELEASE); rightMotor1.run(RELEASE); rightMotor2.run(RELEASE);}  // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
    motorSet = "FORWARD";
    leftMotor1.run(FORWARD);      // turn it on going forward
    leftMotor2.run(FORWARD);      // turn it on going forward
    rightMotor1.run(FORWARD);     // turn it on going forward
    rightMotor2.run(FORWARD);     // turn it on going forward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    leftMotor1.setSpeed(speedSet);
    leftMotor2.setSpeed(speedSet);
    rightMotor1.setSpeed(speedSet); 
    rightMotor2.setSpeed(speedSet);
    delay(5);
  }
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
    motorSet = "BACKWARD";
    leftMotor1.run(BACKWARD);     // turn it on going backward
    leftMotor2.run(BACKWARD);     // turn it on going backward
    rightMotor1.run(BACKWARD);    // turn it on going backward
    rightMotor2.run(BACKWARD);    // turn it on going backward
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
  {
    leftMotor1.setSpeed(speedSet);
    leftMotor2.setSpeed(speedSet);
    rightMotor1.setSpeed(speedSet); 
    rightMotor2.setSpeed(speedSet); 
    delay(5);
  }
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
  motorSet = "RIGHT";
  leftMotor1.run(FORWARD);      // turn motor 1 forward
  leftMotor2.run(FORWARD);      // turn motor 2 forward
  rightMotor1.run(BACKWARD);    // turn motor 3 backward
  rightMotor2.run(BACKWARD);    // turn motor 4 backward
  rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);      
  rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);     
  delay(1500); // run motors this way for 1500        
  motorSet = "FORWARD";
  leftMotor1.run(FORWARD);      // set both motors back to forward
  leftMotor2.run(FORWARD);
  rightMotor1.run(FORWARD);
  rightMotor2.run(FORWARD);      
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
  motorSet = "LEFT";
  leftMotor1.run(BACKWARD);      // turn motor 1 backward
  leftMotor2.run(BACKWARD);      // turn motor 2 backward
  leftMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);     
  leftMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);    
  rightMotor1.run(FORWARD);     // turn motor 3 forward
  rightMotor2.run(FORWARD);     // turn motor 4 forward
  delay(1500); // run motors this way for 1500  
  motorSet = "FORWARD";
  leftMotor1.run(FORWARD);      // turn it on going forward
  leftMotor2.run(FORWARD);      // turn it on going forward
  rightMotor1.run(FORWARD);     // turn it on going forward
  rightMotor2.run(FORWARD);     // turn it on going forward
}  
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
  motorSet = "RIGHT";
  leftMotor1.run(FORWARD);      // turn motor 1 forward
  leftMotor2.run(FORWARD);      // turn motor 2 forward
  rightMotor1.run(BACKWARD);    // turn motor 3 backward
  rightMotor2.run(BACKWARD);    // turn motor 4 backward
  rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);      
  rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
  delay(1700); // run motors this way for 1700
  motorSet = "FORWARD";
  leftMotor1.run(FORWARD);      // set both motors back to forward
  leftMotor2.run(FORWARD);
  rightMotor1.run(FORWARD);
  rightMotor2.run(FORWARD);      
}