mirror of
https://gitee.com/peng_zhihui/Dummy-Robot
synced 2025-09-27 02:09:12 +08:00
[Fw] Update REF protocols.
This commit is contained in:
parent
f0e2c8da3a
commit
fe1007c668
@ -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;
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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,
|
||||
};
|
||||
|
||||
|
@ -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/*.*"
|
||||
|
@ -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/*.*"
|
||||
|
@ -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);
|
||||
|
@ -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 */
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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)
|
||||
|
@ -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},
|
||||
|
@ -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);
|
||||
|
||||
|
@ -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]);
|
||||
}
|
||||
|
||||
|
||||
void DummyRobot::MoveTrajectoryJ(float _j1, float _j2, float _j3, float _j4, float _j5, float _j6)
|
||||
{
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
@ -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;
|
||||
};
|
||||
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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 ↑ -----------------------------*/
|
||||
}
|
||||
|
@ -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())
|
||||
);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user