From f7a4736644b7f21ae5bc128fcf40813818f0fadf Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 14 Sep 2020 18:33:20 +0800 Subject: [PATCH 1/7] fixed can communication issue in mobile_base.cpp, tested with robot --- src/platforms/mobile_base.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 100b4b9..639b970 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -34,6 +34,7 @@ void MobileBase::Connect(std::string dev_name, int32_t baud_rate) { } void MobileBase::Disconnect() { + if (can_connected_) can_if_->StopService(); if (serial_connected_ && serial_if_->IsOpened()) { serial_if_->StopService(); } @@ -43,18 +44,18 @@ void MobileBase::ConfigureCAN(const std::string &can_if_name) { can_if_ = std::make_shared(can_if_name); can_if_->SetReceiveCallback( std::bind(&MobileBase::ParseCANFrame, this, std::placeholders::_1)); - serial_if_->StartListening(); + can_if_->StartListening(); can_connected_ = true; } void MobileBase::ConfigureSerial(const std::string uart_name, int32_t baud_rate) { serial_if_ = std::make_shared(uart_name, baud_rate); - if (serial_if_->IsOpened()) serial_connected_ = true; serial_if_->SetReceiveCallback( std::bind(&MobileBase::ParseUARTBuffer, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); serial_if_->StartListening(); + if (serial_if_->IsOpened()) serial_connected_ = true; } void MobileBase::StartCmdThread() { @@ -66,6 +67,7 @@ void MobileBase::StartCmdThread() { void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; bool print_loop_freq = false; + static uint32_t iter_cnt = 0; while (true) { ctrl_sw.tic(); SendRobotCmd(); From 8ba694b8945653281889c1c349aca2f64aeb2f76 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 14 Sep 2020 18:45:08 +0800 Subject: [PATCH 2/7] saved work on cmd timeout, not finished yet --- include/wrp_sdk/platforms/common/mobile_base.hpp | 8 ++++++++ src/platforms/mobile_base.cpp | 1 - 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index 01f48bb..6e12a6a 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -31,6 +31,8 @@ class MobileBase { MobileBase(const MobileBase &hunter) = delete; MobileBase &operator=(const MobileBase &hunter) = delete; + void SetCmdTimeout(bool enable, uint32_t timeout_ms = 100); + // connect to roboot from CAN or serial void Connect(std::string dev_name, int32_t baud_rate = 0); @@ -50,6 +52,12 @@ class MobileBase { std::shared_ptr can_if_; std::shared_ptr serial_if_; + // timeout to be implemented in each vehicle + bool enable_timeout_ = true; + uint32_t timeout_ms_ = 100; + uint32_t watchdog_counter_ = 0; + virtual void CheckCmdTimeout(); + // command thread std::thread cmd_thread_; int32_t cmd_thread_period_ms_ = 10; diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 639b970..4088020 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -67,7 +67,6 @@ void MobileBase::StartCmdThread() { void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; bool print_loop_freq = false; - static uint32_t iter_cnt = 0; while (true) { ctrl_sw.tic(); SendRobotCmd(); From 9e646c78b7b0ad0553b30e05f2761eba61895396 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 14 Sep 2020 18:57:27 +0800 Subject: [PATCH 3/7] saved work on cmd timeout, not tested yet --- include/wrp_sdk/platforms/common/mobile_base.hpp | 2 +- src/asyncio/async_can.cpp | 2 +- src/platforms/hunter_base.cpp | 1 + src/platforms/mobile_base.cpp | 5 ++++- src/platforms/scout_base.cpp | 1 + 5 files changed, 8 insertions(+), 3 deletions(-) diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index 6e12a6a..b50d4d5 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -56,7 +56,7 @@ class MobileBase { bool enable_timeout_ = true; uint32_t timeout_ms_ = 100; uint32_t watchdog_counter_ = 0; - virtual void CheckCmdTimeout(); + void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; // command thread std::thread cmd_thread_; diff --git a/src/asyncio/async_can.cpp b/src/asyncio/async_can.cpp index ba99ba2..8a9abef 100644 --- a/src/asyncio/async_can.cpp +++ b/src/asyncio/async_can.cpp @@ -107,7 +107,7 @@ void AsyncCAN::SendFrame(const can_frame &frame) { asio::buffer(&frame, sizeof(frame)), [](asio::error_code error, size_t bytes_transferred) { if (error) { - std::cerr << "Failed to send CAN frame"; + std::cerr << "Failed to send CAN frame" << std::endl; } // std::cout << "frame sent" << std::endl; }); diff --git a/src/platforms/hunter_base.cpp b/src/platforms/hunter_base.cpp index a23503b..17fcc52 100644 --- a/src/platforms/hunter_base.cpp +++ b/src/platforms/hunter_base.cpp @@ -17,6 +17,7 @@ namespace westonrobot { void HunterBase::SendRobotCmd() { static uint8_t cmd_count = 0; SendMotionCmd(cmd_count++); + FeedCmdTimeoutWatchdog(); } void HunterBase::SendMotionCmd(uint8_t count) { diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 4088020..bb75e3e 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -67,9 +67,12 @@ void MobileBase::StartCmdThread() { void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; bool print_loop_freq = false; + if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; + uint32_t timeout_iter_num = static_cast(timeout_ms_ / period_ms); while (true) { ctrl_sw.tic(); - SendRobotCmd(); + if (watchdog_counter_ < timeout_iter_num) SendRobotCmd(); + ++watchdog_counter_; ctrl_sw.sleep_until_ms(period_ms); if (print_loop_freq) std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc() diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index 6e78c99..0d32e41 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -17,6 +17,7 @@ void ScoutBase::SendRobotCmd() { static uint8_t cmd_count = 0; static uint8_t light_cmd_count = 0; SendMotionCmd(cmd_count++); + FeedCmdTimeoutWatchdog(); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); } From c501b6a38d3363627d87aeff69afcb25d0495b7c Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Mon, 14 Sep 2020 21:37:08 +0800 Subject: [PATCH 4/7] reverted back changes to function signature of serial rx callback func --- apps/scout_monitor/src/ncolors.cpp | 2 ++ include/wrp_sdk/asyncio/async_serial.hpp | 4 ++-- src/asyncio/async_serial.cpp | 14 +++++++++----- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/apps/scout_monitor/src/ncolors.cpp b/apps/scout_monitor/src/ncolors.cpp index a3ea543..2d76168 100644 --- a/apps/scout_monitor/src/ncolors.cpp +++ b/apps/scout_monitor/src/ncolors.cpp @@ -54,6 +54,8 @@ short CursorColor(int fg) return (COLOR_YELLOW); case 7: /* 111 */ return (COLOR_WHITE); + default: + return COLOR_BLACK; } } } // namespace diff --git a/include/wrp_sdk/asyncio/async_serial.hpp b/include/wrp_sdk/asyncio/async_serial.hpp index 4c7e473..3dea233 100644 --- a/include/wrp_sdk/asyncio/async_serial.hpp +++ b/include/wrp_sdk/asyncio/async_serial.hpp @@ -23,7 +23,7 @@ class AsyncSerial : public AsyncListener, public std::enable_shared_from_this { public: using ReceiveCallback = - std::function; + std::function; public: AsyncSerial(std::string port_name, uint32_t baud_rate = 115200); @@ -56,7 +56,7 @@ class AsyncSerial : public AsyncListener, bool tx_in_progress_ = false; bool SetupPort(); - void DefaultReceiveCallback(uint8_t *data, size_t len, const size_t bufsize); + void DefaultReceiveCallback(uint8_t *data, const size_t bufsize, size_t len); void ReadFromPort(); void WriteToPort(bool check_if_busy); }; diff --git a/src/asyncio/async_serial.cpp b/src/asyncio/async_serial.cpp index 21104b6..1775f03 100644 --- a/src/asyncio/async_serial.cpp +++ b/src/asyncio/async_serial.cpp @@ -80,8 +80,8 @@ void AsyncSerial::StopService() { port_opened_ = false; } -void AsyncSerial::DefaultReceiveCallback(uint8_t *data, size_t len, - const size_t bufsize) {} +void AsyncSerial::DefaultReceiveCallback(uint8_t *data, const size_t bufsize, + size_t len) {} void AsyncSerial::ReadFromPort() { auto sthis = shared_from_this(); @@ -93,9 +93,13 @@ void AsyncSerial::ReadFromPort() { return; } - if (sthis->rcv_cb_ != nullptr) - sthis->rcv_cb_(sthis->rx_buf_.data(), bytes_transferred, - sthis->rx_buf_.size()); + if (sthis->rcv_cb_ != nullptr) { + sthis->rcv_cb_(sthis->rx_buf_.data(), sthis->rx_buf_.size(), + bytes_transferred); + } else { + sthis->DefaultReceiveCallback( + sthis->rx_buf_.data(), sthis->rx_buf_.size(), bytes_transferred); + } sthis->ReadFromPort(); }); } From 0887261a990b07921798fbaf819a7e22645e6e80 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 15 Sep 2020 09:56:26 +0800 Subject: [PATCH 5/7] added cmd timeout logic --- src/platforms/mobile_base.cpp | 20 ++++++++++++++++---- src/platforms/scout_base.cpp | 4 +++- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index bb75e3e..3bbf875 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -67,12 +67,24 @@ void MobileBase::StartCmdThread() { void MobileBase::ControlLoop(int32_t period_ms) { StopWatch ctrl_sw; bool print_loop_freq = false; - if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; - uint32_t timeout_iter_num = static_cast(timeout_ms_ / period_ms); + uint32_t timeout_iter_num; + + if (enable_timeout_) { + if (timeout_ms_ < period_ms) timeout_ms_ = period_ms; + timeout_iter_num = static_cast(timeout_ms_ / period_ms); + std::cout << "Timeout iteration number: " << timeout_iter_num << std::endl; + } while (true) { ctrl_sw.tic(); - if (watchdog_counter_ < timeout_iter_num) SendRobotCmd(); - ++watchdog_counter_; + if (enable_timeout_) { + ++watchdog_counter_; + if (watchdog_counter_ < timeout_iter_num) + SendRobotCmd(); + else + std::cout << "Warning: cmd timeout, not sent to robot" << std::endl; + } else { + SendRobotCmd(); + } ctrl_sw.sleep_until_ms(period_ms); if (print_loop_freq) std::cout << "control loop frequency: " << 1.0 / ctrl_sw.toc() diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index 0d32e41..52a8ab9 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -17,8 +17,8 @@ void ScoutBase::SendRobotCmd() { static uint8_t cmd_count = 0; static uint8_t light_cmd_count = 0; SendMotionCmd(cmd_count++); - FeedCmdTimeoutWatchdog(); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); + FeedCmdTimeoutWatchdog(); } void ScoutBase::SendMotionCmd(uint8_t count) { @@ -56,10 +56,12 @@ void ScoutBase::SendMotionCmd(uint8_t count) { can_frame m_frame; EncodeScoutMsgToCAN(&m_msg, &m_frame); can_if_->SendFrame(m_frame); + std::cout << "CAN cmd sent" << std::endl; } else { // send to serial port EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); + std::cout << "serial cmd sent" << std::endl; } } From a872dd0e0e32d4f257f62823a8a5461113277106 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 15 Sep 2020 11:00:16 +0800 Subject: [PATCH 6/7] tested timeout --- apps/scout_demo/scout_demo.cpp | 208 +++++++++--------- .../wrp_sdk/platforms/common/mobile_base.hpp | 2 +- src/platforms/hunter_base.cpp | 3 +- src/platforms/mobile_base.cpp | 8 +- src/platforms/scout_base.cpp | 7 +- 5 files changed, 119 insertions(+), 109 deletions(-) diff --git a/apps/scout_demo/scout_demo.cpp b/apps/scout_demo/scout_demo.cpp index aa90d4f..2c96f8c 100644 --- a/apps/scout_demo/scout_demo.cpp +++ b/apps/scout_demo/scout_demo.cpp @@ -1,9 +1,9 @@ -/* +/* * demo_scout_can.cpp - * + * * Created on: Jun 12, 2019 05:03 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) */ @@ -11,106 +11,112 @@ using namespace westonrobot; -int main(int argc, char **argv) -{ - std::string device_name; - int32_t baud_rate = 0; +int main(int argc, char **argv) { + std::string device_name; + int32_t baud_rate = 0; - if (argc == 2) - { - device_name = {argv[1]}; - std::cout << "Specified CAN: " << device_name << std::endl; - } - else if (argc == 3) - { - device_name = {argv[1]}; - baud_rate = std::stol(argv[2]); - std::cout << "Specified serial: " << device_name << "@" << baud_rate << std::endl; - } - else - { - std::cout << "Usage: app_scout_demo " << std::endl - << "Example 1: ./app_scout_demo can0" << std::endl - << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl; - return -1; + if (argc == 2) { + device_name = {argv[1]}; + std::cout << "Specified CAN: " << device_name << std::endl; + } else if (argc == 3) { + device_name = {argv[1]}; + baud_rate = std::stol(argv[2]); + std::cout << "Specified serial: " << device_name << "@" << baud_rate + << std::endl; + } else { + std::cout << "Usage: app_scout_demo " << std::endl + << "Example 1: ./app_scout_demo can0" << std::endl + << "Example 2: ./app_scout_demo /dev/ttyUSB0 115200" << std::endl; + return -1; + } + + ScoutBase scout; + scout.Connect(device_name, baud_rate); + +// // light control +// std::cout << "Light: const off" << std::endl; +// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, +// ScoutLightCmd::LightMode::CONST_OFF, 0}); +// sleep(3); +// std::cout << "Light: const on" << std::endl; +// scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, +// ScoutLightCmd::LightMode::CONST_ON, 0}); +// sleep(3); +// std::cout << "Light: breath" << std::endl; +// scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, +// ScoutLightCmd::LightMode::BREATH, 0}); +// sleep(3); +// std::cout << "Light: custom 90-80" << std::endl; +// scout.SetLightCommand({ScoutLightCmd::LightMode::CUSTOM, 90, +// ScoutLightCmd::LightMode::CUSTOM, 80}); +// sleep(3); +// std::cout << "Light: diabled cmd control" << std::endl; +// scout.DisableLightCmdControl(); + + int count = 0; + while (true) { + // motion control + // if (count < 5) + // { + // std::cout << "Motor: 0.2, 0" << std::endl; + // scout.SetMotionCommand(0.2, 0.0); + // } + // else if (count < 10) + // { + // std::cout << "Motor: 0.8, 0" << std::endl; + // scout.SetMotionCommand(0.8, 0.0); + // } + // else if (count < 15) + // { + // std::cout << "Motor: 1.5, 0" << std::endl; + // scout.SetMotionCommand(1.5, 0.0); + // } + // else if (count < 20) + // { + // std::cout << "Motor: 1.0, 0.5" << std::endl; + // scout.SetMotionCommand(1.0, 0.5); + // } + // else if (count < 25) + // { + // std::cout << "Motor: 0.0, 0" << std::endl; + // scout.SetMotionCommand(0.0, 0.0); + // } + // else if (count < 30) + // { + // std::cout << "Motor: -0.5, 0" << std::endl; + // scout.SetMotionCommand(-0.5, 0.0); + // } + // else if (count < 35) + // { + // std::cout << "Motor: -1.0, -0.5" << std::endl; + // scout.SetMotionCommand(-1.0, -0.5); + // } + // else if (count < 40) + // { + // std::cout << "Motor: 0.0, 0, Light: breath" << std::endl; + // scout.SetMotionCommand(0.0, 0.0); + // scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, + // ScoutLightCmd::LightMode::BREATH, 0}); + // } + if (count < 250) { + std::cout << "Motor: 0.2, 0" << std::endl; + scout.SetMotionCommand(0.2, 0.0); } - ScoutBase scout; - scout.Connect(device_name, baud_rate); + auto state = scout.GetScoutState(); + std::cout << "-------------------------------" << std::endl; + std::cout << "count: " << count << std::endl; + std::cout << "control mode: " << static_cast(state.control_mode) + << " , base state: " << static_cast(state.base_state) + << std::endl; + std::cout << "battery voltage: " << state.battery_voltage << std::endl; + std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " + << state.angular_velocity << std::endl; + std::cout << "-------------------------------" << std::endl; - // light control - std::cout << "Light: const off" << std::endl; - scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_OFF, 0, ScoutLightCmd::LightMode::CONST_OFF, 0}); - sleep(3); - std::cout << "Light: const on" << std::endl; - scout.SetLightCommand({ScoutLightCmd::LightMode::CONST_ON, 0, ScoutLightCmd::LightMode::CONST_ON, 0}); - sleep(3); - std::cout << "Light: breath" << std::endl; - scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, ScoutLightCmd::LightMode::BREATH, 0}); - sleep(3); - std::cout << "Light: custom 90-80" << std::endl; - scout.SetLightCommand({ScoutLightCmd::LightMode::CUSTOM, 90, ScoutLightCmd::LightMode::CUSTOM, 80}); - sleep(3); - std::cout << "Light: diabled cmd control" << std::endl; - scout.DisableLightCmdControl(); + usleep(20000); + ++count; + } - int count = 0; - while (true) - { - // motion control - if (count < 5) - { - std::cout << "Motor: 0.2, 0" << std::endl; - scout.SetMotionCommand(0.2, 0.0); - } - else if (count < 10) - { - std::cout << "Motor: 0.8, 0" << std::endl; - scout.SetMotionCommand(0.8, 0.0); - } - else if (count < 15) - { - std::cout << "Motor: 1.5, 0" << std::endl; - scout.SetMotionCommand(1.5, 0.0); - } - else if (count < 20) - { - std::cout << "Motor: 1.0, 0.5" << std::endl; - scout.SetMotionCommand(1.0, 0.5); - } - else if (count < 25) - { - std::cout << "Motor: 0.0, 0" << std::endl; - scout.SetMotionCommand(0.0, 0.0); - } - else if (count < 30) - { - std::cout << "Motor: -0.5, 0" << std::endl; - scout.SetMotionCommand(-0.5, 0.0); - } - else if (count < 35) - { - std::cout << "Motor: -1.0, -0.5" << std::endl; - scout.SetMotionCommand(-1.0, -0.5); - } - else if (count < 40) - { - std::cout << "Motor: 0.0, 0, Light: breath" << std::endl; - scout.SetMotionCommand(0.0, 0.0); - scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, ScoutLightCmd::LightMode::BREATH, 0}); - } - - auto state = scout.GetScoutState(); - std::cout << "-------------------------------" << std::endl; - std::cout << "count: " << count << std::endl; - std::cout << "control mode: " << static_cast(state.control_mode) << " , base state: " << static_cast(state.base_state) << std::endl; - std::cout << "battery voltage: " << state.battery_voltage << std::endl; - std::cout << "velocity (linear, angular): " << state.linear_velocity << ", " << state.angular_velocity << std::endl; - std::cout << "-------------------------------" << std::endl; - - sleep(1); - ++count; - } - - return 0; + return 0; } \ No newline at end of file diff --git a/include/wrp_sdk/platforms/common/mobile_base.hpp b/include/wrp_sdk/platforms/common/mobile_base.hpp index b50d4d5..9f0314f 100644 --- a/include/wrp_sdk/platforms/common/mobile_base.hpp +++ b/include/wrp_sdk/platforms/common/mobile_base.hpp @@ -54,7 +54,7 @@ class MobileBase { // timeout to be implemented in each vehicle bool enable_timeout_ = true; - uint32_t timeout_ms_ = 100; + uint32_t timeout_ms_ = 500; uint32_t watchdog_counter_ = 0; void FeedCmdTimeoutWatchdog() { watchdog_counter_ = 0; }; diff --git a/src/platforms/hunter_base.cpp b/src/platforms/hunter_base.cpp index 17fcc52..3c8a9d2 100644 --- a/src/platforms/hunter_base.cpp +++ b/src/platforms/hunter_base.cpp @@ -17,7 +17,6 @@ namespace westonrobot { void HunterBase::SendRobotCmd() { static uint8_t cmd_count = 0; SendMotionCmd(cmd_count++); - FeedCmdTimeoutWatchdog(); } void HunterBase::SendMotionCmd(uint8_t count) { @@ -81,6 +80,8 @@ void HunterBase::SetMotionCommand( current_motion_cmd_.angular_velocity = static_cast( steering_angle / HunterMotionCmd::max_steering_angle * 100.0); current_motion_cmd_.fault_clear_flag = fault_clr_flag; + + FeedCmdTimeoutWatchdog(); } void HunterBase::ParseCANFrame(can_frame *rx_frame) { diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index 3bbf875..c484b87 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -74,14 +74,16 @@ void MobileBase::ControlLoop(int32_t period_ms) { timeout_iter_num = static_cast(timeout_ms_ / period_ms); std::cout << "Timeout iteration number: " << timeout_iter_num << std::endl; } + while (true) { ctrl_sw.tic(); if (enable_timeout_) { - ++watchdog_counter_; - if (watchdog_counter_ < timeout_iter_num) + if (watchdog_counter_ < timeout_iter_num) { SendRobotCmd(); - else + ++watchdog_counter_; + } else { std::cout << "Warning: cmd timeout, not sent to robot" << std::endl; + } } else { SendRobotCmd(); } diff --git a/src/platforms/scout_base.cpp b/src/platforms/scout_base.cpp index 52a8ab9..ce0d318 100644 --- a/src/platforms/scout_base.cpp +++ b/src/platforms/scout_base.cpp @@ -18,7 +18,6 @@ void ScoutBase::SendRobotCmd() { static uint8_t light_cmd_count = 0; SendMotionCmd(cmd_count++); if (light_ctrl_requested_) SendLightCmd(light_cmd_count++); - FeedCmdTimeoutWatchdog(); } void ScoutBase::SendMotionCmd(uint8_t count) { @@ -56,12 +55,12 @@ void ScoutBase::SendMotionCmd(uint8_t count) { can_frame m_frame; EncodeScoutMsgToCAN(&m_msg, &m_frame); can_if_->SendFrame(m_frame); - std::cout << "CAN cmd sent" << std::endl; + // std::cout << "CAN cmd sent" << std::endl; } else { // send to serial port EncodeScoutMsgToUART(&m_msg, tx_buffer_, &tx_cmd_len_); serial_if_->SendBytes(tx_buffer_, tx_cmd_len_); - std::cout << "serial cmd sent" << std::endl; + // std::cout << "serial cmd sent" << std::endl; } } @@ -161,6 +160,8 @@ void ScoutBase::SetMotionCommand( current_motion_cmd_.angular_velocity = static_cast( angular_vel / ScoutMotionCmd::max_angular_velocity * 100.0); current_motion_cmd_.fault_clear_flag = fault_clr_flag; + + FeedCmdTimeoutWatchdog(); } void ScoutBase::SetLightCommand(ScoutLightCmd cmd) { From 02d5a9ed542817350b7ad29ad79163245427e6f0 Mon Sep 17 00:00:00 2001 From: Ruixiang Du Date: Tue, 15 Sep 2020 11:03:49 +0800 Subject: [PATCH 7/7] saved work on timeout --- apps/scout_demo/scout_demo.cpp | 2 +- src/platforms/mobile_base.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/scout_demo/scout_demo.cpp b/apps/scout_demo/scout_demo.cpp index 2c96f8c..38a1c5f 100644 --- a/apps/scout_demo/scout_demo.cpp +++ b/apps/scout_demo/scout_demo.cpp @@ -98,7 +98,7 @@ int main(int argc, char **argv) { // scout.SetLightCommand({ScoutLightCmd::LightMode::BREATH, 0, // ScoutLightCmd::LightMode::BREATH, 0}); // } - if (count < 250) { + if (count < 100) { std::cout << "Motor: 0.2, 0" << std::endl; scout.SetMotionCommand(0.2, 0.0); } diff --git a/src/platforms/mobile_base.cpp b/src/platforms/mobile_base.cpp index c484b87..9cca69e 100644 --- a/src/platforms/mobile_base.cpp +++ b/src/platforms/mobile_base.cpp @@ -74,7 +74,7 @@ void MobileBase::ControlLoop(int32_t period_ms) { timeout_iter_num = static_cast(timeout_ms_ / period_ms); std::cout << "Timeout iteration number: " << timeout_iter_num << std::endl; } - + while (true) { ctrl_sw.tic(); if (enable_timeout_) { @@ -82,7 +82,7 @@ void MobileBase::ControlLoop(int32_t period_ms) { SendRobotCmd(); ++watchdog_counter_; } else { - std::cout << "Warning: cmd timeout, not sent to robot" << std::endl; + std::cout << "Warning: cmd timeout, old cmd not sent to robot" << std::endl; } } else { SendRobotCmd();