mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-09-27 02:09:12 +08:00
116 lines
4.6 KiB
C++
116 lines
4.6 KiB
C++
#include "common_inc.h"
|
|
|
|
extern DummyRobot dummy;
|
|
|
|
|
|
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
|
{
|
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
|
if (_cmd[0] == '!' || !dummy.IsEnabled())
|
|
{
|
|
std::string s(_cmd);
|
|
if (s.find("STOP") != std::string::npos)
|
|
{
|
|
dummy.commandHandler.EmergencyStop();
|
|
Respond(_responseChannel, "Stopped ok");
|
|
} else if (s.find("START") != std::string::npos)
|
|
{
|
|
dummy.SetEnable(true);
|
|
Respond(_responseChannel, "Started ok");
|
|
} else if (s.find("DISABLE") != std::string::npos)
|
|
{
|
|
dummy.SetEnable(false);
|
|
Respond(_responseChannel, "Disabled ok");
|
|
}
|
|
} else if (_cmd[0] == '#')
|
|
{
|
|
std::string s(_cmd);
|
|
if (s.find("GETJPOS") != std::string::npos)
|
|
{
|
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
|
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
|
|
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
|
|
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
|
} else if (s.find("GETLPOS") != std::string::npos)
|
|
{
|
|
dummy.UpdateJointPose6D();
|
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
|
dummy.currentPose6D.X, dummy.currentPose6D.Y,
|
|
dummy.currentPose6D.Z, dummy.currentPose6D.A,
|
|
dummy.currentPose6D.B, dummy.currentPose6D.C);
|
|
} else if (s.find("CMDMODE") != std::string::npos)
|
|
{
|
|
uint32_t mode;
|
|
sscanf(_cmd, "#CMDMODE %lu", &mode);
|
|
dummy.SetCommandMode(mode);
|
|
Respond(_responseChannel, "Set command mode to [%lu]", mode);
|
|
} else
|
|
Respond(_responseChannel, "ok");
|
|
} else if (_cmd[0] == '>' || _cmd[0] == '@')
|
|
{
|
|
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
|
|
Respond(_responseChannel, "%d", freeSize);
|
|
}
|
|
|
|
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
|
}
|
|
|
|
|
|
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
|
{
|
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
|
if (_cmd[0] == '!' || !dummy.IsEnabled())
|
|
{
|
|
std::string s(_cmd);
|
|
if (s.find("STOP") != std::string::npos)
|
|
{
|
|
dummy.commandHandler.EmergencyStop();
|
|
Respond(_responseChannel, "Stopped ok");
|
|
} else if (s.find("START") != std::string::npos)
|
|
{
|
|
dummy.SetEnable(true);
|
|
Respond(_responseChannel, "Started ok");
|
|
} else if (s.find("DISABLE") != std::string::npos)
|
|
{
|
|
dummy.SetEnable(false);
|
|
Respond(_responseChannel, "Disabled ok");
|
|
}
|
|
} else if (_cmd[0] == '#')
|
|
{
|
|
std::string s(_cmd);
|
|
if (s.find("GETJPOS") != std::string::npos)
|
|
{
|
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
|
dummy.currentJoints.a[0], dummy.currentJoints.a[1],
|
|
dummy.currentJoints.a[2], dummy.currentJoints.a[3],
|
|
dummy.currentJoints.a[4], dummy.currentJoints.a[5]);
|
|
} else if (s.find("GETLPOS") != std::string::npos)
|
|
{
|
|
dummy.UpdateJointPose6D();
|
|
Respond(_responseChannel, "ok %.2f %.2f %.2f %.2f %.2f %.2f",
|
|
dummy.currentPose6D.X, dummy.currentPose6D.Y,
|
|
dummy.currentPose6D.Z, dummy.currentPose6D.A,
|
|
dummy.currentPose6D.B, dummy.currentPose6D.C);
|
|
} else if (s.find("CMDMODE") != std::string::npos)
|
|
{
|
|
uint32_t mode;
|
|
sscanf(_cmd, "#CMDMODE %lu", &mode);
|
|
dummy.SetCommandMode(mode);
|
|
Respond(_responseChannel, "Set command mode to [%lu]", mode);
|
|
} else
|
|
Respond(_responseChannel, "ok");
|
|
} else if (_cmd[0] == '>' || _cmd[0] == '@')
|
|
{
|
|
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
|
|
Respond(_responseChannel, "%d", freeSize);
|
|
}
|
|
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
|
}
|
|
|
|
|
|
void OnUart5AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
|
|
{
|
|
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
|
|
|
|
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
|
|
} |