title: Speed measurement from lidar sensor for automatic braking system
body: <p>For a school project I have been working on a system that will automatically brake when there is danger. Only the requirements are more strict (safety) than just using a ping sensor and detecting a fixed distance. I am going to implement the speed of an incoming object as well.</p>
  
  <p>On the internet I found this awesome 100Hz sample Lidar module that's very cheap. It uses UART with 2 header frames (0x59), after that a dataLowbyte and a dataHighbyte. Here is the <a href="http://www.robotshop.com/media/files/pdf/benewake-tfmini-micro-lidar-module-ip65-12-m-datasheet.pdf" rel="nofollow noreferrer">datasheet for the module</a>.</p>
  
  <p>So I've started inputting coffee and outputting code (in C) on the atmega328P (Arduino).</p>
  
  <p>As you can see here I've set up the code to read the sensor and process the data. The problem is that it gives back weird readings. Like a distance difference when I'm moving further away <em>and</em> when moving closer even though I stated it not to. Does anyone have experience using this module? Or maybe I misread something of the datasheet.</p>
  
  <pre><code>//Includes
  //============================================================================================//
  #include &lt;avr/io.h&gt;
  #include &lt;avr/interrupt.h&gt;
  #include &lt;math.h&gt;
  //============================================================================================//
  
  
  //Defines
  //============================================================================================//
  #define F_CPU 16000000                                          //CPU frequency
  #define Baudrate 115200                                         //UART Baudrate
  #define UBRR_VALUE (((F_CPU / (Baudrate * 8))) - 1)             //UART Baudrate set in Arduino
  // #define SafetyRating 8
  // #define sampleTime 0x65                                          //Time between 2 samples in 10^-4. This value comes from testing (its 1,01ms)
  //============================================================================================//
  
  
  //Enums
  //============================================================================================//
  enum {Safe, Warning, Danger}State;                              //Possible States
  //============================================================================================//
  
  
  //Global variables
  //============================================================================================//
  uint8_t distanceL;                                              //Distance (low value) data taken from the UART buffer coming from Lidar
  uint8_t distanceH;                                              //Distance (high value) data taken from the UART buffer coming from Lidar
  uint16_t Distance;                                              //Distance combined
  uint8_t frameByteCount = 0;                                     //To count which byte in the frame is current
  uint16_t Speed;                                                 //Speed variable
  //volatile uint8_t DangerLevel;                                 //Danger indicator
  
  
  //Function prototypes
  //============================================================================================//
  void calcSpeed(uint16_t Measurement);                       //Function to calculate the speed out of distance readings
  void UART_Transmit(uint8_t data);                               //Funtion to send data via uart
  //============================================================================================//
  
  
  //Interrupts
  //============================================================================================//
  /*UART receive complete interrupt*/
  ISR(USART_RX_vect)                                              //Data taken from the UART buffer
  {
      uint8_t sample;                                             
  
      sample = UDR0;                                              //Read a sample from the receiver buffer
  
      if(frameByteCount == 3)                                     //If its the 4th byte of the frame
      {
          frameByteCount = 0;                                     //Reset the counter
          distanceH = sample;                                     //Read the distance data high byte
          Distance = (8 &lt;&lt; distanceH);                            //Combine the data low and data high
          Distance |= distanceL;
  
          calcSpeed(Distance);                                //Send the data to laptop for debugging purposes
  
      }
  
      if(frameByteCount == 2)                                     //If its the 3rd byte in the frame read the distance data low byte
      {
          distanceL = sample;
          frameByteCount++;                                       //Increment the counter
      }
  
      if (sample == 0x59)                                         //If a sample is a header increment a counter
      {
          frameByteCount++;
      }
      else if (frameByteCount != 2 &amp;&amp; frameByteCount != 3)        //If its not a header or distance data byte reset counter
      {
          frameByteCount = 0;
      }   
  }
  //============================================================================================//
  
  //Timers and Counters
  //============================================================================================//
  /*Timer0 Counter, this decreases the danger level periodically so a really slow approach will not keep incrementing the danger level*/
  ISR(TIMER0_OVF_vect)                
  {
  //  if (DangerLevel &gt; 0)
  //  {
  //      DangerLevel--;
  //  }
  }
  //============================================================================================//
  
  //Main
  //============================================================================================//
  int main(void)
  {
      DDRB    |=      0x02;                                       //For debugging purposes LED pin PB1 (pin 9) as output
  
      UBRR0H = (UBRR_VALUE &gt;&gt; 8);                                 //Setting the Baudrate register high value
      UBRR0L =  UBRR_VALUE;                                       //Setting the Baudrate register low value
      UCSR0A |= (1&lt;&lt;U2X0);                                        //Setting the data sample to double speed to make Baudrate error less
      UCSR0C |= (1&lt;&lt;UCSZ01) | (1&lt;&lt;UCSZ00);                        //Initializing UART, setting frame to 8 data bits
      UCSR0B |= (1&lt;&lt;RXCIE0) | (1&lt;&lt;TXEN0) | (1&lt;&lt;RXEN0);            //Setting receiver interrupt enable, transmitter enable and receiver enable... Transmitter for debugging purposes
  
      TCCR0A = 0x00;                                              //Timer0 Setup
      TCCR0B |= 0x05;                                             //Timer0 clock prescaler to 1024
      TIMSK0 |= 0x01;                                             //Timer0 overflow interrupt enable
  
      sei();                                                      //Set interrupts
  
      State = Safe;                                               //Set state to safe
  
      while (1)
      {
          //checkState(DangerLevel);
      }
  }
  //============================================================================================//
  
  //Functions
  //============================================================================================//
  /*Calculate the danger level out of distance readings*/
  void calcSpeed(uint16_t Measurement)
  {
      static uint8_t samplenumber = 0;                                //Sample tracker
      static uint16_t value0 = 0;                                         //First sample
      static uint16_t value1 = 0;                                         //Second sample
  
  
      switch(samplenumber)                                            //To store the measurements alternately
      {
          case 0:
              value0 = Measurement;                                   //Store first measurement
              samplenumber = 1;                                       //So next measurement goes to a different variable
          break;
          case 1:
              value1 = Measurement;                                   //Store 2nd measurement
              samplenumber = 0;                                       //So next measurement goes to a different variable
          break;
          default:
          break;
      }
  
      if (samplenumber == 0 &amp;&amp; value1 &lt; value0)                       //When value0 is first and value1 is second and the object is closing in
      {
          Speed = value0 - value1;
      }
      else if (samplenumber == 1 &amp;&amp; value0 &lt; value1)                  //When value1 is first and value0 is second and the object is closing in
      {
          Speed = value1 - value0;
      }
      else
      {
          Speed = 0;
      }
      UART_Transmit(Speed);       //I think sending an uint16_t over uint8_t uart is possible because 'Speed' is never greater than 255 when i'm testing it
  }
  
  /*Send data over UART when buffer is empty*/
  void UART_Transmit(uint8_t data)
  {
      /* Wait for empty transmit buffer */
      while ( !( UCSR0A &amp; (1&lt;&lt;UDRE0)) )
      ;
      /* Put data into buffer, sends the data */
      UDR0 = data;
  }
  //============================================================================================//
  </code></pre>