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) {