Lidar values missing when printing to LCD

Status
Not open for further replies.

dhg

Member
Hello,

I am using Slamtec RPLidar A1 with Teensy 4.0 and LCD 20x04 Display. Teensy communicates with the Lidar via UART and with LCD via I2C. I am using this set-up to detect how far the trees are located from the sensor. The Lidar is attached to the vehicle, and as the vehicle moves along trees, I am trying to simply print the average distance from sensor to trees of both sides(left and right) of the scan. The lidar is positioned vertically so it can scan the trees in both left and right side. The readings from the sensor is correct, but the problem is that I am missing Lidar data whenever the display updates. I tried printing the same values on Serial Monitor, and it worked with no problem. it doesn't missing data when it prints to Serial Monitor, but it does when printing to LCD. In addition to that, I also implement a millis() timer to print the values periodically, but again it misses right after it prints to LCD.

Does anyone have any idea about how to do it better?

Any help appreciated,
Thank you

The code is below, I am using RPLidar.h Arduino library from here and LiquidCrystal_I2C.h library.


Code:
#include <RPLidar.h>
#include <math.h>

/// LCD
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd = LiquidCrystal_I2C(0x27, 20, 4);

// You need to create an driver instance
RPLidar lidar;

#define RPLIDAR_MOTOR 5 // The PWM pin for control the speed of RPLIDAR's motor.
// This pin should connected with the RPLIDAR's MOTOCTRL signal


// Enter parameters here
const int h = 1500; //The height of the Lidar from ground [mm]
const int tree_max = 10000; //Maximum possible height of the tree being sprayed
const int tree_min = 1000; //Minimum possible height of the tree being sprayed
const int alley_max = 24000; //Maximum possible alley way distance, i.e. max sensor range x2
const int alley_min = 2500; //Mininum possible alley way distance, i.e. min sensor range x2

const float teta_min = atan(float(alley_min / 2) / float(tree_max - h) );
const float teta_max = atan(float(alley_min / 2) / float(-h + tree_min));

float teta_min_r;
float teta_max_r;
float teta_min_l;
float teta_max_l;

int slope_min_r;
int slope_max_r;
int slope_min_l;
int slope_max_l;

int count_r = 0;
int count_l = 0;

int total_r;
int total_l;
int ave_r;
int ave_l;


const int ledPinRight = 14 ;
const int ledPinLeft = 11 ;

bool flagRight = false;
bool flagLeft = false;

int countRight = 0;
int countLeft = 0;

void setup() {

  Serial.begin(9600);

  if (teta_min < 0) {
    teta_min_r = 180 + (180 / PI) * teta_min;
  }
  else {
    teta_min_r = (180 / PI) * teta_min;
  }

  if (teta_max < 0) {
    teta_max_r = 180 + (180 / PI) * teta_max;
  }
  else {
    teta_max_r = (180 / PI) * teta_max;
  }

  teta_min_l = 360 - teta_max_r;
  teta_max_l = 360 - teta_min_r;

  slope_min_r = int(180 - (teta_min_r - 90)) % 180;
  slope_max_r = int(180 - (teta_max_r - 90)) % 180;
  slope_min_l = int(180 - (teta_min_l - 90)) % 180;
  slope_max_l = int(180 - (teta_max_l - 90)) % 180;

  Serial.print("teta_min_r: "); Serial.println(teta_min_r);
  Serial.print("teta_max_r: "); Serial.println(teta_max_r);
  Serial.print("teta_min_l: "); Serial.println(teta_min_l);
  Serial.print("teta_max_l: "); Serial.println(teta_max_l);
  Serial.println();
  Serial.print("slope_min_r: "); Serial.println(slope_min_r);
  Serial.print("slope_max_r: "); Serial.println(slope_max_r);
  Serial.print("slope_min_l: "); Serial.println(slope_min_l);
  Serial.print("slope_max_l: "); Serial.println(slope_max_l);


  //////////// For LiDAR /////////////////////////
  // bind the RPLIDAR driver to the arduino hardware serial
  lidar.begin(Serial1); //DG
  // set pin modes
  pinMode(RPLIDAR_MOTOR, OUTPUT);

  while (Serial1.read() >= 0) {};

  pinMode(ledPinRight, OUTPUT);
  pinMode(ledPinLeft, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(8, 0); lcd.print("DIST");
  lcd.setCursor(8, 1); lcd.print("COUNT");
  //lcd.setCursor(8, 2); lcd.print("FRAC");

  digitalWrite(ledPinRight, LOW);
  digitalWrite(ledPinLeft, LOW);

  total_r = 0;
  total_l = 0;

}

void loop() {
  if (IS_OK(lidar.waitPoint())) {

    float distance = lidar.getCurrentPoint().distance; //distance value in mm unit
    float angle    = lidar.getCurrentPoint().angle; //anglue value in degree
    bool  startBit = lidar.getCurrentPoint().startBit;
    byte  quality  = lidar.getCurrentPoint().quality; //quality of the current measurement
    //perform data processing here...

    if (angle > teta_min_r && angle < teta_max_r)
    {
      float x = sin(radians(angle)) * distance;
      float y = cos(radians(angle)) * distance;

      if (distance >= alley_min / 2 and y <= tan(radians(slope_min_r))*x and y <= (tree_max - h) and x <= alley_max / 2 and y >= (-h + tree_min) and y >= tan(radians(slope_max_r))*x)
      {
        count_r++;
        total_r = total_r + abs(x);
      }

    }
    else if (angle > teta_min_l && angle < teta_max_l)
    {
      float x = sin(radians(angle)) * distance;
      float y = cos(radians(angle)) * distance;

      if (distance >= alley_min / 2 and y <= tan(radians(slope_max_l))*x  and y <= ( tree_max - h) and x >= -(alley_max / 2) and y >= (-h + tree_min) and y >= tan(radians(slope_min_l))*x)
      {
        count_l++;
        total_l = total_l + abs(x);
      }
    }

    if (lidar.getCurrentPoint().startBit)
    {
      if (count_r > 3)
      {
        digitalWrite(ledPinRight, HIGH);
        flagRight = true;
        countRight = 0;

      }
      else
      {
        if (flagRight == true) {
          countRight++;
          if (countRight > 3) {
            digitalWrite(ledPinRight, LOW);
            flagRight = false;
            countRight = 0;
          }
        }
      }
      if (count_l > 3)
      {
        digitalWrite(ledPinLeft, HIGH);
        flagLeft = true;
        countLeft = 0;
      }
      else
      {
        if (flagLeft == true) {
          countLeft++;
          if (countLeft > 3) {
            digitalWrite(ledPinLeft, LOW);
            flagLeft = false;
            countLeft = 0;
          }
        }
      }
      ave_r = total_r / count_r;
      ave_l = total_l / count_l;
      Serial.println();
      Serial.print("count right: "); Serial.println(count_r);
      Serial.print("count left: "); Serial.println(count_l);
      //Serial.print("ave_r = "); Serial.print(total_r / count_r); Serial.println("  mm");
      //Serial.print("ave_l = "); Serial.print(total_l / count_l); Serial.println("  mm");
      Serial.println("######### end of one loop ################");

      lcd.setCursor(0, 0); lcd.print(ave_l);
      lcd.setCursor(16, 0); lcd.print(ave_r);

      count_r = 0;
      count_l = 0;
      total_r = 0;
      total_l = 0;
    }

  }
  else
  {
    analogWrite(RPLIDAR_MOTOR, 0); //stop the rplidar motor

    // try to detect RPLIDAR...
    rplidar_response_device_info_t info;
    if (IS_OK(lidar.getDeviceInfo(info, 100))) {
      // detected...

      lidar.startScan();

      // start motor rotating at max allowed speed
      analogWrite(RPLIDAR_MOTOR, 255);
      delay(1000);
    }
  }

}
 
Status
Not open for further replies.
Back
Top