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 20cm, os motores serão desligados para evitar a colisão.
   Nas aulas posteriores aprimoramos o programa para fazer com que o robô também desvie dos obstáculos e se desloque  em uma nova direção.

sketch para carregar no robô:

#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);
  }
}


sketch para carregar no robô 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 comando = 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(115200); // 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 (Serial.available()) // verifica se existe algo digitado no monitor serial;
  {
    comando = Serial.read(); // carrega o valor digitado no monitor serial na variavel "comando";
  }
    if (comando == '0')
    {
      analogWrite(3, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      analogWrite(10, 0);
      Serial.println("robo parado !!!!!");

    }
    while (comando == '1')
    {
      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);
  }

}