CARRINHO DIRECIONADO POR SENSOR DE INCLINAÇÃO

Este sketch faz com que o carrinho mantenha um curso inicial andando em linha reta, corrigindo seu curso usando como referencia o sensor mpu 6050. Quando for detectado um objeto a uma distância menor que 50cm, os motores serão desligados para evitar a colisão.

#include <MPU6050_tockn.h>
#include <Wire.h>
#include <NewPing.h>
#define TRIGGER_PIN  13  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     12  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 60 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.


MPU6050 mpu6050(Wire);

long timer = 0;
int direcao = 0;
int corretor = 0;
int velDir = 0;
int velEsq = 0;
int varDir = 0;
int varEsq = 0;
int pot = 80;
int angulo = 7; // angulo de correção máximo em graus;
int distMin = 50; // distância minima até um objeto frontal;

void setup() {

  pinMode(3, OUTPUT); // liga motor da esquerda do carrinho para tras;
  pinMode(6, OUTPUT); // liga motor da direita do carrinho para frente;
  pinMode(9, OUTPUT); // liga motor da esquerda do carrinho para frente;
  pinMode(10, OUTPUT); // liga motor da direita do carrinho para tras;

  Serial.begin(9600); // inicia uma troca de dados na velocidade de 9600 bits/s entre a placa e o pc;
  Wire.begin();      // relacionado com a preparação das portas sda(A4) e scl(A5);
  mpu6050.begin();   // inicia o sensor de mpu 6050;
  mpu6050.calcGyroOffsets(true);
}

void loop() {

  // para a esquerda o valor do sensor de movimento  aumenta;
  // para a direita o valor do sensor de movimento  diminui;

  mpu6050.update();

  if (millis() - timer > 100) {
    Serial.print("Ping: ");
    Serial.print(sonar.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
    Serial.println("cm");

    //Serial.print("\tangleZ : ");Serial.println(mpu6050.getAngleZ());


    diretor(); // função responsavel por controlar os motores mantendo o curso em linha reta;

 if( sonar.ping_cm() <= distMin && sonar.ping_cm() != 0){ // desliga os motores para evitar colisão;
  analogWrite(3,0);
  analogWrite(6,0);
  analogWrite(9,0);
  analogWrite(10,0);
  delay(500);
 }
  }
}
void diretor() {
  mpu6050.update();
  direcao = (mpu6050.getAngleZ()) * 100;
  Serial.print("direcao : "); Serial.println(direcao);
  Serial.println(" ");
  timer = millis();


  if (direcao < corretor && direcao > (angulo * -100)) {
    varEsq = direcao * -1;
  }
  if (direcao >= corretor) {
    varEsq = 0;
  }
  velEsq = map(varEsq, 0, (angulo * 100), pot, 0);
  analogWrite(9, velEsq);

  if (direcao > corretor && direcao < (angulo * 100)) {
    varDir = direcao;
  }
  if (direcao <= corretor) {
    varDir = 0;
  }
  velDir = map(varDir, 0, (angulo * 100), pot, 0);
  analogWrite(6, velDir);
}

*************programa aprimorado 1:****************

#include <MPU6050_tockn.h>
#include <Wire.h>
#include <NewPing.h>
#define TRIGGER_PIN  13  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     12  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 60 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.


MPU6050 mpu6050(Wire);

long timer = 0;
int direcao = 0;
int corretor = 0;
int velDir = 0;
int velEsq = 0;

int pot = 100;
int angulo = 0; // angulo de correção máximo em graus;
int distMin = 20; // distância minima até um objeto frontal;

void setup() {

  pinMode(3, OUTPUT); // liga motor da esquerda do carrinho para tras;
  pinMode(6, OUTPUT); // liga motor da direita do carrinho para frente;
  pinMode(9, OUTPUT); // liga motor da esquerda do carrinho para frente;
  pinMode(10, OUTPUT); // liga motor da direita do carrinho para tras;

  Serial.begin(9600); // inicia uma troca de dados na velocidade de 9600 bits/s entre a placa e o pc;
  Wire.begin();      // relacionado com a preparação das portas sda(A4) e scl(A5);
  mpu6050.begin();   // inicia o sensor de mpu 6050;
  mpu6050.calcGyroOffsets(true);
}

void loop() {

  // para a esquerda o valor do sensor de movimento  aumenta;
  // para a direita o valor do sensor de movimento  diminui;

  if (millis() - timer > 100) {
    Serial.print("Ping: ");
    Serial.print(sonar.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
    Serial.print("cm  ----- ");
    Serial.print(" angulo de correção: ");
    Serial.print(angulo);
    Serial.print(" ----- direcao : ");
    Serial.print(direcao);
    Serial.print(" ----- angulo atual: ");
    Serial.println(mpu6050.getAngleZ());

    diretor(); // função responsavel por controlar os motores mantendo o curso em linha reta;

    conversor(); // função responsavel por fazer o robô parar diante de um obstáculo e fazer uma conversão para direita ou esquerda;

  }
}
void diretor() {
  mpu6050.update();
  direcao = ((mpu6050.getAngleZ()) * 10) + angulo;
  timer = millis();

  if ( direcao >= angulo) {
    velDir = abs(direcao);
    velEsq = 0;
    if (velDir > 100) {
      velDir = 100;}}
   
  if ( direcao <= angulo) {
    velEsq = abs(direcao);
    velDir = 0;
    if (velEsq > 100) {
      velEsq = 100;}}
   
  velDir = map(velDir, 0, 100, pot, 0);
  analogWrite(6, velDir);
  velEsq = map(velEsq, 0, 100, pot, 0);
  analogWrite(9, velEsq);}

void conversor() {
  if ( sonar.ping_cm() <= distMin && sonar.ping_cm() != 0) { // desliga os motores para evitar colisão;
    analogWrite(3, 0);
    analogWrite(6, 0);
    analogWrite(9, 0);
    analogWrite(10, 0);
    delay(500);
    angulo += 900;
    if (angulo >= 3600) {
      angulo = 0;}
   
    delay(1000);
    analogWrite(3,pot);
    analogWrite(10,pot);
    delay(1000);
    analogWrite(3,0);
    analogWrite(10,0);
  }
}

*************programa aprimorado 2:****************

#include <MPU6050_tockn.h>
#include <Wire.h>
#include <NewPing.h>
#define TRIGGER_PIN  13  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     12  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 60 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.


MPU6050 mpu6050(Wire);

long timer = 0;
int direcao = 0;
int corretor = 0;
int velDir = 0;
int velEsq = 0;
int angDist = 900;

int pot = 100;
int angulo = 0; // angulo de correção máximo em graus;
int distMin = 20; // distância minima até um objeto frontal;

void setup() {

  pinMode(3, OUTPUT); // liga motor da esquerda do carrinho para tras;
  pinMode(6, OUTPUT); // liga motor da direita do carrinho para frente;
  pinMode(9, OUTPUT); // liga motor da esquerda do carrinho para frente;
  pinMode(10, OUTPUT); // liga motor da direita do carrinho para tras;

  Serial.begin(9600); // inicia uma troca de dados na velocidade de 9600 bits/s entre a placa e o pc;
  Wire.begin();      // relacionado com a preparação das portas sda(A4) e scl(A5);
  mpu6050.begin();   // inicia o sensor de mpu 6050;
  mpu6050.calcGyroOffsets(true);
}

void loop() {

  // para a esquerda o valor do sensor de movimento  aumenta;
  // para a direita o valor do sensor de movimento  diminui;

  if (millis() - timer > 100) {
    Serial.print("Ping: ");
    Serial.print(sonar.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
    Serial.print("cm  ----- ");
    Serial.print(" angulo de correção: ");
    Serial.print(angulo);
    Serial.print(" ----- direcao : ");
    Serial.print(direcao);
    Serial.print(" ----- angulo atual: ");
    Serial.println(mpu6050.getAngleZ());

    diretor(); // função responsavel por controlar os motores mantendo o curso em linha reta;

    conversor(); // função responsavel por fazer o robô parar diante de um obstáculo e fazer uma conversão para direita ou esquerda;

  }
}
void diretor() {  // esta função faz com que o robô corrija sua direção andando sempre em linha reta;
  mpu6050.update();
  direcao = ((mpu6050.getAngleZ()) * 10) + angulo;
  timer = millis();

  if ( direcao >= angulo) { // atualiza velocidade do motor direito;
    velDir = abs(direcao);
    velEsq = 0;
    if (velDir > 100) {
      velDir = 100;
    }
  }

  if ( direcao <= angulo) { // atualiza velocidade do motor esquerdo;
    velEsq = abs(direcao);
    velDir = 0;
    if (velEsq > 100) {
      velEsq = 100;
    }
  }
 // realiza o ajusta da direção do robô controlando as velocidades dos motores;
  velDir = map(velDir, 0, 100, pot, 0);
  analogWrite(6, velDir);
  velEsq = map(velEsq, 0, 100, pot, 0);
  analogWrite(9, velEsq);
}

void conversor() {
  if ( sonar.ping_cm() <= distMin && sonar.ping_cm() != 0) { // desliga os motores para evitar colisão;
    analogWrite(3, 0);
    analogWrite(6, 0);
    analogWrite(9, 0);
    analogWrite(10, 0);
    delay(500);
    angulo += angDist; // soma um valor na nova direção do robô;
    if (angulo >= 3600) {
      angulo = 0;
    }

    delay(1000); // faz com que ao encontrar um obstaculo o robo ande em marcha ré por 1 segundo.
    analogWrite(3, pot);
    analogWrite(10, pot);
    delay(1000);
    analogWrite(3, 0);
    analogWrite(10, 0);
  }
}