25 réponses à ce sujet

[Mike118]

Ta fonction "smartDelay" n'est pas assez " smart ... 

Elle est bloquante car tu utilises un while ... et pendant ce temps " bloquant" tu lis bien ton GPS mais pas ton lidar ... Du coup ton lidar te boude
 

[Atmos]

Merci Mike,

Il me semblait bien que c'était ici le problème, mais je ne savais pas comment faire. (sans le smartDelay, le GPS ne fonctionne pas)

 

J'ai modifié mon code et cela fonctionne pas trop mal. Certainement à amélioré.... mais c'est bon

// ----------------------------------------------- Include
#include <Moteurs.h>
#include  <Gestion.h>

// ----------------------------------------------- Define
// #define DEBUG
#define AFFICHE
#define GPS
#define COMPAS
#define SDCARD

// ----------------------------------------------- Lidar
/*
  vert TX7, blanc RX7, bleu (3)
*/

#include <RPLidar.h>
  #define RPLIDAR_MOTOR 3
  RPLidar lidar;

  const int LIDAR_speed = 255;
  float minDistance     = 10000;
  float angleAtMinDist  = 0;
  float distOld         = 0;

  int val = 0;
 
  float Distance[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  float Angle[]    = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  bool  Ok[]       = {0, 0, 0, 0, 0, 0};
  String A[]       = {"Distance FA : ", "Distance FD : ", "Distance DR : ", "Distance AR : ", "Distance GA : ", "Distance FG : "};

  #define FA 0
  #define FD 1
  #define DR 2
  #define AR 3
  #define GA 4
  #define FG 5

  #define FACE 360
  #define FACD  45
  #define DROI  90
  #define ARRI 180
  #define GAUC 270
  #define FACG 315
  #define CONS 4      // Constante

// ----------------------------------------------- Gps
#ifdef GPS
#include <TinyGPS++.h>
  TinyGPSPlus gps;
  static const uint32_t GPSBaud = 9600;
#endif

// ----------------------------------------------- Compas
#ifdef COMPAS
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
  Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
#endif

// ----------------------------------------------- SDCard
#ifdef SDCARD
#include <SD.h>
#include <SPI.h>
  File myFile;
  const int        chipSelect = BUILTIN_SDCARD;
  unsigned long currentTime  = 0;
  unsigned long previousTime = 0;
#endif

// ----------------------------------------------- Variables
struct SValeursRover
  {
    float distFA;
    float distFD;
    float distDR;
    float distAR;
    float distGA;
    float distFG;
    char  Date[12];
    char  Time[12];
    float distToWP;
    float distByOP;
    bool  Pivote;
    float Compas;
    float Degres;
    float Azimut;
    float Latitude;
    float Longitude;
    int   Satellites;
    int   Difference;
  };
  struct SValeursRover VR;

// ***********************************************************************
//                    SETUP
// ***********************************************************************
void setup()
{
#ifdef AFFICHE
  Serial.begin(115200);
#endif

//************************************************************************ RPLidar
//  Serial7.begin(115200);
  lidar.begin(Serial7);
  pinMode(RPLIDAR_MOTOR, OUTPUT);

//************************************************************************ Gps
#ifdef GPS
  Serial3.begin(GPSBaud);
#endif

//************************************************************************ Compas
#ifdef COMPAS
  if(!mag.begin())
  {
#ifdef DEBUG
    Serial.println("Initialization COMPAS Failed");
#endif
    while(1);
  }
#endif

//************************************************************************ SDCard
#ifdef SDCARD
  if (!SD.begin(chipSelect))
  {
#ifdef DEBUG
    Serial.println("Initialization SD Failed");
#endif
    return;
  }
#ifdef DEBUG
  Serial.println("Initialization SD Ok");
#endif
#endif
}

// ***********************************************************************
//                    LOOP
// ***********************************************************************
void loop()
{
  AssigneDistances();

  Autonome();

#ifdef AFFICHE
  Affiche();
#endif

#ifdef SDCARD
  currentTime = millis();
  if ((currentTime - previousTime) > 3000)
  {
    previousTime = currentTime;
     if (VR.distByOP > 0)           // Distance parcourue
    {
         AssigneGpsCom();
         MakeBuffer();
    }
  }
#endif
}

// ***********************************************************************
//                    AUTONOME
// ***********************************************************************
// *********************************************************************** Autonome
void Autonome()
{
  if (VR.distFA < 300) Serial.println("Stop Avance");
  else  Serial.println("     - Avance");

  if (VR.distAR < 300) Serial.println("Stop Arriere");
  else  Serial.println("     - Avance");
}

// ***********************************************************************
//                    FONCTIONS LIDAR
// ***********************************************************************
// *********************************************************************** Lidar
void AssigneDistances()
{
  while (val != 5)
  {
    if (IS_OK(lidar.waitPoint()))
    {
      float distance = lidar.getCurrentPoint().distance;
      float angle    = lidar.getCurrentPoint().angle;
      bool  startBit = lidar.getCurrentPoint().startBit;
      byte  quality  = lidar.getCurrentPoint().quality;

    // if (distance > 0 && distance < minDistance)
    // if (distance > 0 && quality > 15)

      if (distance > 10)
      {
         val = GetLidar(distance, angle, quality);
      }
    }
    else
    {
      analogWrite(RPLIDAR_MOTOR, 0);

      rplidar_response_device_info_t info;

      if (IS_OK(lidar.getDeviceInfo(info, 100)))
      {
        lidar.startScan();
        analogWrite(RPLIDAR_MOTOR, LIDAR_speed);
        delay(1000);
      }
    }
  }
  if (val == 5)
  {
    val = 0;
    return;
  }
}

// *********************************************************************** Lidar
int GetLidar(float dis, float ang, byte qua)
{
    if (ang <= FACE && ang > (FACE - CONS))
    {
      if (!Ok[FA])
      {
        Distance[FA]  = dis;
        VR.distFA     = dis;
        Angle[FA]     = ang;
        Ok[FA]        = 1;
      }
    }
    if (ang <= FACG && ang > (FACG - CONS))
    {
      if (!Ok[FG])
      {
        Distance[FG]  = dis;
        VR.distFG     = dis;
        Angle[FG]     = ang;
        Ok[FG]        = 1;
      }
    }
    if (ang <= GAUC && ang > (GAUC - CONS))
    {
      if (!Ok[GA])
      {
        Distance[GA]  = dis;
        VR.distGA     = dis;
        Angle[GA]     = ang;
        Ok[GA]        = 1;
      }
    }
    if (ang <= ARRI && ang > (ARRI - CONS))
    {
      if (!Ok[AR])
      {
        Distance[AR] = dis;
        VR.distAR    = dis;
        Angle[AR]    = ang;
        Ok[AR]        = 1;
      }
    }
   if (ang <= DROI && ang > (DROI - CONS))
     {
      if (!Ok[DR])
      {
        Distance[DR] = dis;
        VR.distDR    = dis;
        Angle[DR]    = ang;
        Ok[DR]        = 1;
      }
    }
   if (ang <= FACD && ang > (FACD - CONS))
     {
      if (!Ok[FD])
      {
        Distance[FD] = dis;
        VR.distFD    = dis;
        Angle[FD]    = ang;
        Ok[FD]        = 1;
      }
    }


  if (Ok[FA] && Ok[FG] && Ok[GA] && Ok[AR] && Ok[DR] && Ok[FD])
  {
    Ok[FA] = 0;
    Ok[FG] = 0;
    Ok[GA] = 0;
    Ok[AR] = 0;
    Ok[DR] = 0;
    Ok[FD] = 0;
    return 5;
  }
  else return 0;
}

[Mike118]

Il te manque la lecture du GPS dans ce nouveau code.

[Atmos]

Je n'ai rien changé concernant le code pour le Gps et le Compas.

 

Pense tu que on pourrait améliorer la fonction smartDelay ? (C'est juste la fonction de l'exemple de la library TinyGps++)

[Mike118]

Je n'ai rien changé concernant le code pour le Gps et le Compas.

 

En fait je viens de comprendre que tu as pas remis tout le code, juste le début du code en fait ... D'où ma remarque comme quoi il manque du code dans ton dernier message ... Mais en effet si tu as rien changé je peux revoir les fonctions dans ton message d'avant ... 

 

 

 

Pense tu que on pourrait améliorer la fonction smartDelay ? (C'est juste la fonction de l'exemple de la library TinyGps++)

Oui on peut améliorer la fonction smartDelay, pour cela il faut lire le lidar dedans ... Tu peux éventuellement appeler à l'intérieur ta fonction AssigneDistances() 

[Atmos]

Merci, je vais essayer.