RPLiDAR with Teensy 4.0

Status
Not open for further replies.
D

dhg

Member
Hello,

I have a project that I am using LiDAR scanner with Teensy 4.0 to detect trees. I modified Arduino code provided by the manufacturer, Slamtec, to retrieve data from lidar scanner and to process it to blink an LED as an indicator. What code simply does is that if there is a tree on the right-hand side of the scanner, the right LED will light-up and same happens for the left-hand side. I also add a 20x04 LCD screen for debugging purposes to see if everything is going the way that it is supposed to. I am printing an integer "count" on LCD screen to see how many cycles the code has made, and also it gives me an idea of how long it has been running without crashing as every cycle is 0.5 second.

The problem, however, is that it works for a while (sometimes 15 mins, and sometimes 40 mins) but it randomly crashes and restarts again. To be more explicit, when Teensy crashes, it creates a sound as you are physically unplugging and plugging back the USB cable. And it restarts and continues to execute the code from beginning.

Please see the following information:

  • The current Source code (Arduino):

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

const int numAngle = 360;
int angleIndex;
//int d;
int arrayCount[numAngle];
int arrayTotal[numAngle];
int arrayAverage[numAngle];
//int total;

// Enter parameters here
const int h = 1500; //The height of the Lidar from ground [mm]
const int tree_max = 7000; //Maximum possible height of the tree being sprayed
const int tree_min = 500; //Minimum possible height of the tree being sprayed
const int alley_max = 12000; //Maximum possible alley way distance, i.e. max sensor range x2
const int alley_min = 2000; //Mininum possible alley way distance, i.e. min sensor range x2
float teta_min_r;
float teta_max_r;
float teta_min_l;
float teta_max_l;
float slope_min_r;
float slope_max_r;
float slope_min_l;
float slope_max_l;

int total_r;
int total_qr;

int total_l;
int total_ql;

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

unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 500;           // interval at which to blink (milliseconds)

void setup() {

  float teta_min = atan((float(alley_min) / 2) / float(tree_max - h) );
  if (teta_min < 0) {
    teta_min_r = 180 + (180 / PI) * teta_min;
  }
  else {
    teta_min_r = (180 / PI) * teta_min;
  }

  Serial.print("teta_min_r: "); Serial.println(teta_min_r);

  float teta_max = atan((float(alley_min) / 2) / (float(tree_min - h)));
  if (teta_max < 0) {
    teta_max_r = 180 + (180 / PI) * teta_max;
  }
  else {
    teta_max_r = (180 / PI) * teta_max;
  }

  Serial.print("teta_max_r: "); Serial.println(teta_max_r);

  teta_min_l = 360 - teta_max_r;
  teta_max_l = 360 - teta_min_r;
  Serial.print("teta_min_l: "); Serial.println(teta_min_l);
  Serial.print("teta_max_l: "); Serial.println(teta_max_l);

  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.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 /////////////////////////
  Serial.begin(9600);

  // 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();

}
int count=1;
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; //whether this point is belong to a new scan
    byte  quality  = lidar.getCurrentPoint().quality; //quality of the current measurement

    //perform data processing here...

    angleIndex = lidar.getCurrentPoint().angle;
    arrayTotal[angleIndex] = arrayTotal[angleIndex] + lidar.getCurrentPoint().distance;
    arrayCount[angleIndex]++;

    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {
      for (int i = 0; i < numAngle; i++) { //This is averaging, i.e. putting all readings angle by angle into big array called arrayAverage 360x1
        arrayAverage[i] = arrayTotal[i] / arrayCount[i];
      }
      
      int count_r = 0;
      for (int a = teta_min_r; a <= teta_max_r; a++) {
        int x = sin(radians(a)) * arrayAverage[a];
        int y = cos(radians(a)) * arrayAverage[a];
        if (2000 <= arrayAverage[a] and arrayAverage[a]<= 3000) {
          count_r++;
        }
      }
      //Serial.print("count_r: ");Serial.print(count_r);
      //lcd.setCursor(17,0); lcd.print(count_r);
      if (float(count_r) /float((teta_max_r-teta_min_r))>=0.1) {
        //Serial.println("   RIGHT VALVE OPEN");
        //lcd.setCursor(15,1); lcd.print("OPEN");
        digitalWrite(ledPinRight, HIGH);
      }
      else {
        //Serial.println("  RIGHT VALVE CLOSE");
        //lcd.setCursor(15,1); lcd.print("CLOSE");
        digitalWrite(ledPinRight, LOW);
      }
        
  
  
      int count_l = 0;
      for (int a = teta_min_l; a <= teta_max_l; a++) {
        int x = sin(radians(a)) * arrayAverage[a];
        int y = cos(radians(a)) * arrayAverage[a];
        if (2000<= arrayAverage[a] and arrayAverage[a]<= 3000) {
          count_l++;
        }
      }
      //Serial.print("count_l: ");Serial.print(count_l);
      //lcd.setCursor(0,0); lcd.print(count_l);
      if (float(count_l) /float((teta_max_l-teta_min_l))>=0.1) {
        //Serial.println("    LEFT VALVE OPEN");
        //lcd.setCursor(0,1); lcd.print("OPEN");
        digitalWrite(ledPinLeft, HIGH);
      }
      else {
        //Serial.println("     LEFT VALVE CLOSE");
        //lcd.setCursor(0,1); lcd.print("CLOSE");
        digitalWrite(ledPinLeft, LOW);
      }
      //Serial.print("0: "); Serial.print(arrayAverage[0]); Serial.print("\t");Serial.println(arrayCount[0]);
      //Serial.print("90: "); Serial.print(arrayAverage[90]);Serial.print("\t");Serial.println(arrayCount[90]);
      //Serial.print("180: "); Serial.print(arrayAverage[180]);Serial.print("\t");Serial.println(arrayCount[180]);
      //Serial.print("270: "); Serial.print(arrayAverage[270]);Serial.print("\t");Serial.println(arrayCount[270]);
      //lcd.setCursor(0,3); lcd.print("count:    ");
      lcd.setCursor(10,3); lcd.print(count);
      //Serial.print("count: ");Serial.println(count);
      //Serial.println();
      //Serial.println("done");


      memset(arrayCount, 0, sizeof(arrayCount));
      memset(arrayTotal, 0, sizeof(arrayTotal));
      memset(arrayAverage, 0, sizeof(arrayAverage));
       
      count++;
      previousMillis = currentMillis;
    }

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

}



  • I am not getting any error message when I am uploading the code to Teensy and neither when it crashes, but here is the console from Arduino IDE:


HTML: 
Teensy_Lidar_indoor: In function 'void loop()':
Teensy_Lidar_indoor:129: warning: unused variable 'x' 
         int x = sin(radians(a)) * arrayAverage[a];
             ^
Teensy_Lidar_indoor:130: warning: unused variable 'y' 
         int y = cos(radians(a)) * arrayAverage[a];
             ^
Teensy_Lidar_indoor:152: warning: unused variable 'x' 
         int x = sin(radians(a)) * arrayAverage[a];
             ^
Teensy_Lidar_indoor:153: warning: unused variable 'y' 
         int y = cos(radians(a)) * arrayAverage[a];
             ^
Teensy_Lidar_indoor:112: warning: unused variable 'startBit' 
     bool  startBit = lidar.getCurrentPoint().startBit; //whether this point is belong to a new scan
           ^
Teensy_Lidar_indoor:113: warning: unused variable 'quality' 
     byte  quality  = lidar.getCurrentPoint().quality; //quality of the current measurement
           ^
Memory Usage on Teensy 4.0:
  FLASH: code:18488, data:4184, headers:9068   free for files:1999876
   RAM1: variables:17088, code:16616, padding:16152   free for local variables:474432
   RAM2: variables:12384  free for malloc/new:511904

  • I am using Teensy 4.0 with Windows 10, and Arduino IDE.


  • The Lidar scanner I am using is RP LiDAR A1 from the manufacturer Slamtec, and I found the repository here. I modified the Arduino code called simple_connect.ino.


For some reason, I couldn't upload the photos of my wiring diagram. But if needed, I can send it directly via e-mail.

I really appreciate any help.

Thank you
 
Last edited:
I haven't found any obvious problems in a casual look through the code but this code strikes me as odd:
Code: 
      for (int a = teta_min_r; a <= teta_max_r; a++) {
        int x = sin(radians(a)) * arrayAverage[a];
        int y = cos(radians(a)) * arrayAverage[a];
        if (2000 <= arrayAverage[a] and arrayAverage[a]<= 3000) {
          count_r++;
        }
      }
The variables 'x' and 'y' only exist inside the for loop and are not used at all - which is why you get the first four warning messages. The compiler will probably optimize the computations out altogether but why are they here? There's another, similar for loop a bit further down in the code.

Pete
 
You can use the "Go Advanced" button to attach an image to your message. It will allow bmp gif jpe jpeg jpg pdf png

Pete
 
resize1.jpegresize2.jpeg
 
Hello Pete,

Thank you very much for your response. I think the problem with the pictures was that their size were big (>2MB). Now, I resize them and able to upload them, hope they are clear enough to see which pins the wires are connected.

You are right, the variables 'x' and 'y' are not used at all. They are from the original code, I should have deleted them. I comment them out, and run the code for a while and still it crashed and restarted itself.

I will try running it without printing anything neither in LCD nor in Serial monitor, and see if it will still crashes.

Thank you.
 
If it makes an audible sound when crashing, my first thought is brownout. Looking at the lidar datasheet, I can see the maximum current draw of the system might exceed 500mA, which for some USBs would be too much. If you're powering this from a laptop or a USB port of unknown current output, try moving it over to something beefier.

Otherwise, in software, the most likely culprit I can guess would be accessing past the end of an array. I haven't read the manual, but at line 116 is it possible that lidar.getCurrentPoint().angle could return a value of 360 instead of 0?
Using 360 as an index to your arrays would be invalid and might cause problems.

A few other things I can see that probably aren't the problem but worth correcting

- All of your minimums and maximums are calculated at run time during setup(), it would be better to declare these as const float so it can be done at compile.
- A number of calls to serial are made before serial.begin(), teensy doesn't care, but it's bad practice.
 
Hi Edward,

Thank you very much for your response, sorry it took me a while to turn back as I run the code again and again to make sure I am replying an healthier way back to the forum.

I went over all of your recommendations, tried to fixed them by my own ways, but it is still crashing and restarting after a while:

Edward said:
If it makes an audible sound when crashing, my first thought is brownout. Looking at the lidar datasheet, I can see the maximum current draw of the system might exceed 500mA, which for some USBs would be too much. If you're powering this from a laptop or a USB port of unknown current output, try moving it over to something beefier.
Click to expand...

I have a 5 VDC power supply with 2.5A output right now, which should be more than enough for the system. No USB connected to Teensy, I am simply plugging +5 VDC from power supply to Teensy's Vin pin and GND to GND. And the rest of the set-up (Lidar and LCD) remains the same as the pictures I have already attached in my previous posts.





Edward said:
Otherwise, in software, the most likely culprit I can guess would be accessing past the end of an array. I haven't read the manual, but at line 116 is it possible that lidar.getCurrentPoint().angle could return a value of 360 instead of 0?
Using 360 as an index to your arrays would be invalid and might cause problems.
Click to expand...

Here is the new code, I added the lines 113&114, not sure if it is the best way correcting it:
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

const int numAngle = 360;
int angleIndex;
//int d;
int arrayCount[numAngle];
int arrayTotal[numAngle];
int arrayAverage[numAngle];
//int total;

// Enter parameters here
const int h = 1500; //The height of the Lidar from ground [mm]
const int tree_max = 7000; //Maximum possible height of the tree being sprayed
const int tree_min = 500; //Minimum possible height of the tree being sprayed
const int alley_max = 12000; //Maximum possible alley way distance, i.e. max sensor range x2
const int alley_min = 2000; //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(tree_min - h)));

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

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

int total_r;
int total_qr;

int total_l;
int total_ql;

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

unsigned long previousMillis = 0;        // will store last time LED was updated
const long interval = 500;           // interval at which to blink (milliseconds)

void setup() {
  //////////// For LiDAR /////////////////////////
  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;
  
  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);


  // 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();

}
int count = 1;
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; //whether this point is belong to a new scan
    //byte  quality  = lidar.getCurrentPoint().quality; //quality of the current measurement

    //perform data processing here...

    angleIndex = lidar.getCurrentPoint().angle;
    if (angleIndex == 360) {
      angleIndex = 0;
    }
    arrayTotal[angleIndex] = arrayTotal[angleIndex] + lidar.getCurrentPoint().distance;
    arrayCount[angleIndex]++;

    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= interval) {
      for (int i = 0; i < numAngle; i++) { //This is averaging, i.e. putting all readings angle by angle into big array called arrayAverage 360x1
        arrayAverage[i] = arrayTotal[i] / arrayCount[i];
      }

      int count_r = 0;
      for (int a = teta_min_r; a <= teta_max_r; a++) {
        //int x = sin(radians(a)) * arrayAverage[a];
        //int y = cos(radians(a)) * arrayAverage[a];
        if (2000 <= arrayAverage[a] and arrayAverage[a] <= 3000) {
          count_r++;
        }
      }
      //Serial.print("count_r: ");Serial.print(count_r);
      //lcd.setCursor(17,0); lcd.print(count_r);
      if (float(count_r) / float((teta_max_r - teta_min_r)) >= 0.1) {
        //Serial.println("   RIGHT VALVE OPEN");
        //lcd.setCursor(15,1); lcd.print("OPEN");
        digitalWrite(ledPinRight, HIGH);
      }
      else {
        //Serial.println("  RIGHT VALVE CLOSE");
        //lcd.setCursor(15,1); lcd.print("CLOSE");
        digitalWrite(ledPinRight, LOW);
      }



      int count_l = 0;
      for (int a = teta_min_l; a <= teta_max_l; a++) {
        //int x = sin(radians(a)) * arrayAverage[a];
        //int y = cos(radians(a)) * arrayAverage[a];
        if (2000 <= arrayAverage[a] and arrayAverage[a] <= 3000) {
          count_l++;
        }
      }
      //Serial.print("count_l: ");Serial.print(count_l);
      //lcd.setCursor(0,0); lcd.print(count_l);
      if (float(count_l) / float((teta_max_l - teta_min_l)) >= 0.1) {
        //Serial.println("    LEFT VALVE OPEN");
        //lcd.setCursor(0,1); lcd.print("OPEN");
        digitalWrite(ledPinLeft, HIGH);
      }
      else {
        //Serial.println("     LEFT VALVE CLOSE");
        //lcd.setCursor(0,1); lcd.print("CLOSE");
        digitalWrite(ledPinLeft, LOW);
      }
      //Serial.print("0: "); Serial.print(arrayAverage[0]); Serial.print("\t");Serial.println(arrayCount[0]);
      //Serial.print("90: "); Serial.print(arrayAverage[90]);Serial.print("\t");Serial.println(arrayCount[90]);
      //Serial.print("180: "); Serial.print(arrayAverage[180]);Serial.print("\t");Serial.println(arrayCount[180]);
      //Serial.print("270: "); Serial.print(arrayAverage[270]);Serial.print("\t");Serial.println(arrayCount[270]);
      lcd.setCursor(0, 3); lcd.print("count:    ");
      lcd.setCursor(10, 3); lcd.print(count);
      //Serial.print("count: ");Serial.println(count);
      //Serial.println();
      //Serial.println("done");


      memset(arrayCount, 0, sizeof(arrayCount));
      memset(arrayTotal, 0, sizeof(arrayTotal));
      memset(arrayAverage, 0, sizeof(arrayAverage));
      // save the last time you blinked the LED

      count++;
      previousMillis = currentMillis;
    }

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

}


Edward said:
A few other things I can see that probably aren't the problem but worth correcting

- All of your minimums and maximums are calculated at run time during setup(), it would be better to declare these as const float so it can be done at compile.
- A number of calls to serial are made before serial.begin(), teensy doesn't care, but it's bad practice.
Click to expand...

As can be seen in the code, I moved them outside the set-up.

The other thing that might give a clue for the problem is that sometime later after it starts, the LCD (20x04) screen shows values in corrupted way (see the attached picture), but the LiDAR still works for a while, meaning it turns the both right and left LEDs on/off in a proper way even though LCD screen is corrupted. And after a while later, Lidar restarts and LCD is cleared and it outputs the "count" value of each cycle in a correct way. I am not sure if it is an indication for something.

resize4.jpeg

Any other thougts would be appreciated.

Thank you
 
Status
Not open for further replies.
Back
Top