Proyectos con Arduino.

Blog de proyectos de Arduino de alumnos de la URJC

Carrito GPS

por · 18/12/2021

Proyecto realizado por el grupo 11 : Pedro Barquero Torres para la asignatura de Diseño de Sistemas Empotrados

Objetivo :

El objetivo del proyecto es diseñar un carro que por bluetooth se conecta al móvil y a través de un módulo GPS y una brújula magnética sigue los pasos de la persona que lo está dirigiendo por el móvil.

Materiales y presupuesto :

Esquema de conexiones :

Casos de uso :

El carrito puede ser controlado mediante el uso del bluetooth (de manera manual) o a través del uso del GPS (de manera automática siguiendo tus pasos ).

Para ello en esta práctica se utiliza la aplicación : Blynk.

Con la siguiente interfaz :

Problemas y posibles soluciones :

El mayor problema encontrado es que los motores de 12V no alcanzan la fuerza suficiente como para mover la estructura del carro, por lo que se necesitarían motores con mayor número de RPM.

Otro problema es que el GPS del carro tarda mucho tiempo en hacer conexión con un satélite por lo que se hace muy tedioso probar el funcionamiento del código. Posible solución, utilizar un GPS con mayor precisión.

Código :

//librerias
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#include <BlynkSimpleSerialBLE.h>
#include "./TinyGPS.h"                


//Definicion
char auth[] = "B1rGOPW6QUwpx5Np1b-RF0oCb9_rwBuK";
#define BLUETOOTH_TX_PIN 10
#define BLUETOOTH_RX_PIN 11
#define GPS_TX_PIN 6
#define MOTOR_A_EN_PIN 5
#define MOTOR_B_EN_PIN 9
#define MOTOR_A_IN_1_PIN 7
#define MOTOR_A_IN_2_PIN 8
#define MOTOR_B_IN_1_PIN 12
#define MOTOR_B_IN_2_PIN 4
#define MOTOR_B_OFFSET 20
#define MOTOR_A_OFFSET 0


//bluetooth
SoftwareSerial bluetoothSerial(BLUETOOTH_TX_PIN, BLUETOOTH_RX_PIN);
SoftwareSerial nss(GPS_TX_PIN, 255);            


//Brujula
#define DECLINATION_ANGLE -0.003f //eror del campo magnetico 
#define COMPASS_OFFSET 0.0f
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

//GPS
#define GPS_UPDATE_INTERVAL 1000
#define GPS_STREAM_TIMEOUT 18
#define GPS_WAYPOINT_TIMEOUT 45

////Variables de control 
bool arriba = false ;
bool abajo = false ;
bool izquierda = false ; 
bool derecha = false; 
bool stp = false ; //stop
WidgetTerminal terminal(V4);

bool seguir = false;


//Mas GPS
struct GeoLoc {
  float lat;
  float lon;
};
TinyGPS gps;

//************************************************************************
// CONTROL POR BLUETOOTH
//************************************************************************
BLYNK_WRITE(V1) {

 Serial.println("Modo bluetooth adelante ") ;
 terminal.println("Modo bluetooth adelante ") ;
 terminal.flush(); 
 bool arriba = true;
bool abajo = false ;
bool izquierda = false ; 
bool derecha = false; 
bool stp = false;
run(arriba,abajo,izquierda,derecha,stp); 
}
BLYNK_WRITE(V2) {
  Serial.println("Modo bluetooth derecha ") ;
   terminal.println("Modo bluetooth derecha ") ;
   terminal.flush(); 
  
  bool arriba = false ;
bool abajo = false ;
bool izquierda = false ; 
bool derecha = true; 
bool stp = false;
run(arriba,abajo,izquierda,derecha,stp); 
}
BLYNK_WRITE(V3) {
  Serial.println("Modo bluetooth izquierda ") ;
    terminal.println("Modo bluetooth izquierda ") ;
     terminal.flush(); 
     bool arriba = false ;
bool abajo = false ;
bool izquierda = true; 
bool derecha = false; 
bool stp = false;
run(arriba,abajo,izquierda,derecha,stp);  
  
}BLYNK_WRITE(V5) {
  Serial.println("Modo bluetooth atras ") ;
   terminal.println("Modo bluetooth atras ") ;
    terminal.flush(); 
   bool arriba = false ;
bool abajo = true ;
bool izquierda = false; 
bool derecha = false; 
bool stp = false;
run(arriba,abajo,izquierda,derecha,stp);  
}
BLYNK_WRITE(V6) {

  Serial.println("Modo bluetooth parar ") ;
   terminal.println("Modo bluetooth parar ") ;
    terminal.flush(); 
   bool arriba = false ;
bool abajo = false;
bool izquierda = false; 
bool derecha = false; 
bool stp = true;
run(arriba,abajo,izquierda,derecha,stp); 
}
void run(bool arriba, bool abajo, bool izquierda,bool derecha, bool stp){ 
  analogWrite(MOTOR_A_EN_PIN,255);
    analogWrite(MOTOR_B_EN_PIN,255);
   
    Serial.println(arriba);
    Serial.println(abajo);
    Serial.println(izquierda);
    Serial.println(derecha);
     Serial.println(stp);
     
  if(arriba){
  

 digitalWrite(MOTOR_A_IN_1_PIN, LOW);
  digitalWrite(MOTOR_A_IN_2_PIN, HIGH);  
  digitalWrite(MOTOR_B_IN_1_PIN, HIGH);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
  }
  if(abajo){


     digitalWrite(MOTOR_A_IN_1_PIN, HIGH);
  digitalWrite(MOTOR_A_IN_2_PIN, LOW);  
  digitalWrite(MOTOR_B_IN_1_PIN, LOW);
  digitalWrite(MOTOR_B_IN_2_PIN,  HIGH);
    }
     if(derecha){
  digitalWrite(MOTOR_A_IN_1_PIN, LOW);
  digitalWrite(MOTOR_A_IN_2_PIN, LOW);  
  digitalWrite(MOTOR_B_IN_1_PIN, HIGH);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
    }
     if(izquierda){
  digitalWrite(MOTOR_A_IN_1_PIN, LOW);
  digitalWrite(MOTOR_A_IN_2_PIN, HIGH);  
  digitalWrite(MOTOR_B_IN_1_PIN, LOW);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
    }
     if(stp){
  digitalWrite(MOTOR_A_IN_1_PIN, LOW);
  digitalWrite(MOTOR_A_IN_2_PIN, LOW);  
  digitalWrite(MOTOR_B_IN_1_PIN, LOW);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
    }
  delay(1000);
  }
 //************************************************************************
 //************************************************************************
 //***********************************TERMINAL ANDROID*********************
BLYNK_WRITE(V4) {
  terminal.println("Hola mundo");
  terminal.flush();
  
  }
//************************************************************************
//************************************************************************
//********CONTROL GPS*****************************************************
 GeoLoc checkGPS() {
  Serial.println("Leyendo GPS carro: ");
  terminal.println("Leyendo GPS carro : \n");
  terminal.flush();
  bool newdata = false;
  unsigned long start = millis();
  while (millis() - start < GPS_UPDATE_INTERVAL) {
    if (feedgps())
      newdata = true;
  }
  if (newdata) {
    return gpsdump(gps);
  }

  GeoLoc carroLoc;
  carroLoc.lat = 0.0;
 carroLoc.lon = 0.0;
  
  return carroLoc;
}

// En caso de tener datos nuevos los procesaremos :
GeoLoc gpsdump(TinyGPS &gps) {
  float flat, flon;
  unsigned long age;
  
  gps.f_get_position(&flat, &flon, &age);

  GeoLoc carroLoc;
  carroLoc.lat = flat;
  carroLoc.lon = flon;

  Serial.print(carroLoc.lat, 7); Serial.print(", "); Serial.println(carroLoc.lon, 7);
  terminal.print(carroLoc.lat, 7); terminal.print(", "); terminal.println(carroLoc.lon, 7);
  terminal.print("Conseguidas las coordenadas del carro ") ;
    terminal.flush();
  return carroLoc;
}

// proporciona datos a medida que estan listos 
bool feedgps() {
  while (nss.available()) {
    if (gps.encode(nss.read()))
      return true;
  }
  return false;
}

//Control para que nos siga el carro 
BLYNK_WRITE(V7) {
 
  seguir = !seguir;

  stop();
   
}
//Control para proporcionarle al carro las coordenadas gps del movil 
BLYNK_WRITE(V8) {
  GpsParam gps(param);
  
  Serial.println("Recibidas coordenadas GPS android : ");
  terminal.println("Recibidas coordenadas GPS android : ");
   
  
  Serial.print(gps.getLat(), 7); Serial.print(", "); Serial.println(gps.getLon(), 7);
   terminal.print(gps.getLat(), 7); terminal.print(", "); terminal.println(gps.getLon(), 7);
     terminal.flush();

  GeoLoc phoneLoc;
  phoneLoc.lat = gps.getLat();
  phoneLoc.lon = gps.getLon();

  driveTo(phoneLoc, GPS_STREAM_TIMEOUT);
}

//************************************************************************
//************************************************************************
//********CONTROL BRUJULA*****************************************************
void displayCompassDetails(void)
{
  sensor_t sensor;
  mag.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");  
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

#ifndef DEGTORAD
#define DEGTORAD 0.0174532925199432957f
#define RADTODEG 57.295779513082320876f
#endif

float geoBearing(struct GeoLoc &a, struct GeoLoc &b) {
  float y = sin(b.lon-a.lon) * cos(b.lat);
  float x = cos(a.lat)*sin(b.lat) - sin(a.lat)*cos(b.lat)*cos(b.lon-a.lon);
  return atan2(y, x) * RADTODEG;
}

float geoDistance(struct GeoLoc &a, struct GeoLoc &b) {
  const float R = 6371000; // km
  float p1 = a.lat * DEGTORAD;
  float p2 = b.lat * DEGTORAD;
  float dp = (b.lat-a.lat) * DEGTORAD;
  float dl = (b.lon-a.lon) * DEGTORAD;

  float x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2);
  float y = 2 * atan2(sqrt(x), sqrt(1-x));

  return R * y;
}

float geoHeading() {
  sensors_event_t event; 
  mag.getEvent(&event);
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  heading -= DECLINATION_ANGLE;
  heading -= COMPASS_OFFSET;
  if(heading < 0)
    heading += 2*PI;
  if(heading > 2*PI)
    heading -= 2*PI;
  float headingDegrees = heading * 180/M_PI; 
  while (headingDegrees < -180) headingDegrees += 360;
  while (headingDegrees >  180) headingDegrees -= 360;
  return headingDegrees;
}

void testDriveNorth() {
  float heading = geoHeading();
  int testDist = 10;
  Serial.println(heading);
  
  while(!(heading < 5 && heading > -5)) {
    drive(testDist, heading);
    heading = geoHeading();
    Serial.println(heading);
    delay(500);
  }
  
  stop();
}

void setupCompass() {
   
  if(!mag.begin())
  {
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    terminal.println("Ooops, no HMC5883 detected ... Check your wiring!");
      terminal.flush();
    while(1);
  }
  
  displayCompassDetails();
}



//************************************************************************
//******************************fUNCIONES DE MOVIMIENTO********************************
void stop() {
  // now turn off motors
  digitalWrite(MOTOR_A_IN_1_PIN, LOW);
  digitalWrite(MOTOR_A_IN_2_PIN, LOW);  
  digitalWrite(MOTOR_B_IN_1_PIN, LOW);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
  
}
void drive(int distance, float turn) {
 

  int fullSpeed = 230;
  int stopSpeed = 0;

  // drive to location
  int s = fullSpeed;
  if ( distance < 8 ) {
    int wouldBeSpeed = s - stopSpeed;
    wouldBeSpeed *= distance / 8.0f;
    s = stopSpeed + wouldBeSpeed;
  }
  
  int autoThrottle = constrain(s, stopSpeed, fullSpeed);
  autoThrottle = 230;

  float t = turn;
  while (t < -180) t += 360;
  while (t >  180) t -= 360;
  
  Serial.print("turn: ");
  Serial.println(t);
  Serial.print("original: ");
  Serial.println(turn);
  
  float t_modifier = (180.0 - abs(t)) / 180.0;
  float autoSteerA = 1;
  float autoSteerB = 1;

  if (t < 0) {
    autoSteerB = t_modifier;
  } else if (t > 0){
    autoSteerA = t_modifier;
  }

  Serial.print("steerA: "); Serial.println(autoSteerA);
  Serial.print("steerB: "); Serial.println(autoSteerB);

  int speedA = (int) (((float) autoThrottle) * autoSteerA);
  int speedB = (int) (((float) autoThrottle) * autoSteerB);
  
  setSpeedMotorA(speedA);
  setSpeedMotorB(speedB);
}
void setSpeedMotorA(int speed) {
  digitalWrite(MOTOR_A_IN_1_PIN, HIGH);
  digitalWrite(MOTOR_A_IN_2_PIN, LOW);
  analogWrite(MOTOR_A_EN_PIN, speed + MOTOR_A_OFFSET);
}

void setSpeedMotorB(int speed) {
  digitalWrite(MOTOR_B_IN_1_PIN, HIGH);
  digitalWrite(MOTOR_B_IN_2_PIN, LOW);
  analogWrite(MOTOR_B_EN_PIN, speed + MOTOR_B_OFFSET);
}

void setSpeed(int speed)
{
  setSpeedMotorA(speed);
  setSpeedMotorB(speed);
}


void driveTo(struct GeoLoc &loc, int timeout) {
  nss.listen();
  GeoLoc carroLoc = checkGPS();
  bluetoothSerial.listen();

  if (carroLoc.lat != 0 && carroLoc.lon != 0 && seguir) {
    float d = 0;
    //Start move loop here
    do {
      nss.listen();
      carroLoc = checkGPS();
      bluetoothSerial.listen();
      
      d = geoDistance(carroLoc, loc);
      float t = geoBearing(carroLoc, loc) - geoHeading();
      
      Serial.print("Distancia : ");
      Serial.println(geoDistance(carroLoc, loc));
    
      Serial.print("rumbo: ");
      Serial.println(geoBearing(carroLoc, loc));

      Serial.print("grados: ");
      Serial.println(geoHeading());
      
      drive(d, t);
      timeout -= 1;
    } while (d > 3.0 && seguir && timeout>0);

    stop();
  }
}
//************************************************************************
//************************************************************************
void setup()
{
   //Terminal movil
   terminal.clear();
   
  // Brujula
  setupCompass();
  
  //Motor
   pinMode(MOTOR_A_IN_1_PIN, OUTPUT);
  pinMode(MOTOR_A_IN_2_PIN, OUTPUT);
  pinMode(MOTOR_B_IN_1_PIN, OUTPUT);
  pinMode(MOTOR_B_IN_2_PIN, OUTPUT);
   pinMode(MOTOR_A_EN_PIN, OUTPUT);
  pinMode(MOTOR_B_EN_PIN, OUTPUT);

   //GPS
  nss.begin(9600);
  
 Serial.begin(9600);       

      //Bluetooth
  bluetoothSerial.begin(9600);
  Blynk.begin(bluetoothSerial, auth);
  
 

}


void loop()
{
    Blynk.run();
}

Memoria :

MemoriaDSEDescarga

Video del proyecto :

También te podría gustar...

Deja una respuesta

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *