From 3029c607e3b879a306fb704d5c9d0677accacd2d Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 6 Nov 2023 14:06:08 -0500 Subject: [PATCH] etherbotix: add support for ld06 laser on usart3/tim12 (#15) --- .../libcpp/inc}/ld06.hpp | 6 +-- projects/etherbotix/README.md | 2 +- projects/etherbotix/main.cpp | 19 ++++++-- projects/etherbotix/user_io.hpp | 48 +++++++++++++++++-- projects/tablebot/main.cpp | 2 +- 5 files changed, 66 insertions(+), 11 deletions(-) rename {projects/tablebot => libraries/libcpp/inc}/ld06.hpp (98%) diff --git a/projects/tablebot/ld06.hpp b/libraries/libcpp/inc/ld06.hpp similarity index 98% rename from projects/tablebot/ld06.hpp rename to libraries/libcpp/inc/ld06.hpp index 44c60de..34b5c00 100644 --- a/projects/tablebot/ld06.hpp +++ b/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_ diff --git a/projects/etherbotix/README.md b/projects/etherbotix/README.md index 97abcf4..c09da62 100644 --- a/projects/etherbotix/README.md +++ b/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 | | | | diff --git a/projects/etherbotix/main.cpp b/projects/etherbotix/main.cpp index 1854098..e4d95ba 100644 --- a/projects/etherbotix/main.cpp +++ b/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) { diff --git a/projects/etherbotix/user_io.hpp b/projects/etherbotix/user_io.hpp index f9b87dc..81ae8d1 100644 --- a/projects/etherbotix/user_io.hpp +++ b/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 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 */ diff --git a/projects/tablebot/main.cpp b/projects/tablebot/main.cpp index 2ff5908..06f9f93 100644 --- a/projects/tablebot/main.cpp +++ b/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);