Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 7 of 7

Thread: RPLiDAR with Teensy 4.0

Threaded View

  1. #1
    Junior Member
    Join Date
    Jul 2021
    Posts
    5

    RPLiDAR with Teensy 4.0

    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 Code:
    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 by dhg; 07-25-2021 at 09:04 PM.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •