diff --git a/ugv_sdk/apps/tracer_demo/tracer_demo.cpp b/ugv_sdk/apps/tracer_demo/tracer_demo.cpp index 7fc8cd6..732f3c8 100644 --- a/ugv_sdk/apps/tracer_demo/tracer_demo.cpp +++ b/ugv_sdk/apps/tracer_demo/tracer_demo.cpp @@ -28,55 +28,61 @@ int main(int argc, char **argv) { tracer.Connect(device_name); tracer.EnableCommandedMode(); + // tracer.DisableTimeout(); // light control std::cout << "Light: const off" << std::endl; tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_OFF, 0, TracerLightCmd::LightMode::CONST_OFF, 0}); - sleep(5); + // usleep(50000); + sleep(3); std::cout << "Light: const on" << std::endl; tracer.SetLightCommand({TracerLightCmd::LightMode::CONST_ON, 0, TracerLightCmd::LightMode::CONST_ON, 0}); + // usleep(50000); sleep(3); std::cout << "Light: breath" << std::endl; tracer.SetLightCommand({TracerLightCmd::LightMode::BREATH, 0, TracerLightCmd::LightMode::BREATH, 0}); - sleep(3); + // usleep(50000); + sleep(8); std::cout << "Light: custom 90-80" << std::endl; tracer.SetLightCommand({TracerLightCmd::LightMode::CUSTOM, 90, TracerLightCmd::LightMode::CUSTOM, 80}); + // usleep(50000); sleep(3); std::cout << "Light: diabled cmd control" << std::endl; - tracer.DisableLightCmdControl(); + tracer.SetLightCommand(TracerLightCmd()); int count = 0; while (true) { - // // motion control - // if (count < 5) { - // std::cout << "Motor: 0.2, 0.0" << std::endl; - // tracer.SetMotionCommand(0.2, 0.0); - // } else if (count < 10) { - // std::cout << "Motor: 0.8, 0.3" << std::endl; - // tracer.SetMotionCommand(0.8, 0.3); - // } else if (count < 15) { - // std::cout << "Motor: 1.5, 0.5" << std::endl; - // tracer.SetMotionCommand(1.5, 0.5); - // } else if (count < 20) { - // std::cout << "Motor: 1.0, 0.3" << std::endl; - // tracer.SetMotionCommand(1.0, 0.3); - // } else if (count < 25) { - // std::cout << "Motor: 0.0, 0.0" << std::endl; - // tracer.SetMotionCommand(0.0, 0.0); - // } else if (count < 30) { - // std::cout << "Motor: -0.5, -0.3" << std::endl; - // tracer.SetMotionCommand(-0.5, -0.3); - // } else if (count < 35) { - // std::cout << "Motor: -1.0, -0.5" << std::endl; - // tracer.SetMotionCommand(-1.0, -0.5); - // } else if (count < 40) { - // std::cout << "Motor: 0.0, 0.0," << std::endl; - // tracer.SetMotionCommand(0.0, 0.0); - // } + // motion control + if (count < 5) { + std::cout << "Motor: 0.2, 0.0" << std::endl; + tracer.SetMotionCommand(0.2, 0.0); + } else if (count < 10) { + std::cout << "Motor: 0.8, 0.3" << std::endl; + tracer.SetMotionCommand(0.8, 0.3); + } else if (count < 15) { + std::cout << "Motor: 1.5, 0.5" << std::endl; + tracer.SetMotionCommand(1.5, 0.5); + } else if (count < 20) { + std::cout << "Motor: 1.0, 0.3" << std::endl; + tracer.SetMotionCommand(1.0, 0.3); + } else if (count < 25) { + std::cout << "Motor: 0.0, 0.0" << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } else if (count < 30) { + std::cout << "Motor: -0.5, -0.3" << std::endl; + tracer.SetMotionCommand(-0.5, -0.3); + } else if (count < 35) { + std::cout << "Motor: -1.0, -0.5" << std::endl; + tracer.SetMotionCommand(-1.0, -0.5); + } else if (count < 40) { + std::cout << "Motor: 0.0, 0.0," << std::endl; + tracer.SetMotionCommand(0.0, 0.0); + } + // tracer.SetMotionCommand(0.8, 0.8); auto state = tracer.GetTracerState(); std::cout << "-------------------------------" << std::endl; @@ -89,6 +95,7 @@ int main(int argc, char **argv) { << state.angular_velocity << std::endl; std::cout << "-------------------------------" << std::endl; + // usleep(20000); sleep(1); ++count; } diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp index 0e8acee..3670af2 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp @@ -34,8 +34,7 @@ class TracerBase : public MobileBase { void SetMotionCommand(double linear_vel, double angular_vel); // light control - void SetLightCommand(TracerLightCmd cmd); - void DisableLightCmdControl(); + void SetLightCommand(const TracerLightCmd &cmd); private: // cmd/status update related variables @@ -53,7 +52,7 @@ class TracerBase : public MobileBase { bool light_ctrl_requested_ = false; void SendMotionCmd(uint8_t count); - void SendLightCmd(uint8_t count); + void SendLightCmd(const TracerLightCmd &cmd, uint8_t count); void SendRobotCmd() override; void ParseCANFrame(can_frame *rx_frame) override; diff --git a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp index 475a8b7..a898001 100644 --- a/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp +++ b/ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp @@ -77,11 +77,13 @@ struct TracerLightCmd { TracerLightCmd() = default; TracerLightCmd(LightMode f_mode, uint8_t f_value, LightMode r_mode, uint8_t r_value) - : front_mode(f_mode), + : enable_ctrl(true), + front_mode(f_mode), front_custom_value(f_value), rear_mode(r_mode), rear_custom_value(r_value) {} + bool enable_ctrl = false; LightMode front_mode; uint8_t front_custom_value; LightMode rear_mode; diff --git a/ugv_sdk/src/agx_msg_parser.c b/ugv_sdk/src/agx_msg_parser.c index ecaae82..3077aed 100644 --- a/ugv_sdk/src/agx_msg_parser.c +++ b/ugv_sdk/src/agx_msg_parser.c @@ -108,8 +108,6 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) { tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->body.motion_command_msg.raw, tx_frame->can_dlc); - // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, - // tx_frame->can_dlc); break; } case AgxMsgLightCommand: { @@ -117,8 +115,6 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) { tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->body.light_command_msg.raw, tx_frame->can_dlc); - // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, - // tx_frame->can_dlc); break; } case AgxMsgCtrlModeSelect: { @@ -126,16 +122,12 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) { tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->body.ctrl_mode_select_msg.raw, tx_frame->can_dlc); - // tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, - // tx_frame->can_dlc); break; } case AgxMsgFaultByteReset: { tx_frame->can_id = CAN_MSG_STATE_RESET_ID; tx_frame->can_dlc = 8; memcpy(tx_frame->data, msg->body.state_reset_msg.raw, tx_frame->can_dlc); - tx_frame->data[7] = CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, - tx_frame->can_dlc); break; } // state feedback frame @@ -214,8 +206,9 @@ void EncodeCanFrame(const AgxMessage *msg, struct can_frame *tx_frame) { default: break; } - tx_frame->data[7] = - CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, tx_frame->can_dlc); + // tx_frame->data[7] = + // CalcCanFrameChecksum(tx_frame->can_id, tx_frame->data, + // tx_frame->can_dlc); } uint8_t CalcCanFrameChecksum(uint16_t id, uint8_t *data, uint8_t dlc) { diff --git a/ugv_sdk/src/tracer_base.cpp b/ugv_sdk/src/tracer_base.cpp index 6a9296f..af0c226 100644 --- a/ugv_sdk/src/tracer_base.cpp +++ b/ugv_sdk/src/tracer_base.cpp @@ -15,14 +15,9 @@ namespace westonrobot { void TracerBase::SendRobotCmd() { static uint8_t cmd_count = 0; - static uint8_t light_cmd_count = 0; + // EnableCommandedMode(); if (can_connected_) { SendMotionCmd(cmd_count++); - SendLightCmd(light_cmd_count++); - // if (light_ctrl_requested_) - // SendLightCmd(light_cmd_count++); - // else - // std::cout << "not updating light cmd" << std::endl; } } @@ -46,9 +41,9 @@ void TracerBase::SendMotionCmd(uint8_t count) { motion_cmd_mutex_.lock(); int16_t linear_cmd = - static_cast(current_motion_cmd_.linear_velocity) * 1000; + static_cast(current_motion_cmd_.linear_velocity * 1000); int16_t angular_cmd = - static_cast(current_motion_cmd_.angular_velocity) * 1000; + static_cast(current_motion_cmd_.angular_velocity * 1000); motion_cmd_mutex_.unlock(); // SendControlCmd(); @@ -67,28 +62,24 @@ void TracerBase::SendMotionCmd(uint8_t count) { can_if_->SendFrame(m_frame); } -void TracerBase::SendLightCmd(uint8_t count) { +void TracerBase::SendLightCmd(const TracerLightCmd &lcmd, uint8_t count) { AgxMessage l_msg; l_msg.type = AgxMsgLightCommand; memset(l_msg.body.light_command_msg.raw, 0, 8); - light_cmd_mutex_.lock(); - if (light_ctrl_enabled_) { + if (lcmd.enable_ctrl) { l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_ENABLE; l_msg.body.light_command_msg.cmd.front_light_mode = - static_cast(current_light_cmd_.front_mode); + static_cast(lcmd.front_mode); l_msg.body.light_command_msg.cmd.front_light_custom = - current_light_cmd_.front_custom_value; + lcmd.front_custom_value; l_msg.body.light_command_msg.cmd.rear_light_mode = - static_cast(current_light_cmd_.rear_mode); - l_msg.body.light_command_msg.cmd.rear_light_custom = - current_light_cmd_.rear_custom_value; + static_cast(lcmd.rear_mode); + l_msg.body.light_command_msg.cmd.rear_light_custom = lcmd.rear_custom_value; } else { l_msg.body.light_command_msg.cmd.light_ctrl_enabled = LIGHT_CTRL_DISABLE; } - light_ctrl_requested_ = false; - light_cmd_mutex_.unlock(); l_msg.body.light_command_msg.cmd.count = count; @@ -96,6 +87,8 @@ void TracerBase::SendLightCmd(uint8_t count) { can_frame l_frame; EncodeCanFrame(&l_msg, &l_frame); can_if_->SendFrame(l_frame); + + std::cout << "light cmd sent <---------------------" << std::endl; } TracerState TracerBase::GetTracerState() { @@ -123,19 +116,9 @@ void TracerBase::SetMotionCommand(double linear_vel, double angular_vel) { FeedCmdTimeoutWatchdog(); } -void TracerBase::SetLightCommand(TracerLightCmd cmd) { - if (!cmd_thread_started_) StartCmdThread(); - - std::lock_guard guard(light_cmd_mutex_); - current_light_cmd_ = cmd; - light_ctrl_enabled_ = true; - light_ctrl_requested_ = true; -} - -void TracerBase::DisableLightCmdControl() { - std::lock_guard guard(light_cmd_mutex_); - light_ctrl_enabled_ = false; - light_ctrl_requested_ = true; +void TracerBase::SetLightCommand(const TracerLightCmd &cmd) { + static uint8_t light_cmd_count = 0; + SendLightCmd(cmd, light_cmd_count++); } void TracerBase::ParseCANFrame(can_frame *rx_frame) {