Having Trouble Displaying Data from RoboPeak Lidar

Status
Not open for further replies.

spumoni

Member
Hi,

I have interfaced this RoboPeak Lidar http://www.robopeak.com/blog/?cat=5
with a Teensy 3.2. Teensy is getting data from the Lidar serial port (115200 baud) @ 2000 samples
per sec and then formatting and sending it out on another serial port.

The data format is as follows:
A####R####lf where A = token for angle R = token for range

Feeding the output into a Serial monitor Putty program on my computer clearly shows the
data streaming out correctly.

I want to display the data on a ILI9341 TFT display but at this point I am not even
successful at reading and parsing the incoming data. For this I am also using another Teensy 3.2
interfaced to the TFT display using SPI.

My code in the main loop below is reading the serial data from the port and parsing the incoming
character string. When the line feed is received, the characters arrays are converted to integers.
Currently, nothing is displaying on the monitor.

I have tried with another non-scanning lidar with the same data format and I am successful
at reading and displaying the data.

Questions:
1) What am
2) Is the data rate too much for Teensy?
3) Is there another approach I can take?

As an aside, I sometimes have problems with Teensy serial monitor ports.
Sometimes they work, other times they don't.

Any help or suggestions would be greatly appreciated.
Thanks in advance!



Code:
#include <ILI9341_t3.h>
#include <font_Arial.h>      
#include <SPI.h>

#define TFT_DC_PIN     9
#define TFT_CS_PIN    10

ILI9341_t3 tft = ILI9341_t3 (TFT_CS_PIN, TFT_DC_PIN);

//-----------------------------------------------------------------------------------------------
void setup() 
{
  Serial.begin(115200);
//while(!Serial);
  delay(3000);               

  Serial1.begin (115200);   // In coming Serial Data from LidarLite 
  delay(1000);
  
  tft.begin();
  tft.setRotation (1);
  tft.setTextSize (1);
  tft.fillScreen (ILI9341_BLACK);
  }

//-----------------------------------------------------------------------------------------------
// Main Loop
// Incoming Serial Data format:  A####R####lf
// where A = angle   R =  Range (deg)
//-----------------------------------------------------------------------------------------------
void loop()
{
  int iAng, iRng;
  int cnt;
  char cChar, cFld;
  char cAng[7], cRng[7];             // Max 6 digits plus null string terminator

  if (Serial1.available() > 0)
  {
    while (Serial1.available() > 0)
    {
       cChar = Serial1.read();
       Serial.print(cChar);          //NOTHING GETS PRINTED HERE!!!
       
       if (cChar == '\n')            // New Line
       {
         cRng[cnt] = '\0';           // Terminate string for Range

         iAng = atoi(cAng);          // Output: Angle   -90 to +90 deg
         iRng = atoi(cRng);          //         Range   0 to 400 cm
         cnt = 0;
       }
       else if (cChar == 'A')        //Field delimiter Angle
       {
         cFld = cChar;
         cnt = 0;
       }
       else if (cChar == 'R')        //Field delimiter Range
       {
         cAng[cnt] = '\0';           //Terminate string for Angle 

         cFld = cChar;
         cnt = 0;
       }
       else                          //Build String with numerical values
       {
         if      (cFld == 'A') cAng[cnt] = cChar;  // -90 to +90 degrees
         else if (cFld == 'R') cRng[cnt] = cChar;  // 0 - 400cm
         cnt++;
         if (cnt > 6) cnt = 6;       //String size - don't overflow
      }
    }
  }
  delay (10);
}
 
your program works (prints input line to monitor) on one Teensy, if i run it with data coming from this sketch on a 2nd Teensy
Code:
void setup() {
 Serial1.begin (115200);   // In coming Serial Data from LidarLite 
  delay(1000);
}

void loop() {
  Serial1.println("A1234R5678");
  delay(1000);
}

And Tx on transmit Teensy connected to Rx on receiving Teensy and common GND.

2000 samples of 11 chars at 10bits/char (baud) is 220000 baud! too fast for Serial.begin(115200); ?? ... and to that you have a delay(10);

Receiver works with transmitter using delay(5), but misses characters with delay(1), and hangs with no delay in transmitting sketch.

Does device have a TTL or RS232 interface? Teensy Serial1 is TTL, so you'll need to convert RS232 to TTL ?

Arduino library https://github.com/robopeak/rplidar_arduino
and discussion https://forum.arduino.cc/index.php?topic=426712.0
 
Last edited:
Hi,
Here is the simple code running on a Teensy 3.2 and outputting serial data at 115200 to Putty on my computer. It is pretty much the same as what you provided on your links.

Don't understand why I can spit it out at 115200 but cannot read it in with another Teensy 3.2!
Thanks for the help!

Code:
//------------------------------------------------------------------------------------------
// Teensy 3.2
// Read data from Robo Peak 360 Degree Scanning Lidar
// Outputs A###R#####\n      A = angle 0-359    R = range in mm
// Serial1 incoming data from RP Lidar
// Serial2 to another Teensy with TFT display
//------------------------------------------------------------------------------------------ 
#include <RPLidar.h>
RPLidar lidar;

#define RPLIDAR_MOTOR_PIN  6        

//------------------------------------------------------------------------------------------ 
// Setup                       
//------------------------------------------------------------------------------------------                        
void setup() 
{
  Serial.begin (115200);
//while(!Serial);
  delay(3000);
 
  lidar.begin(Serial1);
  
  delay(2000);
  pinMode(RPLIDAR_MOTOR_PIN, OUTPUT);
  
  Serial2.begin(115200);          // External output
  
}

float distance;   //distance value in mm unit
float angle;      //angle value in degree
bool  startBit;   //whether this point is belong to a new scan
byte  quality;    //quality of the current measurement
//------------------------------------------------------------------------------------------  
// Main Loop                      
//------------------------------------------------------------------------------------------                        
void loop() 
{
  if ( IS_OK(lidar.waitPoint()) ) 
  {
    distance = lidar.getCurrentPoint().distance;   //distance value in mm unit
    angle    = lidar.getCurrentPoint().angle;      //angle value in degree
    startBit = lidar.getCurrentPoint().startBit;   //whether this point is belong to a new scan
    quality  = lidar.getCurrentPoint().quality;    //quality of the current measurement
   
    if (lidar.getCurrentPoint().startBit)     
    {
      Serial.print ("A");Serial.print (distance); 
	  Serial.print ("R"); Serial.print (angle);
	  Serial.print ('\n');
    } 

  } 
  else 
  {
    analogWrite(RPLIDAR_MOTOR_PIN, 0);         // Stop motor
    rplidar_response_device_info_t info;       // Check Status
    
    if (IS_OK(lidar.getDeviceInfo(info, 100))) 
    {
       lidar.startScan();                      // Start Scanning
       analogWrite(RPLIDAR_MOTOR_PIN, 255);    // Start Motor
       delay(1000);                           
    }
  }
}
 
I don't understand how you have things wired. You have two T3.2 and LiDar? Tx connected to Rx and common GND? attach a photo of your wiring ...
 
I actually have two setups. The first is a Teensy getting serial data from the RP Lidar on Serial1 port and just spitting out Angle and Range on the Serial2 port. That's the code immediately above.

The second is another Teensy controlling the TFT display. It is taking the Angle and Range serial data from the first Teensy into its Serial1 port. That data is now being displayed in a polar plot on the TFT. My problem lies in the input of the serial data to this 2nd Teensy's serial port. In the serial read routine, I can't even print out the incoming characters. Just can't seem to read data from serial1 and spit it back out to the Serial monitor.
 
I suspect you need to enable hardware flow control on the serial ports, so that each side can say, 'slow down, my buffers are full'. I've not actually used these on a Teensy (I did use hardware flow control back in the day when computers still had serial ports and the boards we were debugging also used serial ports). But here is the description of how to enable RTS and CTS (note CTS is a fixed pin for each serial port). Like the hook-up where you link one boards RX to the others TX and vice versa, you would hook one boards CTS to the others RTS, and vice versa:

You also want to use just Serial1/Serial2 as those two serial ports have an 8 byte deep fifo that can help avoid losing bytes if interrupts are disabled.
 
Would be good to see the 2nd Serial rcv Teensy sketch.

Above in 1st Teensy it shows Serial2.begin - but I don't see Serial2.print?

Is that what these should be?
Code:
    if (lidar.getCurrentPoint().startBit)     
    {
      Serial.print ("A");Serial.print (distance); 
	  Serial.print ("R"); Serial.print (angle);
	  Serial.print ('\n');
    }

the 115200 rate is pretty slow so as long as 2nd Teensy is polling or waiting and reading messages until '\n' the buffer should not overflow as those look to be short messages so a few would fit the 64 byte buffer.
 
Ahhh, that would be a problem.

Sorry, I am not using a Teensy with the Lidar. It is an Arduino Pro Mini, so the Serial.print is correct. Taking this serial output into Putty shows that data streaming out.

Defragster, If you look at my initial post #1, it shows the receiving code. Maybe I'll try another Teensy.

Will look into CTS,RTS but I am not sure how to set it on the Arduino Pro Mini side.
 
Sorry, I am not using a Teensy with the Lidar. It is an Arduino Pro Mini, so the Serial.print is correct. Taking this serial output into Putty shows that data streaming out.
.

What?
your post #3 says "Here is the simple code running on a Teensy 3.2 and outputting serial data at 115200 to Putty on my computer. It is pretty much the same as what you provided on your links." and the sketch references Serial2 (which wouldn't compile on a pro mini -- you'd need Software Serial)

and in post 5 you say " I actually have two setups. The first is a Teensy getting serial data from the RP Lidar on Serial1 port and just spitting out Angle and Range on the Serial2 port. That's the code immediately above. " ?? that says you are running Lidar on Teensy ??

you'd need to connect Serial2 Tx to the other Teensy Serial1 Rx, and add Serial2.print()'s

here is your lidar sketch in post #3 with Lidar stuff removed and using int's for distance and angle
Code:
//------------------------------------------------------------------------------------------
// Teensy 3.2
// Read data from Robo Peak 360 Degree Scanning Lidar
// Outputs A###R#####\n      A = angle 0-359    R = range in mm
// Serial1 incoming data from RP Lidar
// Serial2 to another Teensy with TFT display
//------------------------------------------------------------------------------------------

#define RPLIDAR_MOTOR_PIN  6

//------------------------------------------------------------------------------------------
// Setup
//------------------------------------------------------------------------------------------
void setup()
{
  Serial.begin (115200);
  //while(!Serial);
  delay(3000);

  pinMode(RPLIDAR_MOTOR_PIN, OUTPUT);

  Serial2.begin(115200);          // External output

}

int distance;   //distance value in mm unit
int angle;      //angle value in degree
bool  startBit;   //whether this point is belong to a new scan
byte  quality;    //quality of the current measurement
//------------------------------------------------------------------------------------------
// Main Loop
//------------------------------------------------------------------------------------------
void loop()
{

  distance = 1234;
  angle    = 5678;

  Serial.print ("A"); Serial.print (distance);
  Serial.print ("R"); Serial.print (angle);
  Serial.print ('\n');
  Serial2.print ("A"); Serial2.print (distance);
  Serial2.print ("R"); Serial2.print (angle);
  Serial2.print ('\n');
  delay(1000);

}

With teensy pin 10 (Serial2 Tx) connected to Serial1 Rx (pin 0) on the other teensy running your sketch in post #1, i get the results from one teensy to the other and printed to the monitor. Need common GND between the teensy's
 
Last edited:
Hi,
I am so, so sorry for confusing you! I was confusing myself because I am actually working with 2 lidars. The first is the RP 360 degree scanning Lidar with a Teensy that I am having trouble with. The second is a Lidar Lite with an Arduino Pro Mini which I have on a servo and is doing a 180 degree scan. Both are putting angle & range data to a serial port. This morning, I was working with the Lidar Lite when I replied so I was confusing both of us - sorry.

The Teensy with the TFT is my display to show the data from either lidar unit. The Lidar Lite data displays correctly. The RP Lidar did not even show the incoming serial data. I have found out why. When I plugged the usb cord into a USB Multifunction Tester, it showed that I was pulling .42A from the RP Lidar and the two Teensies. This caused the Teenies to shut down and try to reboot.

I put an external lipo battery with more juice and now I am getting the serial data! Will continue coding to see if the TFT display can keep up.

Sorry for leading you (and myself) down the rabbit hole and again thank you very much for taking the time to help me! People on this forum are great!
 
Hi,
Got this project but I have couple small problems that are bothering me.

This first has to do with power. I have two Teensy, the 1st running the lidar and the 2nd running the TFT. When I plug the USB power into the 1st Teensy everything works fine. If I plug the USB power into the 2nd Teensy, the TFT display correctly and the lidar Teensy starts the scanning motor on the lidar. When the motor starts the Teensy on the TFT resets and the screen starts flashing. It looks like the 2nd Teensy cannot supply enough current for everything, yet the first can.

I have recrimped all my connectors and the motor power input to the lidar is connected directly to Vin & Grd of the Teensy. (tried connecting to either Teensy with same results. It seems like the 2nd Teensy is shutting down when its load is about .5 amp, yet the 1st Teensy can handle it. What is max current capacity of Teensy 3.2? Should I try replacing the 2nd Teensy?

On the second issue, I incorporated the touch screen code into the display. Without it everything displays correctly. When I just instantiate the touch, I get a few random red dots plotted on the screen! Nowhere in the code do I use the color RED! Also, I believe I am using the non-interrupt mode. Anyone have ideas on this?
 
Status
Not open for further replies.
Back
Top