Skip to content

Interface and application programming

This week we had to :

  • Write an application that interface a user with an input &/or output device we made
  • Compare as many tool options as possible

Lidar analyser

Each year, the French robotics cup call hundreds of robotics enthousiast. As one of them, and member of a team, I’m taking parts to the challenge since 2017.

This challenge isn’t about robot figthing even if lot’s of them die in the process… The challenge is peaceful and robot needs to acheive complex tasks such as object manipulation, strategy … Of course the robots are fully autonomous, when the match start, we can’t interfere in the program.

If you’re intersted by how what kind of task the robot should make, please watch a short match.

This year we decided to implements a Lidar in our robot to avoid opponent. As we had some problem with our previous solution, switching to Lidar seems a good idea. However, I need to mention that we are only using microcontroler in our robots. Thus, I need to optimize the code to be powerful enough to run on a microcontroler. Which wasn’t so easy.

How it works ?

We are using two Teensy board in ou robot. The first is in charge of : - Strategy - HMI - Actuators - Communication - Sensors - Motion planning - …

The second one is in charge of the Lidar. We are not sure if the secondary teensy is neccessary but for a prototype it’s ok.

I began by programming the teensy to get data from the Lidar. I used a cheap LD06 Lidar. This lidar is available for ~80€ in france You have propably seen a lidar one day. Their are very common in autonomous vacuum robot :

Lidar are a great piece of technologie. Their are in fact faster than light ! No joke, here is how it works :

A laser is emmiting light which reflect on a distant object and come back to the photosensor. The onboard microcontroleur measure the time the light took to make the travel and the distance is deducted from those measure.

Our LD06 is a 2D Lidar fast enough to perform 4500 measure per second ! Impressive ! Obviously I won’t read and parse those 4500 point. But I will use part of this information to perform obstacle avoidance and path planning.

Microcontroller (C++) program :

Here is the code running in the Teensy :

//   _______       _       _      _     _            
//  |__   __|     (_)     | |    (_)   | |           
//     | |_      ___ _ __ | |     _  __| | __ _ _ __ 
//     | \ \ /\ / / | '_ \| |    | |/ _` |/ _` | '__|
//     | |\ V  V /| | | | | |____| | (_| | (_| | |   
//     |_| \_/\_/ |_|_| |_|______|_|\__,_|\__,_|_|                                                                            
//     Author  : Nadarbreicq, JulesTopart
//     Version : 0.0.1
//     Last update : 24 / 04 / 2022

#include "TwinLidar.h"

void setup(){
    //--- INIT ---
    Debugger::log << "Init OK.";

void loop(){


Not a lot to say about this part. The teensy initiatlize the serial communication with : - The PC : Debugger - The lidar : The sensor - Intercom : The communication with the main microcontroller (Strategy)

Then the loop function execute forever. First, the teensy read the incomming data from the lidar and store them as Points (distanse and angle) in an array. Then, It just copy those data into the PC Serial port to forward data to my processing application.

If your interested by the code, eveerything is available Here

Processing (Java) application

Then I began to work on my Processing application Processing is a very simple IDE made to facilitate the developpement of small software.

In processing, as an ancestor of arduino, you’ll use 2 main function : - setup : which is executed once - draw : Which is executed forever

Each time the draw function is executed, the graphical application is rendered. Which means that if you draw things in the draw functions, they will be rendered at the end of the function execution.

I began by drawing/programming a simple coordinate software. As my lidar will display point using distance and angle, It make sens for me to use concentric circles.

int circleCount = 10; //Number of concentric circle
int spacing = 10;   //spacing between circles

void setup(){
  size(600,800);    //Create the 600x800 window

void draw(){
    background(200); // Reset the screen by filling with light gray
    noFill();       // Disable filling to make circle hollow
    stroke(255);    // Use White as circle color
    for(int i = 1 ; i <=  circleCount; i++){


When you click render you get this :

After a while, I created a new Tab and a class to render the coordinates system properly. Here is the function :

PolarSystem coordSystem = new PolarSystem();

class PolarSystem{

  float radius = 0.8;
  int subdivision = 20;


  void draw(){


     float dR = radius/subdivision;
     float dRpix = toPixel(dR);
     for(int i = 1; i <= subdivision; i++){
       line( dRpix*i/2 - 5, 0,   dRpix*i/2 + 5 ,0);
       line(-dRpix*i/2 - 5, 0,  -dRpix*i/2 + 5 ,0);
       line(0,  dRpix*i/2 - 5, 0,  dRpix*i/2 + 5);
       line(0, -dRpix*i/2 - 5, 0, -dRpix*i/2 + 5);

void setup(){
  size(800,600);    //Create the 600x800 window

void draw(){
  background(200); // Reset the screen by filling with light gray


Here is the result :

Perfect ! Isn’t it ? Now It’s time for Serial communication.

To gather data from the lidar, I need to communicate with it using the USB Serial. There is a library in charge of the serial communication in Processing.

So connecting to USB Serial is simple as that :

import processing.serial.*;          // Import the serial library
Serial myPort;                       // Create the serial port object

boolean connected = false;
int baud = 115200;

public void serialBegin(String port){
 myPort = new Serial(this, port, baud);
 println("Serial connection has begin on port " + port);

This small function will open the desired Serial port. After that, each time the teensy is sending data trough the serial port, Processing will call an event function named serialEvent.

Here is the difficult part. I began by just printing the incomming message trough the console :

void serialEvent(Serial p){
    if(connected == false) connected = true;
    while(p.available() > 0){

When you run the sketch, you get the first data coming trough the Teensy straight to the PC Serial port.

To facilitate the Serial connection I decided to add a very simple UI. This UI will allow the user to switch the COM port of the board. To make the UI Simple, I used a GUI Library nammed ControlP5.

Here is how to create a few buttons and a dynamic list containing available COM Ports:

import controlP5.*;                  // Import the CP5 library
ControlP5 cp5;                       // Create the CP5 object

// Dynamic list
ScrollableList serialList;

// Buttons
Button serialBegin;
Button serialStop;
Button serialRefresh;

// State of the buttons
boolean clicSerialBegin = false ;
boolean clicSerialStop = false ;
boolean clicSerialRefresh = false ;

void createGUI()

  cp5 = new ControlP5(this); // Initialize Library Event handler

     .setLabel("Liaison Serie")

  serialList = cp5.addScrollableList("dropdown")
     .setCaptionLabel("Liste des liaisons serie")
     .setPosition(20, 20)
     .setSize(170, 800)

  serialRefresh = cp5.addButton("serialRefresh")

  serialBegin = cp5.addButton("serialBegin")

  serialStop = cp5.addButton("serialStop")

   println("Bievenue dans l'interface LiDar");
   println("Appuyez sur 'h' pour faire apparaitre l'aide");


  cp5.get(Button.class,   "serialBegin")  .setVisible(GUIVisible);
  cp5.get(Button.class,   "serialStop")   .setVisible(GUIVisible);
  cp5.get(Button.class,   "serialRefresh").setVisible(GUIVisible);
  cp5.get(ScrollableList.class, "dropdown").setVisible(GUIVisible);

void keyPressed()
  if ( key == 'v' ) GUIVisible = !GUIVisible;
  if ( key == 'h' )
    println("Help for the beacon interface");
    println("'h' - Help");
    println("'v' - Visibility of the GUI");

Here is What you get when running the createGUI function :

We need to somehow parse those data to store a list of Points. I created a Point class to store the distance and the angle of the point in polar coordinates and an other one in cartesian coordinate. When data are incomming, their are checked, converted, and store in a point array.

class Point{
  PVector pos;
  color c;
  int opacity;
  int cluster = 0;

    pos = new PVector();
    pos.x = random(-1,1);
    pos.y = random(-1,1);
    c = color(255,0,0);
    opacity = 255;

  Point(float x, float y){
    pos = new PVector();
    pos.x = x;
    pos.y = y;
    c = color(255,0,0);
    opacity = 255;

  Point(PVector origin){
    pos = new PVector();
    pos.x = origin.x;
    pos.y = origin.y;
    c = color(255,0,0);
    opacity = 255;

  void update(){
    opacity -= 5;
    if(opacity < 0) opacity = 0;

  void draw(){
    translate(width/2, height/2);
    stroke(c, opacity);


class PolarPoint{
  float _distance, _theta;
  int _intensity;

    _intensity = 100;
    _distance = random(-0.8,0.8);
    _theta = random(-0.8,0.8);

  PolarPoint(float distance, float theta, int intensity){
    _intensity = intensity;
    _distance = distance;
    _theta = theta;

  void draw(){
    stroke(map(_intensity, 0,100,0,150),0,0);


  void debug(){
    println("polar coordinate : [" + _theta + "°, " + _distance + " u]"); 

  Point toPoint(){
     return new Point((_distance* cos(_theta)), (_distance*sin(_theta)));


Now I able to display the points on the coordinate system.


The last step is to interprete those points to take decision. My my goal is to detect and opponent, not obstacles. This is my attempt on K-Mean clustering algorithm :

More info on K-mean algorithm here :

KMean is easy


Here is my full code, If you are interested :

Last update: June 28, 2022