mirrors/ugv_sdk
1
0
Fork 0
mirror of https://github.com/westonrobot/ugv_sdk synced 2023-04-08 06:32:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

tracer protocol v2.0 works

Browse Source
This commit is contained in:
Ruixiang Du
2020-11-06 09:37:37 +08:00
parent 87cd3665aa
commit 3ca082d1f0
5 changed files with 58 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
ugv_sdk/apps/tracer_demo/tracer_demo.cpp
View File

@@ -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;
}

5
ugv_sdk/include/ugv_sdk/tracer/tracer_base.hpp
View File

@@ -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;

4
ugv_sdk/include/ugv_sdk/tracer/tracer_types.hpp
View File

@@ -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;

13
ugv_sdk/src/agx_msg_parser.c
View File

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

45
ugv_sdk/src/tracer_base.cpp
View File

@@ -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<int16_t>(current_motion_cmd_.linear_velocity) * 1000;
static_cast<int16_t>(current_motion_cmd_.linear_velocity * 1000);
int16_t angular_cmd =
static_cast<int16_t>(current_motion_cmd_.angular_velocity) * 1000;
static_cast<int16_t>(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<uint8_t>(current_light_cmd_.front_mode);
static_cast<uint8_t>(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<uint8_t>(current_light_cmd_.rear_mode);
l_msg.body.light_command_msg.cmd.rear_light_custom =
current_light_cmd_.rear_custom_value;
static_cast<uint8_t>(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<std::mutex> guard(light_cmd_mutex_);
current_light_cmd_ = cmd;
light_ctrl_enabled_ = true;
light_ctrl_requested_ = true;
}
void TracerBase::DisableLightCmdControl() {
std::lock_guard<std::mutex> 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) {