etherbotix: add support for ld06 laser on usart3/tim12 (#15)

projects/tablebot/ld06.hpp → libraries/libcpp/inc/ld06.hpp

@@ -27,8 +27,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef _TABLEBOT_LD06_HPP_
-#define _TABLEBOT_LD06_HPP_
+#ifndef _LD06_HPP_
+#define _LD06_HPP_
 
 /*
  * NOTE: Each packet from laser is max 107 bytes per datasheet - but ROS
@@ -264,4 +264,4 @@ class LD06
   uint16_t control_pwm_;
 };
 
-#endif  // _TABLEBOT_LD06_HPP_
+#endif  // _LD06_HPP_

projects/etherbotix/README.md

@@ -20,7 +20,7 @@ The Etherbotix has 2 leds:
 
 ## User IO
 
-| Pin     | Analog In   | Digital In | Digital Out | SPI    | USART   | TIM9  | TIM12  |
+| Pin     | Analog In   | Digital In | Digital Out | SPI    | USART3  | TIM9  | TIM12  |
 |---------|-------------|------------|-------------|--------|---------|-------|--------|
 | A0      | Yes, 12-bit |            | Yes         |        |         |       |        |
 | A1      | Yes, 12-bit |            | Yes         | MOSI   |         |       |        |

projects/etherbotix/main.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2020, Michael E. Ferguson
+ * Copyright (c) 2012-2023, Michael E. Ferguson
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -356,6 +356,7 @@ void udp_callback(void *arg, struct udp_pcb *udp, struct pbuf *p,
               // Set baud, start usart
               registers.usart3_baud = data[i + 6 + j];
               user_io_usart3_init();
+              usart3_port = port;
             }
             else if (write_addr + j == REG_USART3_CHAR)
             {
@@ -587,7 +588,7 @@ int main(void)
 
   // Setup register table data
   registers.model_number = 301;  // Arbotix was 300
-  registers.version = 5;
+  registers.version = 6;
   registers.id = 253;
   registers.baud_rate = 1;  // 1mbps
   registers.digital_dir = 0;  // all in
@@ -711,7 +712,19 @@ int main(void)
     }
     registers.imu_flags = imu.get_flags();
 
-    if (user_io_usart3_active_ && registers.usart3_char != 255)
+    if (user_io_laser_active_)
+    {
+      int8_t length = laser.update(&usart3, registers.system_time);
+      if (length > 0)
+      {
+        udp_send_packet((unsigned char *) &laser.packet, sizeof(laser.packet), usart3_port);
+      }
+      else if (length < 0)
+      {
+        laser.reset(&usart3);
+      }
+    }
+    else if (user_io_usart3_active_ && registers.usart3_char != 255)
     {
       while (1)
       {

projects/etherbotix/user_io.hpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2018, Michael E. Ferguson
+ * Copyright (c) 2014-2023, Michael E. Ferguson
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -30,13 +30,19 @@
 #ifndef _ETHERBOTIX_USER_IO_HPP_
 #define _ETHERBOTIX_USER_IO_HPP_
 
+#include "ld06.hpp"
+
 // user_io stuff is only called from the main while() loop, so
 // we don't have to worry about the register table changing.
 
 usart3_t usart3;
 
+// Laser uses both USART3 and TIM12
+LD06<usart3_t> laser;
+
 // State of user IO
 uint8_t user_io_usart3_active_;
+uint8_t user_io_laser_active_;
 uint8_t user_io_spi2_active_;
 uint8_t user_io_tim9_active_;
 uint8_t user_io_tim12_active_;
@@ -49,6 +55,7 @@ inline void user_io_init()
 {
   // Disable everything
   user_io_usart3_active_ = 0;
+  user_io_laser_active_ = 0;
   user_io_spi2_active_ = 0;
   user_io_tim9_active_ = 0;
   user_io_tim12_active_ = 0;
@@ -225,6 +232,28 @@ inline void user_io_set_direction()
   }
 }
 
+/******************************************************************************
+ * LD06 Lidar
+ */
+
+inline bool user_io_ld06_init()
+{
+  user_io_laser_active_ = 1;
+
+  // Uses USART3 and TIM12
+  RCC->APB1ENR |= RCC_APB1ENR_USART3EN | RCC_APB1ENR_TIM12EN;
+
+  // Only using USART3 RX
+  d4::mode(GPIO_ALTERNATE | GPIO_AF_USART3);
+  user_io_pin_status_ |= (1 << 4);
+
+  d5::mode(GPIO_ALTERNATE | GPIO_AF_TIM12);
+  user_io_pin_status_ |= (1<<5);
+
+  laser.init(&usart3);
+  return true;
+}
+
 /******************************************************************************
  * USER USART
  */
@@ -252,11 +281,19 @@ inline bool user_io_usart3_init()
     baud = 2500000;
   else if (registers.usart3_baud == 252)
     baud = 3000000;
+  else if (registers.usart3_baud == 255)
+  {
+    // Enable LD-06 laser
+    user_io_usart3_active_ = 0;
+    user_io_tim12_active_ = 0;
+    return user_io_ld06_init();
+  }
   else
   {
     // Unsupported baud, turn off usart3
     registers.usart3_baud = 0;
     user_io_usart3_active_ = 0;
+    user_io_laser_active_ = 0;
 
     d3::mode(GPIO_INPUT);
     user_io_pin_status_ &= ~(1 << 3);
@@ -275,6 +312,7 @@ inline bool user_io_usart3_init()
   RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
   usart3.init(baud, 8);
   user_io_usart3_active_ = 1;
+  user_io_laser_active_ = 0;
   return true;
 }
 
@@ -313,7 +351,12 @@ inline uint8_t user_io_usart3_read(uint8_t * data, uint8_t max_len)
 
 inline bool user_io_tim12_init()
 {
-  if (registers.tim12_mode == 1)  // Count on external clock
+  if (user_io_laser_active_)
+  {
+    // Cannot enable tim12 if laser is active
+    registers.tim12_mode = 0;
+  }
+  else if (registers.tim12_mode == 1)  // Count on external clock
   {
     // Turn on clock
     RCC->APB1ENR |= RCC_APB1ENR_TIM12EN;
@@ -346,7 +389,6 @@ inline uint16_t user_io_tim12_get_count()
     return 0;
 }
 
-
 /******************************************************************************
  * Feedback
  */

projects/tablebot/main.cpp

@@ -253,7 +253,7 @@ int main(void)
   center_cliff::mode(GPIO_INPUT_ANALOG);
   right_cliff::mode(GPIO_INPUT_ANALOG);
 
-  // Laser interface - temporarily disabled
+  // Laser interface
   RCC->APB1ENR |= RCC_APB1ENR_USART3EN | RCC_APB1ENR_TIM12EN;
   laser_rx::mode(GPIO_ALTERNATE | GPIO_AF_USART3);
   laser_pwm::mode(GPIO_ALTERNATE | GPIO_AF_TIM12);