[Fw] Update REF protocols.

This commit is contained in:
peng-zhihui 2022-02-09 11:52:03 +08:00
parent f0e2c8da3a
commit fe1007c668
21 changed files with 3793 additions and 3705 deletions

View File

@ -19,16 +19,14 @@ volatile bool endpointListValid = false;
osThreadId_t commTaskHandle;
const osThreadAttr_t commTask_attributes = {
.name = "commTask",
.stack_size = 10000 * 4,
.stack_size = 45000,
.priority = (osPriority_t) osPriorityNormal,
};
void InitCommunication(void)
{
printf("\r\nHello, REF v%.1f Started!\r\n", CONFIG_FW_VERSION);
// Start command handling thread
commTaskHandle = osThreadNew(CommunicationTask, NULL, &commTask_attributes);
commTaskHandle = osThreadNew(CommunicationTask, nullptr, &commTask_attributes);
while (!endpointListValid)
osDelay(1);
@ -84,12 +82,8 @@ int _write(int file, const char* data, int len);
// @brief This is what printf calls internally
int _write(int file, const char* data, int len)
{
#ifdef DEBUG_VIA_USB_SERIAL
usbStreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
uart4StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
#endif
uart5StreamOutputPtr->process_bytes((const uint8_t*) data, len, nullptr);
return len;
}

View File

@ -245,8 +245,6 @@ void CanSendMessage(CAN_context* canCtx, uint8_t* txData, CAN_TxHeaderTypeDef* t
return;
if (semaphore_status == osOK)
{
HAL_CAN_AddTxMessage(canCtx->handle, txHeader, txData, &canCtx->last_heartbeat_mailbox);
}
}

View File

@ -178,14 +178,14 @@ static void UartServerTask(void* ctx)
const osThreadAttr_t uartServerTask_attributes = {
.name = "UartServerTask",
.stack_size = 1000 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
void StartUartServer()
{
// DMA is set up to recieve in a circular buffer forever.
// We dont use interrupts to fetch the data, instead we periodically read
// DMA is set up to receive in a circular buffer forever.
// We don't use interrupts to fetch the data, instead we periodically read
// data out of the circular buffer into a parse buffer, controlled by a state machine
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer[0], sizeof(dma_rx_buffer[0]));
dma_last_rcv_idx[0] = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;

View File

@ -179,7 +179,7 @@ void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair)
const osThreadAttr_t usbServerTask_attributes = {
.name = "UsbServerTask",
.stack_size = 512 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};

View File

@ -79,7 +79,6 @@ include_directories(
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
"startup/*.*"

View File

@ -79,7 +79,6 @@ include_directories(
)
add_definitions(-DUSE_HAL_DRIVER -DSTM32F4 -DSTM32F4xx -DSTM32F405xx -DconfigAPPLICATION_ALLOCATED_HEAP)
# add_definitions(-DDEBUG_VIA_USB_SERIAL)
file(GLOB_RECURSE SOURCES
"startup/*.*"

View File

@ -62,7 +62,7 @@ osSemaphoreId sem_can2_tx;
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask",
.stack_size = 500 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
@ -126,7 +126,7 @@ void MX_FREERTOS_Init(void) {
// This Task must run before MX_USB_DEVICE_Init(), so have to put it here.
const osThreadAttr_t usbIrqTask_attributes = {
.name = "usbIrqTask",
.stack_size = 128 * 4,
.stack_size = 500,
.priority = (osPriority_t) osPriorityAboveNormal,
};
usbIrqTaskHandle = osThreadNew(UsbDeferredInterruptTask, NULL, &usbIrqTask_attributes);

View File

@ -255,7 +255,16 @@ void CAN1_TX_IRQHandler(void)
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
if (( READ_REG(hcan1.Instance->IER) & CAN_IT_RX_FIFO0_MSG_PENDING) != 0U)
{
/* Check if message is still pending */
if ((hcan1.Instance->RF0R & CAN_RF0R_FMP0) != 0U)
{
/* Call weak (surcharged) callback */
HAL_CAN_RxFifo0MsgPendingCallback(&hcan1);
}
}
return;
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */

View File

@ -3,9 +3,9 @@
CtrlStepMotor::CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse,
uint8_t _reduction, float _minAngle, float _maxAngle) :
uint8_t _reduction, float _angleLimitMin, float _angleLimitMax) :
nodeID(_id), hcan(_hcan), inverseDirection(_inverse), reduction(_reduction),
angleLimitMin(_minAngle), angleLimitMax(_maxAngle)
angleLimitMin(_angleLimitMin), angleLimitMax(_angleLimitMax)
{
txHeader =
{
@ -79,8 +79,6 @@ void CtrlStepMotor::SetVelocitySetPoint(float _val)
void CtrlStepMotor::SetPositionSetPoint(float _val)
{
state = RUNNING;
uint8_t mode = 0x05;
txHeader.StdId = nodeID << 7 | mode;
@ -88,32 +86,13 @@ void CtrlStepMotor::SetPositionSetPoint(float _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need ACK
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetPositionWithTime(float _pos, float _time)
{
state = RUNNING;
uint8_t mode = 0x06;
txHeader.StdId = nodeID << 7 | mode;
// Float to Bytes
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
b = (unsigned char*) &_time;
for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i - 4);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel)
void CtrlStepMotor::SetPositionWithVelocityLimit(float _pos, float _vel)
{
uint8_t mode = 0x07;
txHeader.StdId = nodeID << 7 | mode;
@ -122,7 +101,6 @@ void CtrlStepMotor::AddTrajectoryPoint(float _pos, float _vel)
auto* b = (unsigned char*) &_pos;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
b = (unsigned char*) &_vel;
for (int i = 4; i < 8; i++)
canBuf[i] = *(b + i - 4);
@ -140,6 +118,7 @@ void CtrlStepMotor::SetNodeID(uint32_t _id)
auto* b = (unsigned char*) &_id;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -154,6 +133,7 @@ void CtrlStepMotor::SetCurrentLimit(float _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -168,12 +148,13 @@ void CtrlStepMotor::SetVelocityLimit(float _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem)
void CtrlStepMotor::SetAcceleration(float _val)
{
uint8_t mode = 0x14;
txHeader.StdId = nodeID << 7 | mode;
@ -182,7 +163,7 @@ void CtrlStepMotor::SetAcceleration(float _val, bool _storeToMem)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = _storeToMem ? 1 : 0;
canBuf[4] = 0; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -207,12 +188,13 @@ void CtrlStepMotor::SetEnableOnBoot(bool _enable)
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableAck(bool _enable)
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{
uint8_t mode = 0x1B;
txHeader.StdId = nodeID << 7 | mode;
@ -221,20 +203,7 @@ void CtrlStepMotor::SetEnableAck(bool _enable)
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
void CtrlStepMotor::SetEnableStallProtect(bool _enable)
{
uint8_t mode = 0x1C;
txHeader.StdId = nodeID << 7 | mode;
uint32_t val = _enable ? 1 : 0;
auto* b = (unsigned char*) &val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -261,19 +230,16 @@ void CtrlStepMotor::EraseConfigs()
void CtrlStepMotor::SetAngle(float _angle)
{
_angle = inverseDirection ? -_angle : _angle;
if (_angle <= angleLimitMax && _angle >= angleLimitMin)
{
float stepMotorCnt = _angle / 360.0f * reduction * CTRL_CIRCLE_COUNT;
float stepMotorCnt = _angle / 360.0f * (float) reduction;
SetPositionSetPoint(stepMotorCnt);
}
}
void CtrlStepMotor::SetAngleWithTime(float _angle, float _time)
void CtrlStepMotor::SetAngleWithVelocityLimit(float _angle, float _vel)
{
_angle = inverseDirection ? -_angle : _angle;
float stepMotorCnt = _angle / 360.0f * reduction;
SetPositionWithTime(stepMotorCnt, _time);
float stepMotorCnt = _angle / 360.0f * (float) reduction;
SetPositionWithVelocityLimit(stepMotorCnt, _vel);
}
@ -286,10 +252,9 @@ void CtrlStepMotor::UpdateAngle()
}
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isAck)
void CtrlStepMotor::UpdateAngleCallback(float _pos, bool _isFinished)
{
if (_isAck)
state = FINISH;
state = _isFinished ? FINISH : RUNNING;
float tmp = _pos / (float) reduction * 360;
angle = inverseDirection ? -tmp : tmp;
@ -304,6 +269,7 @@ void CtrlStepMotor::SetDceKp(int32_t _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -317,6 +283,7 @@ void CtrlStepMotor::SetDceKv(int32_t _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -330,6 +297,7 @@ void CtrlStepMotor::SetDceKi(int32_t _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}
@ -343,6 +311,7 @@ void CtrlStepMotor::SetDceKd(int32_t _val)
auto* b = (unsigned char*) &_val;
for (int i = 0; i < 4; i++)
canBuf[i] = *(b + i);
canBuf[4] = 1; // Need save to EEPROM or not
CanSendMessage(get_can_ctx(hcan), canBuf, &txHeader);
}

View File

@ -18,7 +18,7 @@ public:
const uint32_t CTRL_CIRCLE_COUNT = 200 * 256;
CtrlStepMotor(CAN_HandleTypeDef* _hcan, uint8_t _id, bool _inverse = false, uint8_t _reduction = 1,
float _minAngle = -180, float _maxAngle = 180);
float _angleLimitMin = -180, float _angleLimitMax = 180);
uint8_t nodeID;
float angle = 0;
@ -29,32 +29,30 @@ public:
State state = STOP;
void SetAngle(float _angle);
void SetAngleWithTime(float _angle, float _time);
void SetAngleWithVelocityLimit(float _angle, float _vel);
// CAN Command
void SetEnable(bool _enable);
void DoCalibration();
void SetCurrentSetPoint(float _val);
void SetVelocitySetPoint(float _val);
void SetPositionSetPoint(float _val);
void SetPositionWithTime(float _pos, float _time);
void AddTrajectoryPoint(float _pos, float _vel);
void SetPositionWithVelocityLimit(float _pos, float _vel);
void SetNodeID(uint32_t _id);
void SetCurrentLimit(float _val);
void SetVelocityLimit(float _val);
void SetAcceleration(float _val, bool _storeToMem = false);
void SetAcceleration(float _val);
void SetDceKp(int32_t _val);
void SetDceKv(int32_t _val);
void SetDceKi(int32_t _val);
void SetDceKd(int32_t _val);
void ApplyPositionAsHome();
void SetEnableOnBoot(bool _enable);
void SetEnableAck(bool _enable);
void SetEnableStallProtect(bool _enable);
void Reboot();
void EraseConfigs();
void UpdateAngle();
void UpdateAngleCallback(float _pos, bool _isAck);
void UpdateAngleCallback(float _pos, bool _isFinished);
// Communication protocol definitions
@ -65,16 +63,15 @@ public:
make_protocol_function("reboot", *this, &CtrlStepMotor::Reboot),
make_protocol_function("erase_configs", *this, &CtrlStepMotor::EraseConfigs),
make_protocol_function("set_enable", *this, &CtrlStepMotor::SetEnable, "enable"),
make_protocol_function("set_position_with_time", *this, &CtrlStepMotor::SetPositionWithTime,
"pos", "time"),
make_protocol_function("set_position_with_time", *this,
&CtrlStepMotor::SetPositionWithVelocityLimit, "pos", "time"),
make_protocol_function("set_position", *this, &CtrlStepMotor::SetPositionSetPoint, "pos"),
make_protocol_function("set_velocity", *this, &CtrlStepMotor::SetVelocitySetPoint, "vel"),
make_protocol_function("set_velocity_limit", *this, &CtrlStepMotor::SetVelocityLimit, "vel"),
make_protocol_function("set_current", *this, &CtrlStepMotor::SetCurrentSetPoint, "current"),
make_protocol_function("set_current_limit", *this, &CtrlStepMotor::SetCurrentLimit, "current"),
make_protocol_function("set_node_id", *this, &CtrlStepMotor::SetNodeID, "id"),
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc",
"stored"),
make_protocol_function("set_acceleration", *this, &CtrlStepMotor::SetAcceleration, "acc"),
make_protocol_function("apply_home_offset", *this, &CtrlStepMotor::ApplyPositionAsHome),
make_protocol_function("do_calibration", *this, &CtrlStepMotor::DoCalibration),
make_protocol_function("set_enable_on_boot", *this, &CtrlStepMotor::SetEnableOnBoot, "enable"),
@ -82,7 +79,6 @@ public:
make_protocol_function("set_dce_kv", *this, &CtrlStepMotor::SetDceKv, "vel"),
make_protocol_function("set_dce_ki", *this, &CtrlStepMotor::SetDceKi, "vel"),
make_protocol_function("set_dce_kd", *this, &CtrlStepMotor::SetDceKd, "vel"),
make_protocol_function("set_enable_ack", *this, &CtrlStepMotor::SetEnableAck, "enable"),
make_protocol_function("set_enable_stall_protect", *this, &CtrlStepMotor::SetEnableStallProtect,
"enable"),
make_protocol_function("update_angle", *this, &CtrlStepMotor::UpdateAngle)

View File

@ -82,8 +82,8 @@ static void EulerAngleToRotMat(const float* _eulerAngles, float* _rotationM)
}
DOF6Kinematic::DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT)
: armConfig(ArmConfig_t{L_BS, D_BS, L_SE, L_EW, D_EW, L_WT})
DOF6Kinematic::DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT)
: armConfig(ArmConfig_t{L_BS, D_BS, L_AM, L_FA, D_EW, L_WT})
{
float tmp_DH_matrix[6][4] = {
{0.0f, armConfig.L_BASE, armConfig.D_BASE, -(float) M_PI_2},

View File

@ -73,7 +73,7 @@ public:
char solFlag[8][3];
};
DOF6Kinematic(float L_BS, float D_BS, float L_SE, float L_EW, float D_EW, float L_WT);
DOF6Kinematic(float L_BS, float D_BS, float L_AM, float L_FA, float D_EW, float L_WT);
bool SolveFK(const Joint6D_t &_inputJoint6D, Pose6D_t &_outputPose6D);

View File

@ -1,16 +1,16 @@
#include <FreeRTOS.h>
#include "communication.hpp"
#include "dummy_robot.h"
inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints)
inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints, uint8_t &_index)
{
float max = -1;
for (float i: _joints.a)
for (uint8_t i = 0; i < 6; i++)
{
if (abs(i) > max)
max = abs(i);
if (abs(_joints.a[i]) > max)
{
max = abs(_joints.a[i]);
_index = i;
}
}
return max;
@ -20,7 +20,7 @@ inline float AbsMaxOf6(DOF6Kinematic::Joint6D_t _joints)
DummyRobot::DummyRobot(CAN_HandleTypeDef* _hcan) :
hcan(_hcan)
{
motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 30, -180, 180);
motorJ[ALL] = new CtrlStepMotor(_hcan, 0, false, 1, -180, 180);
motorJ[1] = new CtrlStepMotor(_hcan, 1, true, 50, -170, 170);
motorJ[2] = new CtrlStepMotor(_hcan, 2, false, 30, -73, 90);
motorJ[3] = new CtrlStepMotor(_hcan, 3, true, 30, 35, 180);
@ -43,6 +43,13 @@ DummyRobot::~DummyRobot()
}
void DummyRobot::Init()
{
SetCommandMode(DEFAULT_COMMAND_MODE);
SetJointSpeed(DEFAULT_JOINT_SPEED);
}
void DummyRobot::Reboot()
{
motorJ[ALL]->Reboot();
@ -51,43 +58,51 @@ void DummyRobot::Reboot()
}
float DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints)
void DummyRobot::MoveJoints(DOF6Kinematic::Joint6D_t _joints)
{
float maxAngle = AbsMaxOf6(_joints - currentJoints);
float time = maxAngle / jointSpeed;
jointsStateFlag = 0;
for (int j = 1; j <= 6; j++)
motorJ[j]->SetAngleWithTime(_joints.a[j - 1] - initPose.a[j - 1], time);
return time;
{
motorJ[j]->SetAngleWithVelocityLimit(_joints.a[j - 1] - initPose.a[j - 1],
dynamicJointSpeeds.a[j - 1]);
}
}
float DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
bool DummyRobot::MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
{
float joints[6] = {_j1, _j2, _j3, _j4, _j5, _j6};
DOF6Kinematic::Joint6D_t targetJointsTmp(_j1, _j2, _j3, _j4, _j5, _j6);
bool valid = true;
for (int j = 1; j <= 6; j++)
{
if (joints[j - 1] > motorJ[j]->angleLimitMax ||
joints[j - 1] < motorJ[j]->angleLimitMin)
if (targetJointsTmp.a[j - 1] > motorJ[j]->angleLimitMax ||
targetJointsTmp.a[j - 1] < motorJ[j]->angleLimitMin)
valid = false;
}
float time = 0;
if (valid)
time = MoveJoints(DOF6Kinematic::Joint6D_t(
joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]));
{
DOF6Kinematic::Joint6D_t deltaJoints = targetJointsTmp - currentJoints;
uint8_t index;
float maxAngle = AbsMaxOf6(deltaJoints, index);
float time = maxAngle * (float) (motorJ[index + 1]->reduction) / jointSpeed;
for (int j = 1; j <= 6; j++)
{
dynamicJointSpeeds.a[j - 1] =
abs(deltaJoints.a[j - 1] * (float) (motorJ[j]->reduction) / time * 0.1f); //0~10r/s
}
return time;
jointsStateFlag = 0;
targetJoints = targetJointsTmp;
return true;
}
return false;
}
float DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c)
bool DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float _c)
{
DOF6Kinematic::Pose6D_t pose6D(_x, _y, _z, _a, _b, _c);
DOF6Kinematic::IKSolves_t ikSolves{};
@ -118,7 +133,7 @@ float DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float
if (validCnt)
{
float min = 1000;
uint8_t index = 0;
uint8_t indexConfig = 0, indexJoint = 0;
for (int i = 0; i < 8; i++)
{
if (valid[i])
@ -126,34 +141,23 @@ float DummyRobot::MoveL(float _x, float _y, float _z, float _a, float _b, float
for (int j = 0; j < 6; j++)
lastJoint6D.a[j] = ikSolves.config[i].a[j];
DOF6Kinematic::Joint6D_t tmp = currentJoints - lastJoint6D;
float maxAngle = AbsMaxOf6(tmp);
float maxAngle = AbsMaxOf6(tmp, indexJoint);
if (maxAngle < min)
{
min = maxAngle;
index = i;
indexConfig = i;
}
}
}
return MoveJ(ikSolves.config[index].a[0], ikSolves.config[index].a[1], ikSolves.config[index].a[2],
ikSolves.config[index].a[3], ikSolves.config[index].a[4], ikSolves.config[index].a[5]);
} else
return 0;
return MoveJ(ikSolves.config[indexConfig].a[0], ikSolves.config[indexConfig].a[1],
ikSolves.config[indexConfig].a[2], ikSolves.config[indexConfig].a[3],
ikSolves.config[indexConfig].a[4], ikSolves.config[indexConfig].a[5]);
}
return false;
}
void DummyRobot::MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
{
}
void DummyRobot::MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c)
{
}
void DummyRobot::UpdateJointAngles()
{
motorJ[ALL]->UpdateAngle();
@ -179,21 +183,26 @@ void DummyRobot::SetJointSpeed(float _speed)
if (_speed < 0)_speed = 0;
else if (_speed > 100) _speed = 100;
jointSpeed = _speed;
jointSpeed = _speed * jointSpeedRatio;
}
void DummyRobot::SetJointAcceleration(float _acc)
{
if (_acc < 0)_acc = 0;
else if (_acc > 1000) _acc = 1000;
else if (_acc > 100) _acc = 100;
motorJ[ALL]->SetAcceleration(_acc, false);
for (int i = 1; i <= 6; i++)
motorJ[i]->SetAcceleration(_acc / 100 * DEFAULT_JOINT_ACCELERATION_BASES.a[i - 1]);
}
void DummyRobot::CalibrateHomeOffset()
{
// Disable FixUpdate, but not disable motors
isEnabled = false;
motorJ[ALL]->SetEnable(true);
// 1.Manually move joints to L-Pose [precisely]
// ...
motorJ[2]->SetCurrentLimit(0.5);
@ -208,8 +217,6 @@ void DummyRobot::CalibrateHomeOffset()
initPose = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
currentJoints = DOF6Kinematic::Joint6D_t(0, 0, 90, 0, 0, 0);
Resting();
while (IsMoving())
osDelay(10);
osDelay(500);
// 4.Apply Home-Offset the second time
@ -229,6 +236,7 @@ void DummyRobot::Homing()
SetJointSpeed(10);
MoveJ(0, 0, 90, 0, 0, 0);
MoveJoints(targetJoints);
while (IsMoving())
osDelay(10);
@ -243,6 +251,7 @@ void DummyRobot::Resting()
MoveJ(REST_POSE.a[0], REST_POSE.a[1], REST_POSE.a[2],
REST_POSE.a[3], REST_POSE.a[4], REST_POSE.a[5]);
MoveJoints(targetJoints);
while (IsMoving())
osDelay(10);
@ -253,7 +262,7 @@ void DummyRobot::Resting()
void DummyRobot::SetEnable(bool _enable)
{
motorJ[ALL]->SetEnable(_enable);
hand->SetEnable(_enable);
isEnabled = _enable;
}
@ -272,25 +281,39 @@ bool DummyRobot::IsMoving()
}
void DummyRobot::SetCommandMode(uint8_t _mode)
bool DummyRobot::IsEnabled()
{
return isEnabled;
}
void DummyRobot::SetCommandMode(uint32_t _mode)
{
if (_mode < COMMAND_TARGET_POINT_SEQUENTIAL ||
_mode > COMMAND_MOTOR_TUNING)
return;
commandMode = static_cast<CommandMode>(_mode);
switch (commandMode)
{
case COMMAND_TARGET_POINT_SEQUENTIAL:
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION);
break;
case COMMAND_TARGET_POINT_INTERRUPTABLE:
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION);
jointSpeedRatio = 1;
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_LOW);
break;
case COMMAND_CONTINUES_TRAJECTORY:
SetJointAcceleration(1000);
SetJointAcceleration(DEFAULT_JOINT_ACCELERATION_HIGH);
jointSpeedRatio = 0.3;
break;
case COMMAND_MOTOR_TUNING:
break;
}
}
DummyHand::DummyHand(CAN_HandleTypeDef* _hcan, uint8_t _id) :
DummyHand::DummyHand(CAN_HandleTypeDef* _hcan, uint8_t
_id) :
nodeID(_id), hcan(_hcan)
{
txHeader =
@ -362,18 +385,12 @@ void DummyRobot::CommandHandler::EmergencyStop()
{
context->MoveJ(context->currentJoints.a[0], context->currentJoints.a[1], context->currentJoints.a[2],
context->currentJoints.a[3], context->currentJoints.a[4], context->currentJoints.a[5]);
context->isStopped = true;
context->MoveJoints(context->targetJoints);
context->isEnabled = false;
ClearFifo();
}
void DummyRobot::CommandHandler::Resume()
{
context->isStopped = false;
}
std::string DummyRobot::CommandHandler::Pop(uint32_t timeout)
{
osStatus_t status = osMessageQueueGet(commandFifo, strBuffer, nullptr, timeout);
@ -395,6 +412,7 @@ uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd)
switch (context->commandMode)
{
case COMMAND_TARGET_POINT_SEQUENTIAL:
case COMMAND_CONTINUES_TRAJECTORY:
if (_cmd[0] == '>')
{
float joints[6];
@ -412,12 +430,37 @@ uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd)
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
// Trigger a transmission immediately, in case IsMoving() returns false
context->MoveJoints(context->targetJoints);
while (context->IsMoving() && context->IsEnabled())
osDelay(5);
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
}
// Trigger a transmission immediately, in case IsMoving() returns false
context->MoveJoints(context->targetJoints);
while (context->IsMoving())
osDelay(10);
osDelay(5);
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
}
break;
case COMMAND_TARGET_POINT_INTERRUPTABLE:
@ -438,12 +481,29 @@ uint32_t DummyRobot::CommandHandler::ParseCommand(const std::string &_cmd)
context->MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd.c_str(), "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
context->SetJointSpeed(speed);
context->MoveL(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5]);
}
Respond(*usbStreamOutputPtr, "ok");
Respond(*uart4StreamOutputPtr, "ok");
}
break;
case COMMAND_CONTINUES_TRAJECTORY:
case COMMAND_MOTOR_TUNING:
break;
}
@ -456,3 +516,31 @@ void DummyRobot::CommandHandler::ClearFifo()
osMessageQueueReset(commandFifo);
}
void DummyRobot::TuningHelper::SetTuningFlag(uint8_t _flag)
{
tuningFlag = _flag;
}
void DummyRobot::TuningHelper::Tick(uint32_t _timeMillis)
{
time += PI * 2 * frequency * (float) _timeMillis / 1000.0f;
float delta = amplitude * sinf(time);
for (int i = 1; i <= 6; i++)
if (tuningFlag & (1 << (i - 1)))
context->motorJ[i]->SetAngle(delta);
}
void DummyRobot::TuningHelper::SetFreqAndAmp(float _freq, float _amp)
{
if (_freq > 5)_freq = 5;
else if (_freq < 0.1) _freq = 0.1;
if (_amp > 50)_amp = 50;
else if (_amp < 1) _amp = 1;
frequency = _freq;
amplitude = _amp;
}

View File

@ -9,12 +9,12 @@
/*
| PARAMS | `current_limit` | `acceleration` | `dce_kp` | `dce_kv` | `dce_ki` | `dce_kd` |
| ---------- | --------------- | -------------- | -------- | -------- | -------- | -------- |
| **Joint1** | 1 | 25 | 1000 | 80 | 300 | 250 |
| **Joint2** | 1.5 | 25 | 1000 | 250 | 300 | 200 |
| **Joint3** | 1 | 25 | 1000 | 350 | 500 | 250 |
| **Joint4** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
| **Joint5** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
| **Joint6** | 1.5 | 25 | 1000 | 350 | 500 | 250 |
| **Joint1** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint2** | 2 | 30 | 1000 | 80 | 200 | 200 |
| **Joint3** | 2 | 30 | 1500 | 80 | 200 | 250 |
| **Joint4** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint5** | 2 | 30 | 1000 | 80 | 200 | 250 |
| **Joint6** | 2 | 30 | 1000 | 80 | 200 | 250 |
*/
@ -64,28 +64,68 @@ public:
{
COMMAND_TARGET_POINT_SEQUENTIAL = 1,
COMMAND_TARGET_POINT_INTERRUPTABLE,
COMMAND_CONTINUES_TRAJECTORY
COMMAND_CONTINUES_TRAJECTORY,
COMMAND_MOTOR_TUNING
};
class TuningHelper
{
public:
explicit TuningHelper(DummyRobot* _context) : context(_context)
{
}
void SetTuningFlag(uint8_t _flag);
void Tick(uint32_t _timeMillis);
void SetFreqAndAmp(float _freq, float _amp);
// Communication protocol definitions
auto MakeProtocolDefinitions()
{
return make_protocol_member_list(
make_protocol_function("set_tuning_freq_amp", *this,
&TuningHelper::SetFreqAndAmp, "freq", "amp"),
make_protocol_function("set_tuning_flag", *this,
&TuningHelper::SetTuningFlag, "flag")
);
}
private:
DummyRobot* context;
float time = 0;
uint8_t tuningFlag = 0;
float frequency = 1;
float amplitude = 1;
};
TuningHelper tuningHelper = TuningHelper(this);
// This is the pose when power on.
const DOF6Kinematic::Joint6D_t REST_POSE = {0, -73, 180, 0, 0, 0};
const float DEFAULT_JOINT_SPEED = 30;
const float DEFAULT_JOINT_ACCELERATION = 50;
const float DEFAULT_JOINT_SPEED = 30; // degree/s
const DOF6Kinematic::Joint6D_t DEFAULT_JOINT_ACCELERATION_BASES = {150, 100, 200, 200, 200, 200};
const float DEFAULT_JOINT_ACCELERATION_LOW = 30; // 0~100
const float DEFAULT_JOINT_ACCELERATION_HIGH = 100; // 0~100
const CommandMode DEFAULT_COMMAND_MODE = COMMAND_TARGET_POINT_INTERRUPTABLE;
DOF6Kinematic::Joint6D_t currentJoints = REST_POSE;
DOF6Kinematic::Joint6D_t targetJoints = REST_POSE;
DOF6Kinematic::Joint6D_t initPose = REST_POSE;
DOF6Kinematic::Pose6D_t currentPose6D = {};
volatile uint8_t jointsStateFlag = 0b00000000;
CommandMode commandMode = COMMAND_TARGET_POINT_SEQUENTIAL;
bool isStopped = false;
CommandMode commandMode = DEFAULT_COMMAND_MODE;
CtrlStepMotor* motorJ[7] = {nullptr};
DummyHand* hand = {nullptr};
float MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
float MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
void MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
void MoveTrajectoryL(float _x, float _y, float _z, float _a, float _b, float _c);
void Init();
bool MoveJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6);
bool MoveL(float _x, float _y, float _z, float _a, float _b, float _c);
void MoveJoints(DOF6Kinematic::Joint6D_t _joints);
void SetJointSpeed(float _speed);
void SetJointAcceleration(float _acc);
void UpdateJointAngles();
@ -97,7 +137,8 @@ public:
void Homing();
void Resting();
bool IsMoving();
void SetCommandMode(uint8_t _mode);
bool IsEnabled();
void SetCommandMode(uint32_t _mode);
// Communication protocol definitions
@ -121,8 +162,8 @@ public:
make_protocol_function("move_l", *this, &DummyRobot::MoveL, "x", "y", "z", "a", "b", "c"),
make_protocol_function("set_joint_speed", *this, &DummyRobot::SetJointSpeed, "speed"),
make_protocol_function("set_joint_acc", *this, &DummyRobot::SetJointAcceleration, "acc"),
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode")
make_protocol_function("set_command_mode", *this, &DummyRobot::SetCommandMode, "mode"),
make_protocol_object("tuning", tuningHelper.MakeProtocolDefinitions())
);
}
@ -133,7 +174,6 @@ public:
explicit CommandHandler(DummyRobot* _context) : context(_context)
{
commandFifo = osMessageQueueNew(16, 64, nullptr);
commandLifo = osMessageQueueNew(1, 64, nullptr);
}
uint32_t Push(const std::string &_cmd);
@ -142,13 +182,11 @@ public:
uint32_t GetSpace();
void ClearFifo();
void EmergencyStop();
void Resume();
private:
DummyRobot* context;
osMessageQueueId_t commandFifo;
osMessageQueueId_t commandLifo;
char strBuffer[64]{};
};
CommandHandler commandHandler = CommandHandler(this);
@ -157,10 +195,10 @@ public:
private:
CAN_HandleTypeDef* hcan;
float jointSpeed = DEFAULT_JOINT_SPEED;
float jointSpeedRatio = 1;
DOF6Kinematic::Joint6D_t dynamicJointSpeeds = {1, 1, 1, 1, 1, 1};
DOF6Kinematic* dof6Solver;
float MoveJoints(DOF6Kinematic::Joint6D_t _joints);
bool isEnabled = false;
};

View File

@ -22,17 +22,27 @@ void ThreadControlLoopFixUpdate(void* argument)
// Suspended here until got Notification.
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
if (dummy.IsEnabled())
{
// Send control command to Motors & update Joint states
switch (dummy.commandMode)
{
case DummyRobot::COMMAND_TARGET_POINT_SEQUENTIAL:
case DummyRobot::COMMAND_TARGET_POINT_INTERRUPTABLE:
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
dummy.MoveJoints(dummy.targetJoints);
dummy.UpdateJointPose6D();
break;
case DummyRobot::COMMAND_MOTOR_TUNING:
dummy.tuningHelper.Tick(10);
dummy.UpdateJointPose6D();
break;
}
} else
{
// Just update Joint states
dummy.UpdateJointAngles();
dummy.UpdateJointPose6D();
break;
case DummyRobot::COMMAND_CONTINUES_TRAJECTORY:
// ToDo: handle trajectory, while update state at the mean time
dummy.UpdateJointPose6D();
break;
}
}
}
@ -53,14 +63,13 @@ void ThreadOledUpdate(void* argument)
{
uint32_t t = micros();
char buf[16];
char cmdModeNames[3][4] = {"SEQ", "INT", "TRJ"};
char cmdModeNames[4][4] = {"SEQ", "INT", "TRJ", "TUN"};
for (;;)
{
mpu6050.Update(true);
oled.clearBuffer();
oled.setFont(u8g2_font_5x8_tr);
oled.setCursor(0, 10);
oled.printf("IMU:%.3f/%.3f", mpu6050.data.ax, mpu6050.data.ay);
@ -92,10 +101,16 @@ void ThreadOledUpdate(void* argument)
oled.setFont(u8g2_font_10x20_tr);
oled.setCursor(0, 78);
if (dummy.IsEnabled())
{
for (int i = 1; i <= 6; i++)
buf[i - 1] = (dummy.jointsStateFlag & (1 << i) ? '*' : '_');
buf[6] = 0;
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], buf);
} else
{
oled.printf("[%s] %s", cmdModeNames[dummy.commandMode - 1], "======");
}
oled.sendBuffer();
}
@ -116,9 +131,12 @@ void OnTimer7Callback()
/* Default Entry -------------------------------------------------------*/
void Main(void)
{
// Init all communication staff, include USB-CDC/VCP/UART/CAN etc.
// Init all communication staff, including USB-CDC/VCP/UART/CAN etc.
InitCommunication();
// Init Robot.
dummy.Init();
// Init IMU.
do
{
@ -134,7 +152,7 @@ void Main(void)
// Init & Run User Threads.
const osThreadAttr_t controlLoopTask_attributes = {
.name = "ControlLoopFixUpdateTask",
.stack_size = 1000 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityRealtime,
};
controlLoopFixUpdateHandle = osThreadNew(ThreadControlLoopFixUpdate, nullptr,
@ -142,7 +160,7 @@ void Main(void)
const osThreadAttr_t ControlLoopUpdateTask_attributes = {
.name = "ControlLoopUpdateTask",
.stack_size = 1000 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal,
};
ControlLoopUpdateHandle = osThreadNew(ThreadControlLoopUpdate, nullptr,
@ -150,7 +168,7 @@ void Main(void)
const osThreadAttr_t oledTask_attributes = {
.name = "OledTask",
.stack_size = 1000 * 4,
.stack_size = 2000,
.priority = (osPriority_t) osPriorityNormal, // should >= Normal
};
oledTaskHandle = osThreadNew(ThreadOledUpdate, nullptr, &oledTask_attributes);
@ -160,5 +178,6 @@ void Main(void)
timerCtrlLoop.Start();
// System started, light switch-led up.
Respond(*uart4StreamOutputPtr, "[sys] Heap remain: %d Bytes\n", xPortGetMinimumEverFreeHeapSize());
pwm.SetDuty(PWM::CH_A1, 0.5);
}

View File

@ -6,42 +6,46 @@ extern DummyRobot dummy;
void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
if (_cmd[0] == '!' || dummy.isStopped)
if (_cmd[0] == '!' || !dummy.IsEnabled())
{
std::string s(_cmd);
if (s.find("STOP") != std::string::npos)
{
dummy.commandHandler.EmergencyStop();
Respond(_responseChannel, "!!!Stopped!!!");
} else if (s.find("RESUME") != std::string::npos)
Respond(_responseChannel, "Stopped ok");
} else if (s.find("START") != std::string::npos)
{
dummy.commandHandler.Resume();
Respond(_responseChannel, "Resumed");
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(*usbStreamOutputPtr, "JNTS %.2f %.2f %.2f %.2f %.2f %.2f",
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(*usbStreamOutputPtr, "POSE %.2f %.2f %.2f %.2f %.2f %.2f",
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)
{
int mode;
sscanf(_cmd, "CMDMODE %d", &mode);
uint32_t mode;
sscanf(_cmd, "#CMDMODE %lu", &mode);
dummy.SetCommandMode(mode);
Respond(*usbStreamOutputPtr, "Set command mode to [%d]", mode);
Respond(_responseChannel, "Set command mode to [%lu]", mode);
} else
Respond(*usbStreamOutputPtr, "ok");
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '>' || _cmd[0] == '@')
{
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
@ -55,61 +59,50 @@ void OnUsbAsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
void OnUart4AsciiCmd(const char* _cmd, size_t _len, StreamSink &_responseChannel)
{
/*---------------------------- ↓ Add Your CMDs Here ↓ -----------------------------*/
uint8_t argNum;
if (_cmd[0] == '#')
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, "JNTS %.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]);
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] == '>')
} else if (_cmd[0] == '>' || _cmd[0] == '@')
{
float joints[6];
float speed;
argNum = sscanf(_cmd, ">%f,%f,%f,%f,%f,%f,%f", joints, joints + 1, joints + 2,
joints + 3, joints + 4, joints + 5, &speed);
if (argNum == 6)
{
dummy.MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
} else if (argNum == 7)
{
dummy.SetJointSpeed(speed);
dummy.MoveJ(joints[0], joints[1], joints[2],
joints[3], joints[4], joints[5]);
}
while (dummy.IsMoving())
osDelay(10);
Respond(_responseChannel, "ok");
} else if (_cmd[0] == '@')
{
float pose[6];
float speed;
argNum = sscanf(_cmd, "@%f,%f,%f,%f,%f,%f,%f", pose, pose + 1, pose + 2,
pose + 3, pose + 4, pose + 5, &speed);
if (argNum == 6)
{
dummy.MoveL(pose[0], pose[1], pose[2],
pose[3], pose[4], pose[5]);
} else if (argNum == 7)
{
dummy.SetJointSpeed(speed);
dummy.MoveL(pose[0], pose[1], pose[2],
pose[3], pose[4], pose[5]);
}
while (dummy.IsMoving())
osDelay(10);
Respond(_responseChannel, "ok");
uint32_t freeSize = dummy.commandHandler.Push(_cmd);
Respond(_responseChannel, "%d", freeSize);
}
/*---------------------------- ↑ Add Your CMDs Here ↑ -----------------------------*/
}

View File

@ -8,24 +8,9 @@ class HelperFunctions
{
public:
/*--------------- 2.Add Your Helper Functions Helper Here (optional) ----------------*/
int32_t TestFunction(int32_t delta)
{
static int cnt = 0;
return cnt += delta;
}
void SaveConfigurationHelper()
{}
void EraseConfigurationHelper()
{}
float GetTemperatureHelper()
{ return AdcGetChipTemperature(); }
void SystemResetHelper()
{ NVIC_SystemReset(); }
} staticFunctions;
@ -36,6 +21,7 @@ static inline auto MakeObjTree()
return make_protocol_member_list(
// Add Read-Only Variables
make_protocol_ro_property("serial_number", &serialNumber),
make_protocol_function("get_temperature", staticFunctions, &HelperFunctions::GetTemperatureHelper),
make_protocol_object("robot", dummy.MakeProtocolDefinitions())
);
}