新控制方案，从头开始测试
This commit is contained in:
parent
e0e6b33791
commit
50d6c0b337
@ -73,8 +73,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
||||
User/device/motor_lz.c
|
||||
User/device/motor_rm.c
|
||||
User/device/dr16.c
|
||||
User/device/buzzer.c
|
||||
# User/module sources (C++)
|
||||
User/module/arm.cpp
|
||||
User/module/config.c
|
||||
User/module/chassis.c
|
||||
User/module/cmd/cmd.c
|
||||
|
||||
@ -32,12 +32,15 @@ extern "C" {
|
||||
|
||||
/* USER CODE END Includes */
|
||||
|
||||
extern TIM_HandleTypeDef htim4;
|
||||
|
||||
extern TIM_HandleTypeDef htim5;
|
||||
|
||||
/* USER CODE BEGIN Private defines */
|
||||
|
||||
/* USER CODE END Private defines */
|
||||
|
||||
void MX_TIM4_Init(void);
|
||||
void MX_TIM5_Init(void);
|
||||
|
||||
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
|
||||
|
||||
@ -98,6 +98,7 @@ int main(void)
|
||||
MX_CAN2_Init();
|
||||
MX_TIM5_Init();
|
||||
MX_USART3_UART_Init();
|
||||
MX_TIM4_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
/* USER CODE END 2 */
|
||||
|
||||
|
||||
103
Core/Src/tim.c
103
Core/Src/tim.c
@ -24,8 +24,63 @@
|
||||
|
||||
/* USER CODE END 0 */
|
||||
|
||||
TIM_HandleTypeDef htim4;
|
||||
TIM_HandleTypeDef htim5;
|
||||
|
||||
/* TIM4 init function */
|
||||
void MX_TIM4_Init(void)
|
||||
{
|
||||
|
||||
/* USER CODE BEGIN TIM4_Init 0 */
|
||||
|
||||
/* USER CODE END TIM4_Init 0 */
|
||||
|
||||
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
|
||||
TIM_MasterConfigTypeDef sMasterConfig = {0};
|
||||
TIM_OC_InitTypeDef sConfigOC = {0};
|
||||
|
||||
/* USER CODE BEGIN TIM4_Init 1 */
|
||||
|
||||
/* USER CODE END TIM4_Init 1 */
|
||||
htim4.Instance = TIM4;
|
||||
htim4.Init.Prescaler = 17-1;
|
||||
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
htim4.Init.Period = 2000;
|
||||
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
|
||||
if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
|
||||
if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
|
||||
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
|
||||
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
sConfigOC.OCMode = TIM_OCMODE_PWM1;
|
||||
sConfigOC.Pulse = 0;
|
||||
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
|
||||
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
/* USER CODE BEGIN TIM4_Init 2 */
|
||||
|
||||
/* USER CODE END TIM4_Init 2 */
|
||||
HAL_TIM_MspPostInit(&htim4);
|
||||
|
||||
}
|
||||
/* TIM5 init function */
|
||||
void MX_TIM5_Init(void)
|
||||
{
|
||||
@ -92,7 +147,18 @@ void MX_TIM5_Init(void)
|
||||
void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
|
||||
{
|
||||
|
||||
if(tim_baseHandle->Instance==TIM5)
|
||||
if(tim_baseHandle->Instance==TIM4)
|
||||
{
|
||||
/* USER CODE BEGIN TIM4_MspInit 0 */
|
||||
|
||||
/* USER CODE END TIM4_MspInit 0 */
|
||||
/* TIM4 clock enable */
|
||||
__HAL_RCC_TIM4_CLK_ENABLE();
|
||||
/* USER CODE BEGIN TIM4_MspInit 1 */
|
||||
|
||||
/* USER CODE END TIM4_MspInit 1 */
|
||||
}
|
||||
else if(tim_baseHandle->Instance==TIM5)
|
||||
{
|
||||
/* USER CODE BEGIN TIM5_MspInit 0 */
|
||||
|
||||
@ -112,7 +178,27 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
|
||||
{
|
||||
|
||||
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
||||
if(timHandle->Instance==TIM5)
|
||||
if(timHandle->Instance==TIM4)
|
||||
{
|
||||
/* USER CODE BEGIN TIM4_MspPostInit 0 */
|
||||
|
||||
/* USER CODE END TIM4_MspPostInit 0 */
|
||||
__HAL_RCC_GPIOD_CLK_ENABLE();
|
||||
/**TIM4 GPIO Configuration
|
||||
PD14 ------> TIM4_CH3
|
||||
*/
|
||||
GPIO_InitStruct.Pin = GPIO_PIN_14;
|
||||
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
||||
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
||||
GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
|
||||
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
|
||||
|
||||
/* USER CODE BEGIN TIM4_MspPostInit 1 */
|
||||
|
||||
/* USER CODE END TIM4_MspPostInit 1 */
|
||||
}
|
||||
else if(timHandle->Instance==TIM5)
|
||||
{
|
||||
/* USER CODE BEGIN TIM5_MspPostInit 0 */
|
||||
|
||||
@ -141,7 +227,18 @@ void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
|
||||
void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
|
||||
{
|
||||
|
||||
if(tim_baseHandle->Instance==TIM5)
|
||||
if(tim_baseHandle->Instance==TIM4)
|
||||
{
|
||||
/* USER CODE BEGIN TIM4_MspDeInit 0 */
|
||||
|
||||
/* USER CODE END TIM4_MspDeInit 0 */
|
||||
/* Peripheral clock disable */
|
||||
__HAL_RCC_TIM4_CLK_DISABLE();
|
||||
/* USER CODE BEGIN TIM4_MspDeInit 1 */
|
||||
|
||||
/* USER CODE END TIM4_MspDeInit 1 */
|
||||
}
|
||||
else if(tim_baseHandle->Instance==TIM5)
|
||||
{
|
||||
/* USER CODE BEGIN TIM5_MspDeInit 0 */
|
||||
|
||||
|
||||
@ -28,6 +28,7 @@ static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
|
||||
{&htim5, TIM_CHANNEL_1},
|
||||
{&htim5, TIM_CHANNEL_2},
|
||||
{&htim5, TIM_CHANNEL_3},
|
||||
{&htim4, TIM_CHANNEL_3},
|
||||
};
|
||||
|
||||
/* Private function -------------------------------------------------------- */
|
||||
|
||||
@ -26,6 +26,7 @@ typedef enum {
|
||||
BSP_PWM_TIM5_CH1,
|
||||
BSP_PWM_TIM5_CH2,
|
||||
BSP_PWM_TIM5_CH3,
|
||||
BSP_PWM_BUZZER,
|
||||
BSP_PWM_NUM,
|
||||
BSP_PWM_ERR,
|
||||
} BSP_PWM_Channel_t;
|
||||
|
||||
@ -200,21 +200,21 @@ int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAng
|
||||
return -1;
|
||||
}
|
||||
|
||||
// // 姿态误差：计算两旋转矩阵的测地距离 theta = arccos((trace(R_v * R_t^T) - 1) / 2)
|
||||
// // 使用旋转矩阵方法避免 RPY 奇异点和角度折叠问题
|
||||
// Matrixf<3, 3> R_verify = robotics::t2r(T_verify);
|
||||
// Matrixf<3, 3> R_diff = R_verify * R_target.trans();
|
||||
// float trace_val = R_diff[0][0] + R_diff[1][1] + R_diff[2][2];
|
||||
// float cos_theta = (trace_val - 1.0f) * 0.5f;
|
||||
// // 数值钳位防止 acosf 域溢出
|
||||
// if (cos_theta > 1.0f) cos_theta = 1.0f;
|
||||
// if (cos_theta < -1.0f) cos_theta = -1.0f;
|
||||
// float ang_error = acosf(cos_theta); // [0, π]
|
||||
// 姿态误差：计算两旋转矩阵的测地距离 theta = arccos((trace(R_v * R_t^T) - 1) / 2)
|
||||
// 使用旋转矩阵方法避免 RPY 奇异点和角度折叠问题
|
||||
Matrixf<3, 3> R_verify = robotics::t2r(T_verify);
|
||||
Matrixf<3, 3> R_diff = R_verify * R_target.trans();
|
||||
float trace_val = R_diff[0][0] + R_diff[1][1] + R_diff[2][2];
|
||||
float cos_theta = (trace_val - 1.0f) * 0.5f;
|
||||
// 数值钳位防止 acosf 域溢出
|
||||
if (cos_theta > 1.0f) cos_theta = 1.0f;
|
||||
if (cos_theta < -1.0f) cos_theta = -1.0f;
|
||||
float ang_error = acosf(cos_theta); // [0, π]
|
||||
|
||||
// const float ang_tol = 0.1f; // 姿态收敛阈值 (rad ≈ 5.7°)
|
||||
// if (ang_error > ang_tol) { // 姿态未收敛（收敛到错误分支）
|
||||
// return -1;
|
||||
// }
|
||||
const float ang_tol = 0.1f; // 姿态收敛阈值 (rad ≈ 5.7°)
|
||||
if (ang_error > ang_tol) { // 姿态未收敛（收敛到错误分支或近奇异点）
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -27,13 +27,15 @@ typedef struct {
|
||||
float m; // 连杆质量 (kg)
|
||||
float rc[3]; // 质心在 DH 连杆坐标系下的坐标 (m): {x, y, z}
|
||||
// 由 URDF inertial/origin 经 Rz(-theta_offset) 旋转得到
|
||||
|
||||
} Arm6dof_DHParams_t;
|
||||
typedef struct {
|
||||
// MIT控制参数（可选，如不设置则使用默认值）
|
||||
float kp; // 位置刚度 (N·m/rad)，默认值见Joint类
|
||||
float kd; // 阻尼系数 (N·m·s/rad)，默认值见Joint类
|
||||
} Arm6dof_DHParams_t;
|
||||
} Arm6dof_MotorMTICtrlParams_t;
|
||||
typedef struct {
|
||||
Arm6dof_DHParams_t DH_params[6];
|
||||
Arm6dof_MotorMTICtrlParams_t motor_ctrl_params[6]; // 每个关节的MIT控制参数（可选）
|
||||
struct {
|
||||
float x, y, z; // 工具中心偏移 (m)
|
||||
float roll, pitch, yaw; // 工具姿态偏移 (rad)
|
||||
@ -65,7 +67,7 @@ typedef struct {
|
||||
bool enable; /* 使能开关 */
|
||||
bool set_target_as_current; /* true: 将目标位姿同步为当前实际位姿 */
|
||||
Arm_CtrlType_t ctrl_type; /* 控制类型 */
|
||||
Arm6dof_Pose_t target_pose; /* 目标末端位姿（遥控器模式，笛卡尔空间） */
|
||||
Arm6dof_Pose_t target_pose; /* 目标末端位姿（世界坐标系，供调试观察） */
|
||||
Arm6dof_JointAngles_t target_joints; /* 目标六个关节角度（自定义控制器模式） */
|
||||
} Arm_CMD_t;
|
||||
/**
|
||||
|
||||
@ -325,17 +325,14 @@ robotics::Link& robotics::Link::operator=(Link link) {
|
||||
}
|
||||
|
||||
Matrixf<4, 4> robotics::Link::T(float q) {
|
||||
// 先对关节变量q进行限幅
|
||||
float q_limited = q;
|
||||
if (qmin_ < qmax_) { // 有效的限幅范围
|
||||
q_limited = math::limit(q, qmin_, qmax_);
|
||||
}
|
||||
|
||||
// 再计算实际的DH参数
|
||||
// 纯 DH 运动学，不做关节限位截断。
|
||||
// 限位检查在上层（StepTrajectory / PositionControl）执行；
|
||||
// 若在此截断会导致迭代中 FK 出现平台区（超界后输出不变），
|
||||
// 使 Jacobian 为零，LM 算法完全失去收敛方向。
|
||||
if (type_ == R) {
|
||||
dh_.theta = q_limited + offset_;
|
||||
dh_.theta = q + offset_;
|
||||
} else {
|
||||
dh_.d = q_limited + offset_;
|
||||
dh_.d = q + offset_;
|
||||
}
|
||||
|
||||
return dh_.fkine();
|
||||
|
||||
@ -258,10 +258,18 @@ class Serial_Link {
|
||||
|
||||
// 评估新误差
|
||||
Matrixf<_n, 1> q_new = q + dq;
|
||||
// 关节角折叠到有效区间
|
||||
// 仅对真实循环关节（qmin≈0, qmax≈2π）做 loopLimit 折叠，消除周期歧义。
|
||||
// 其余非循环关节（J2/J3/J5等小范围）不折叠：一旦超界 loopLimit 会产生大跳变
|
||||
// （例如 J3 在 3.0 边界被折到 -1.0），破坏 LM 迭代连续性导致无法收敛。
|
||||
// 注：Link::T() 不再做任何截断，迭代过程允许 q 暂时超界以保持连续的 Jacobian；
|
||||
// 最终关节超限的检查在 StepTrajectory() 的限位验证中完成。
|
||||
for (int i = 0; i < _n; i++) {
|
||||
if (links_[i].type() == R)
|
||||
q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_);
|
||||
if (links_[i].type() == R) {
|
||||
constexpr float TWO_PI = 2.0f * (float)M_PI;
|
||||
bool is_cyclic = (links_[i].qmin_ < 0.1f) && (links_[i].qmax_ > TWO_PI - 0.1f);
|
||||
if (is_cyclic)
|
||||
q_new[i][0] = math::loopLimit(q_new[i][0], links_[i].qmin_, links_[i].qmax_);
|
||||
}
|
||||
}
|
||||
T = fkine(q_new);
|
||||
pe = t2p(Td) - t2p(T);
|
||||
|
||||
187
User/device/buzzer.c
Normal file
187
User/device/buzzer.c
Normal file
@ -0,0 +1,187 @@
|
||||
#include "device/buzzer.h"
|
||||
#include "bsp/time.h"
|
||||
#include <math.h>
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* USER DEFINE BEGIN */
|
||||
|
||||
/* USER DEFINE END */
|
||||
|
||||
#define MUSIC_DEFAULT_VOLUME 0.5f
|
||||
#define MUSIC_A4_FREQ 440.0f // A4音符频率
|
||||
|
||||
/* USER MUSIC MENU BEGIN */
|
||||
// RM音乐
|
||||
const Tone_t RM[] = {
|
||||
{NOTE_B, 5, 200},
|
||||
{NOTE_G, 4, 200},
|
||||
{NOTE_B, 5, 400},
|
||||
{NOTE_G, 4, 200},
|
||||
{NOTE_B, 5, 400},
|
||||
{NOTE_G, 4, 200},
|
||||
{NOTE_D, 5, 400},
|
||||
{NOTE_G, 4, 200},
|
||||
{NOTE_C, 5, 200},
|
||||
{NOTE_C, 5, 200},
|
||||
{NOTE_G, 4, 200},
|
||||
{NOTE_B, 5, 200},
|
||||
{NOTE_C, 5, 200}
|
||||
};
|
||||
|
||||
// Nokia 经典铃声音符
|
||||
const Tone_t NOKIA[] = {
|
||||
{NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250},
|
||||
{NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250},
|
||||
{NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
|
||||
{NOTE_A, 4, 500}
|
||||
};
|
||||
|
||||
// 蔡徐坤 - 只因你太美(副歌高潮段落)
|
||||
const Tone_t JNTM[] = {
|
||||
{NOTE_E, 5, 150}, {NOTE_D, 5, 150}, {NOTE_E, 5, 150}, {NOTE_C, 5, 150}, {NOTE_D, 5, 300}, // 只 因 你 太 美
|
||||
{NOTE_C, 5, 150}, {NOTE_A, 4, 300}, {NOTE_REST, 0, 150}, // ba by ~
|
||||
{NOTE_E, 5, 150}, {NOTE_D, 5, 150}, {NOTE_E, 5, 150}, {NOTE_C, 5, 150}, {NOTE_D, 5, 300}, // 只 因 你 太 美
|
||||
{NOTE_C, 5, 150}, {NOTE_A, 4, 300}, {NOTE_REST, 0, 150}, // ba by ~
|
||||
{NOTE_E, 5, 150}, {NOTE_D, 5, 150}, {NOTE_E, 5, 150}, {NOTE_C, 5, 150}, // 只 因 你 实
|
||||
{NOTE_D, 5, 150}, {NOTE_E, 5, 150}, {NOTE_D, 5, 150}, {NOTE_C, 5, 150}, {NOTE_D, 5, 300}, // 在 是 太 美
|
||||
{NOTE_C, 5, 150}, {NOTE_A, 4, 300}, {NOTE_REST, 0, 150}, // ba by ~
|
||||
{NOTE_E, 5, 150}, {NOTE_D, 5, 150}, {NOTE_E, 5, 150}, {NOTE_C, 5, 150}, {NOTE_D, 5, 500} // 只 因 你 太 美 ~
|
||||
};
|
||||
/* USER MUSIC MENU END */
|
||||
|
||||
static void BUZZER_Update(BUZZER_t *buzzer){
|
||||
buzzer->header.online = true;
|
||||
buzzer->header.last_online_time = BSP_TIME_Get_ms();
|
||||
}
|
||||
|
||||
// 根据音符和八度计算频率的辅助函数
|
||||
static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) {
|
||||
if (note == NOTE_REST) {
|
||||
return 0.0f; // 休止符返回0频率
|
||||
}
|
||||
|
||||
// 将音符和八度转换为MIDI音符编号
|
||||
int midi_num = (int)note + (int)((octave + 1) * 12);
|
||||
|
||||
// 使用A4 (440Hz) 作为参考，计算频率
|
||||
// 公式: freq = 440 * 2^((midi_num - 69)/12)
|
||||
float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f);
|
||||
|
||||
return freq;
|
||||
}
|
||||
|
||||
// 播放单个音符
|
||||
static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) {
|
||||
if (buzzer == NULL || !buzzer->header.online)
|
||||
return DEVICE_ERR;
|
||||
|
||||
float freq = BUZZER_CalcFreq(note, octave);
|
||||
|
||||
if (freq > 0.0f) {
|
||||
// 播放音符
|
||||
if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK)
|
||||
return DEVICE_ERR;
|
||||
|
||||
if (BUZZER_Start(buzzer) != DEVICE_OK)
|
||||
return DEVICE_ERR;
|
||||
} else {
|
||||
// 休止符，停止播放
|
||||
BUZZER_Stop(buzzer);
|
||||
}
|
||||
|
||||
// 等待指定时间
|
||||
BSP_TIME_Delay_ms(duration_ms);
|
||||
|
||||
// 停止当前音符，为下一个音符做准备
|
||||
BUZZER_Stop(buzzer);
|
||||
BSP_TIME_Delay_ms(20); // 短暂间隔
|
||||
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
|
||||
if (buzzer == NULL) return DEVICE_ERR;
|
||||
|
||||
buzzer->channel = channel;
|
||||
buzzer->header.online = true;
|
||||
|
||||
BUZZER_Stop(buzzer);
|
||||
|
||||
return DEVICE_OK ;
|
||||
}
|
||||
|
||||
int8_t BUZZER_Start(BUZZER_t *buzzer) {
|
||||
if (buzzer == NULL || !buzzer->header.online)
|
||||
return DEVICE_ERR;
|
||||
BUZZER_Update(buzzer);
|
||||
return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ?
|
||||
DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
int8_t BUZZER_Stop(BUZZER_t *buzzer) {
|
||||
if (buzzer == NULL || !buzzer->header.online)
|
||||
return DEVICE_ERR;
|
||||
BUZZER_Update(buzzer);
|
||||
return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ?
|
||||
DEVICE_OK : DEVICE_ERR;
|
||||
}
|
||||
|
||||
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
|
||||
if (buzzer == NULL || !buzzer->header.online)
|
||||
return DEVICE_ERR;
|
||||
|
||||
int result = DEVICE_OK ;
|
||||
BUZZER_Update(buzzer);
|
||||
if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK)
|
||||
result = DEVICE_ERR;
|
||||
|
||||
if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK)
|
||||
result = DEVICE_ERR;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) {
|
||||
if (buzzer == NULL || !buzzer->header.online)
|
||||
return DEVICE_ERR;
|
||||
|
||||
const Tone_t *melody = NULL;
|
||||
size_t melody_length = 0;
|
||||
|
||||
// 根据音乐类型选择对应的音符数组
|
||||
switch (music) {
|
||||
case MUSIC_RM:
|
||||
melody = RM;
|
||||
melody_length = sizeof(RM) / sizeof(Tone_t);
|
||||
break;
|
||||
case MUSIC_NOKIA:
|
||||
melody = NOKIA;
|
||||
melody_length = sizeof(NOKIA) / sizeof(Tone_t);
|
||||
break;
|
||||
case MUSIC_JNTM:
|
||||
melody = JNTM;
|
||||
melody_length = sizeof(JNTM) / sizeof(Tone_t);
|
||||
break;
|
||||
default:
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
|
||||
// 播放整首音乐
|
||||
for (size_t i = 0; i < melody_length; i++) {
|
||||
if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) {
|
||||
BUZZER_Stop(buzzer); // 出错时停止播放
|
||||
return DEVICE_ERR;
|
||||
}
|
||||
}
|
||||
|
||||
// 音乐播放完成后停止
|
||||
BUZZER_Stop(buzzer);
|
||||
return DEVICE_OK;
|
||||
}
|
||||
|
||||
/* USER FUNCTION BEGIN */
|
||||
|
||||
/* USER FUNCTION END */
|
||||
139
User/device/buzzer.h
Normal file
139
User/device/buzzer.h
Normal file
@ -0,0 +1,139 @@
|
||||
/**
|
||||
* @file buzzer.h
|
||||
* @brief 蜂鸣器设备驱动头文件
|
||||
* @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放
|
||||
* @author Generated by STM32CubeMX
|
||||
* @date 2025年10月23日
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* Includes ----------------------------------------------------------------- */
|
||||
#include "bsp/pwm.h" // PWM底层硬件抽象层
|
||||
#include "device.h" // 设备通用头文件
|
||||
#include <stddef.h> // 标准定义
|
||||
#include <stdint.h> // 标准整型定义
|
||||
|
||||
/* USER INCLUDE BEGIN */
|
||||
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Exported constants ------------------------------------------------------- */
|
||||
|
||||
/* USER DEFINE BEGIN */
|
||||
|
||||
/* USER DEFINE END */
|
||||
|
||||
/* Exported types ----------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 音符枚举类型
|
||||
* @details 定义标准十二平均律音符，用于音乐播放
|
||||
*/
|
||||
typedef enum {
|
||||
NOTE_C = 0, ///< Do音符
|
||||
NOTE_CS = 1, ///< Do#音符 (升Do)
|
||||
NOTE_D = 2, ///< Re音符
|
||||
NOTE_DS = 3, ///< Re#音符 (升Re)
|
||||
NOTE_E = 4, ///< Mi音符
|
||||
NOTE_F = 5, ///< Fa音符
|
||||
NOTE_FS = 6, ///< Fa#音符 (升Fa)
|
||||
NOTE_G = 7, ///< Sol音符
|
||||
NOTE_GS = 8, ///< Sol#音符 (升Sol)
|
||||
NOTE_A = 9, ///< La音符
|
||||
NOTE_AS = 10, ///< La#音符 (升La)
|
||||
NOTE_B = 11, ///< Si音符
|
||||
NOTE_REST = 255 ///< 休止符 (无声音)
|
||||
} NOTE_t;
|
||||
|
||||
/**
|
||||
* @brief 音调结构体
|
||||
* @details 定义一个完整的音调信息，包括音符、八度和持续时间
|
||||
*/
|
||||
typedef struct {
|
||||
NOTE_t note; ///< 音符名称 (使用NOTE_t枚举)
|
||||
uint8_t octave; ///< 八度 (0-8，通常使用3-7)
|
||||
uint16_t duration_ms; ///< 持续时间，单位毫秒
|
||||
} Tone_t;
|
||||
|
||||
/**
|
||||
* @brief 预设音乐枚举类型
|
||||
* @details 定义可播放的预设音乐类型
|
||||
*/
|
||||
typedef enum {
|
||||
/* USER MUSIC MENU BEGIN */
|
||||
MUSIC_RM, ///< RM战队音乐
|
||||
MUSIC_NOKIA, ///< 诺基亚经典铃声
|
||||
MUSIC_JNTM, ///< 只因你太美
|
||||
/* USER MUSIC MENU END */
|
||||
} MUSIC_t;
|
||||
|
||||
/**
|
||||
* @brief 蜂鸣器设备结构体
|
||||
* @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道
|
||||
*/
|
||||
typedef struct {
|
||||
DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等)
|
||||
BSP_PWM_Channel_t channel; ///< PWM输出通道
|
||||
} BUZZER_t;
|
||||
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @brief 初始化蜂鸣器设备
|
||||
* @param buzzer 蜂鸣器设备结构体指针
|
||||
* @param channel PWM输出通道
|
||||
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
|
||||
* @note 初始化后蜂鸣器处于停止状态
|
||||
*/
|
||||
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
|
||||
|
||||
/**
|
||||
* @brief 启动蜂鸣器播放
|
||||
* @param buzzer 蜂鸣器设备结构体指针
|
||||
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
|
||||
* @note 需要先调用BUZZER_Set设置频率和占空比
|
||||
*/
|
||||
int8_t BUZZER_Start(BUZZER_t *buzzer);
|
||||
|
||||
/**
|
||||
* @brief 停止蜂鸣器播放
|
||||
* @param buzzer 蜂鸣器设备结构体指针
|
||||
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
|
||||
*/
|
||||
int8_t BUZZER_Stop(BUZZER_t *buzzer);
|
||||
|
||||
/**
|
||||
* @brief 设置蜂鸣器频率和占空比
|
||||
* @param buzzer 蜂鸣器设备结构体指针
|
||||
* @param freq 频率 (Hz)，通常范围20Hz-20kHz
|
||||
* @param duty_cycle 占空比 (0.0-1.0)，影响音量大小
|
||||
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
|
||||
* @note 设置后需要调用BUZZER_Start才能听到声音
|
||||
*/
|
||||
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
|
||||
|
||||
/**
|
||||
* @brief 播放预设音乐
|
||||
* @param buzzer 蜂鸣器设备结构体指针
|
||||
* @param music 音乐类型 (使用MUSIC_t枚举)
|
||||
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
|
||||
* @note 这是一个阻塞函数，会播放完整首音乐后返回
|
||||
*/
|
||||
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
|
||||
|
||||
/* USER FUNCTION BEGIN */
|
||||
|
||||
/* USER FUNCTION END */
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -1,507 +0,0 @@
|
||||
|
||||
|
||||
// /* Includes ----------------------------------------------------------------- */
|
||||
// #include <math.h>
|
||||
// #include <stdint.h>
|
||||
// #include <string.h>
|
||||
// #include "arm.h"
|
||||
// #include "bsp/mm.h"
|
||||
// #include "bsp/time.h"
|
||||
// #include "component/user_math.h"
|
||||
// #include "device/motor_dm.h"
|
||||
// #include "device/motor_lz.h"
|
||||
// /* Private typedef ---------------------------------------------------------- */
|
||||
// /* Private define ----------------------------------------------------------- */
|
||||
|
||||
// /* Private macro ------------------------------------------------------------ */
|
||||
// /* Private variables -------------------------------------------------------- */
|
||||
|
||||
// /* Private function -------------------------------------------------------- */
|
||||
|
||||
// // ============================================================================
|
||||
// // Joint（关节）级别操作 - 面向对象接口
|
||||
// // ============================================================================
|
||||
|
||||
// /**
|
||||
// * @brief 初始化单个关节
|
||||
// */
|
||||
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
|
||||
// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq) {
|
||||
// if (joint == NULL || dh_params == NULL || pid_params == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// joint->id = id;
|
||||
// joint->motor_type = motor_type;
|
||||
// memcpy(&joint->dh_params, dh_params, sizeof(Arm6dof_DHParams_t));
|
||||
// joint->q_offset = 0.0f;
|
||||
|
||||
// PID_Init(&joint->pid, KPID_MODE_CALC_D, freq, pid_params);
|
||||
|
||||
// joint->state.current_angle = 0.0f;
|
||||
// joint->state.current_velocity = 0.0f;
|
||||
// joint->state.current_torque = 0.0f;
|
||||
// joint->state.target_angle = 0.0f;
|
||||
// joint->state.target_velocity = 0.0f;
|
||||
// joint->state.online = false;
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 更新关节状态
|
||||
// */
|
||||
// int8_t Joint_Update(Joint_t *joint) {
|
||||
// if (joint == NULL) return -1;
|
||||
|
||||
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
|
||||
// joint->state.current_angle = joint->motor.lz_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
|
||||
// joint->state.current_velocity = joint->motor.lz_motor.motor.feedback.rotor_speed;
|
||||
// joint->state.current_torque = joint->motor.lz_motor.motor.feedback.torque_current;
|
||||
// joint->state.online = joint->motor.lz_motor.motor.header.online;
|
||||
// } else {
|
||||
// joint->state.current_angle = joint->motor.dm_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
|
||||
// joint->state.current_velocity = joint->motor.dm_motor.motor.feedback.rotor_speed;
|
||||
// joint->state.current_torque = joint->motor.dm_motor.motor.feedback.torque_current;
|
||||
// joint->state.online = joint->motor.dm_motor.motor.header.online;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 关节位置控制
|
||||
// */
|
||||
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt) {
|
||||
// if (joint == NULL) return -1;
|
||||
|
||||
// if (target_angle < joint->dh_params.qmin || target_angle > joint->dh_params.qmax) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// joint->state.target_angle = target_angle;
|
||||
// float pid_output = PID_Calc(&joint->pid, target_angle, joint->state.current_angle, 0, dt);
|
||||
|
||||
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
|
||||
// joint->output.lz_output.target_angle = target_angle + joint->q_offset;
|
||||
// joint->output.lz_output.target_velocity = 0.0f;
|
||||
// joint->output.lz_output.kp = 10.0f;
|
||||
// joint->output.lz_output.kd = 0.5f;
|
||||
// joint->output.lz_output.torque = pid_output;
|
||||
// MOTOR_LZ_MotionControl(&joint->motor_params.lz_params, &joint->output.lz_output);
|
||||
// } else {
|
||||
// joint->output.dm_output.angle = target_angle + joint->q_offset;
|
||||
// joint->output.dm_output.velocity = 0.0f;
|
||||
// joint->output.dm_output.kp = 50.0f;
|
||||
// joint->output.dm_output.kd = 3.0f;
|
||||
// joint->output.dm_output.torque = pid_output;
|
||||
// MOTOR_DM_MITCtrl(&joint->motor_params.dm_params, &joint->output.dm_output);
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 关节松弛
|
||||
// */
|
||||
// int8_t Joint_Relax(Joint_t *joint) {
|
||||
// if (joint == NULL) return -1;
|
||||
|
||||
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
|
||||
// MOTOR_LZ_Relax(&joint->motor_params.lz_params);
|
||||
// } else {
|
||||
// MOTOR_DM_Relax(&joint->motor_params.dm_params);
|
||||
// }
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 检查关节是否到达目标
|
||||
// */
|
||||
// bool Joint_IsReached(const Joint_t *joint, float tolerance) {
|
||||
// if (joint == NULL) return false;
|
||||
// return fabsf(joint->state.target_angle - joint->state.current_angle) < tolerance;
|
||||
// }
|
||||
|
||||
// // ============================================================================
|
||||
// // Arm（机械臂）级别操作
|
||||
// // ============================================================================
|
||||
|
||||
// /* Exported functions ------------------------------------------------------- */
|
||||
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq) {
|
||||
// if (arm == NULL||arm_param == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
// arm->param = arm_param;
|
||||
|
||||
// BSP_CAN_Init();
|
||||
|
||||
// MOTOR_LZ_Init();
|
||||
|
||||
|
||||
// // 初始化控制参数
|
||||
// arm->control.mode = ARM_MODE_IDLE;
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
// arm->control.position_tolerance = 0.02f; // 关节角容差：0.02rad ≈ 1.15度
|
||||
// arm->control.velocity_tolerance = 0.01f; // 速度容差：0.01rad/s
|
||||
// arm->control.enable = false;
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// int8_t Arm_Updata(Arm_t *arm) {
|
||||
// if (arm == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
|
||||
// MOTOR_LZ_Update(&arm->param->jointMotor1_params);
|
||||
// MOTOR_LZ_Update(&arm->param->jointMotor2_params);
|
||||
// MOTOR_LZ_Update(&arm->param->jointMotor3_params);
|
||||
// MOTOR_DM_Update(&arm->param->jointMotor4_params);
|
||||
// MOTOR_DM_Update(&arm->param->jointMotor5_params);
|
||||
// MOTOR_DM_Update(&arm->param->jointMotor6_params);
|
||||
|
||||
// // 同步电机实例数据到actuator
|
||||
// MOTOR_LZ_t *motor1 = MOTOR_LZ_GetMotor(&arm->param->jointMotor1_params);
|
||||
// if (motor1 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor1, motor1, sizeof(MOTOR_LZ_t));
|
||||
// }
|
||||
|
||||
// MOTOR_LZ_t *motor2 = MOTOR_LZ_GetMotor(&arm->param->jointMotor2_params);
|
||||
// if (motor2 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor2, motor2, sizeof(MOTOR_LZ_t));
|
||||
// }
|
||||
|
||||
// MOTOR_LZ_t *motor3 = MOTOR_LZ_GetMotor(&arm->param->jointMotor3_params);
|
||||
// if (motor3 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor3, motor3, sizeof(MOTOR_LZ_t));
|
||||
// }
|
||||
|
||||
// MOTOR_DM_t *motor4 = MOTOR_DM_GetMotor(&arm->param->jointMotor4_params);
|
||||
// if (motor4 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor4, motor4, sizeof(MOTOR_DM_t));
|
||||
// }
|
||||
|
||||
// MOTOR_DM_t *motor5 = MOTOR_DM_GetMotor(&arm->param->jointMotor5_params);
|
||||
// if (motor5 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor5, motor5, sizeof(MOTOR_DM_t));
|
||||
// }
|
||||
|
||||
// MOTOR_DM_t *motor6 = MOTOR_DM_GetMotor(&arm->param->jointMotor6_params);
|
||||
// if (motor6 != NULL) {
|
||||
// memcpy(&arm->actuator.jointMotor6, motor6, sizeof(MOTOR_DM_t));
|
||||
// }
|
||||
|
||||
// // 从电机反馈更新当前关节角度
|
||||
// arm->stateVariable.currentJointAngles.q[0] =
|
||||
// arm->actuator.jointMotor1.motor.feedback.rotor_abs_angle - arm->param->q_offset[0];
|
||||
// arm->stateVariable.currentJointAngles.q[1] =
|
||||
// arm->actuator.jointMotor2.motor.feedback.rotor_abs_angle - arm->param->q_offset[1];
|
||||
// arm->stateVariable.currentJointAngles.q[2] =
|
||||
// arm->actuator.jointMotor3.motor.feedback.rotor_abs_angle - arm->param->q_offset[2];
|
||||
// arm->stateVariable.currentJointAngles.q[3] =
|
||||
// arm->actuator.jointMotor4.motor.feedback.rotor_abs_angle - arm->param->q_offset[3];
|
||||
// arm->stateVariable.currentJointAngles.q[4] =
|
||||
// arm->actuator.jointMotor5.motor.feedback.rotor_abs_angle - arm->param->q_offset[4];
|
||||
// arm->stateVariable.currentJointAngles.q[5] =
|
||||
// arm->actuator.jointMotor6.motor.feedback.rotor_abs_angle - arm->param->q_offset[5];
|
||||
|
||||
// // 正运动学：计算当前末端位姿
|
||||
// Arm6dof_ForwardKinematics(&arm->stateVariable.currentJointAngles,
|
||||
// &arm->stateVariable.currentEndPose);
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
// int8_t Arm_Ctrl(Arm_t *arm) {
|
||||
// if (arm == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// arm->timer.now = BSP_TIME_Get_us() / 1000000.0f;
|
||||
// arm->timer.dt = (BSP_TIME_Get_us() - arm->timer.last_wakeup) / 1000000.0f;
|
||||
// arm->timer.last_wakeup = BSP_TIME_Get_us();
|
||||
|
||||
// // 如果未使能，松弛所有电机
|
||||
// if (!arm->control.enable) {
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor4_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor5_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor6_params);
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// // 根据控制模式执行不同的控制策略
|
||||
// switch (arm->control.mode) {
|
||||
// case ARM_MODE_IDLE:
|
||||
// // 空闲模式：电机松弛
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
|
||||
// MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor4_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor5_params);
|
||||
// MOTOR_DM_Relax(&arm->param->jointMotor6_params);
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
// break;
|
||||
|
||||
// case ARM_MODE_JOINT_POSITION:
|
||||
// case ARM_MODE_CARTESIAN_POSITION:
|
||||
// // 关节位置控制（关节空间和笛卡尔空间最终都转为关节角控制）
|
||||
// {
|
||||
// // 检查是否到达目标
|
||||
// bool reached = true;
|
||||
// float max_error = 0.0f;
|
||||
|
||||
// for (int i = 0; i < 6; i++) {
|
||||
// float error = arm->stateVariable.targetJointAngles.q[i] -
|
||||
// arm->stateVariable.currentJointAngles.q[i];
|
||||
// if (fabsf(error) > arm->control.position_tolerance) {
|
||||
// reached = false;
|
||||
// }
|
||||
// if (fabsf(error) > max_error) {
|
||||
// max_error = fabsf(error);
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (reached) {
|
||||
// arm->control.state = ARM_STATE_REACHED;
|
||||
// } else {
|
||||
// arm->control.state = ARM_STATE_MOVING;
|
||||
// }
|
||||
|
||||
// // PID位置控制（关节1-3使用LZ电机）
|
||||
// for (int i = 0; i < 3; i++) {
|
||||
// float pid_output = PID_Calc(&arm->controller.joint_pid[i],
|
||||
// arm->stateVariable.targetJointAngles.q[i],
|
||||
// arm->stateVariable.currentJointAngles.q[i],
|
||||
// 0,
|
||||
// arm->timer.dt
|
||||
// );
|
||||
|
||||
// MOTOR_LZ_MotionParam_t* output = NULL;
|
||||
// if (i == 0) output = &arm->output.jointMotor1_output;
|
||||
// else if (i == 1) output = &arm->output.jointMotor2_output;
|
||||
// else output = &arm->output.jointMotor3_output;
|
||||
|
||||
// output->target_angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
|
||||
// output->target_velocity = 0.0f; // 位置模式速度由PID控制
|
||||
// output->kp = 10.0f; // 可调参数
|
||||
// output->kd = 0.5f;
|
||||
// output->torque = pid_output;
|
||||
// }
|
||||
|
||||
// // MIT模式控制（关节4-6使用DM电机）
|
||||
// for (int i = 3; i < 6; i++) {
|
||||
// float pid_output = PID_Calc(&arm->controller.joint_pid[i],
|
||||
// arm->stateVariable.targetJointAngles.q[i],
|
||||
// arm->stateVariable.currentJointAngles.q[i],
|
||||
// 0,
|
||||
// arm->timer.dt
|
||||
// );
|
||||
|
||||
// MOTOR_MIT_Output_t* output = NULL;
|
||||
// if (i == 3) output = &arm->output.jointMotor4_output;
|
||||
// else if (i == 4) output = &arm->output.jointMotor5_output;
|
||||
// else output = &arm->output.jointMotor6_output;
|
||||
|
||||
// output->angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
|
||||
// output->velocity = 0.0f;
|
||||
// output->kp = 50.0f; // 位置刚度（提供阻抗和稳定性）
|
||||
// output->kd = 3.0f; // 速度阻尼（抑制振荡）
|
||||
// output->torque = pid_output; // PID输出作为前馈力矩（主要驱动力）
|
||||
// }
|
||||
|
||||
// // 发送控制命令
|
||||
// MOTOR_LZ_MotionControl(&arm->param->jointMotor1_params, &arm->output.jointMotor1_output);
|
||||
// MOTOR_LZ_MotionControl(&arm->param->jointMotor2_params, &arm->output.jointMotor2_output);
|
||||
// MOTOR_LZ_MotionControl(&arm->param->jointMotor3_params, &arm->output.jointMotor3_output);
|
||||
// MOTOR_DM_MITCtrl(&arm->param->jointMotor4_params, &arm->output.jointMotor4_output);
|
||||
// MOTOR_DM_MITCtrl(&arm->param->jointMotor5_params, &arm->output.jointMotor5_output);
|
||||
// MOTOR_DM_MITCtrl(&arm->param->jointMotor6_params, &arm->output.jointMotor6_output);
|
||||
// }
|
||||
// break;
|
||||
|
||||
// case ARM_MODE_TRAJECTORY:
|
||||
// // 轨迹跟踪模式（待实现）
|
||||
// arm->control.state = ARM_STATE_ERROR;
|
||||
// break;
|
||||
|
||||
// case ARM_MODE_TEACH:
|
||||
// // 示教模式（待实现）
|
||||
// arm->control.state = ARM_STATE_ERROR;
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// arm->control.state = ARM_STATE_ERROR;
|
||||
// break;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 逆运动学求解：根据目标末端位姿计算关节角度
|
||||
// * @note 此函数计算量大，不要在控制循环中频繁调用
|
||||
// */
|
||||
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles) {
|
||||
// if (arm == NULL || target_pose == NULL || result_angles == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// // 验证目标位姿的有效性
|
||||
// if (isnan(target_pose->x) || isnan(target_pose->y) || isnan(target_pose->z) ||
|
||||
// isnan(target_pose->roll) || isnan(target_pose->pitch) || isnan(target_pose->yaw) ||
|
||||
// isinf(target_pose->x) || isinf(target_pose->y) || isinf(target_pose->z) ||
|
||||
// isinf(target_pose->roll) || isinf(target_pose->pitch) || isinf(target_pose->yaw)) {
|
||||
// return -1; // 无效的目标位姿
|
||||
// }
|
||||
|
||||
// // 使用当前关节角度作为初始猜测值（如果已初始化）
|
||||
// Arm6dof_JointAngles_t initial_guess;
|
||||
|
||||
// // 检查currentJointAngles是否已初始化（不全为0）
|
||||
// bool has_init = false;
|
||||
// for (int i = 0; i < 6; i++) {
|
||||
// if (arm->stateVariable.currentJointAngles.q[i] != 0.0f) {
|
||||
// has_init = true;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (has_init) {
|
||||
// // 使用当前角度作为初始猜测
|
||||
// memcpy(&initial_guess, &arm->stateVariable.currentJointAngles, sizeof(Arm6dof_JointAngles_t));
|
||||
// } else {
|
||||
// // 使用零位作为初始猜测（机械臂竖直向上）
|
||||
// memset(&initial_guess, 0, sizeof(Arm6dof_JointAngles_t));
|
||||
// }
|
||||
|
||||
// // 调用逆运动学求解，容限1mm，最大迭代10次（减少计算量）
|
||||
// int ret = Arm6dof_InverseKinematics(target_pose, &initial_guess, result_angles, 1.0f, 10);
|
||||
|
||||
// if (ret == 0) {
|
||||
// // 求解成功，更新到inverseKineJointAngles
|
||||
// memcpy(&arm->stateVariable.inverseKineJointAngles, result_angles, sizeof(Arm6dof_JointAngles_t));
|
||||
// }
|
||||
|
||||
// return ret;
|
||||
// }
|
||||
|
||||
// // ============================================================================
|
||||
// // 控制API函数实现
|
||||
// // ============================================================================
|
||||
|
||||
// /**
|
||||
// * @brief 设置控制模式
|
||||
// */
|
||||
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode) {
|
||||
// if (arm == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
// arm->control.mode = mode;
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 使能/失能机械臂
|
||||
// */
|
||||
// int8_t Arm_Enable(Arm_t *arm, bool enable) {
|
||||
// if (arm == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
// arm->control.enable = enable;
|
||||
|
||||
// if (!enable) {
|
||||
// // 失能时切换到空闲模式
|
||||
// arm->control.mode = ARM_MODE_IDLE;
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
// }
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 关节空间位置控制
|
||||
// */
|
||||
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles) {
|
||||
// if (arm == NULL || target_angles == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// // 检查关节角度是否在限制范围内
|
||||
// for (int i = 0; i < 6; i++) {
|
||||
// float qmin = arm->param->arm6dof_params.DH_params[i].qmin;
|
||||
// float qmax = arm->param->arm6dof_params.DH_params[i].qmax;
|
||||
|
||||
// if (target_angles->q[i] < qmin || target_angles->q[i] > qmax) {
|
||||
// return -1; // 超出关节限位
|
||||
// }
|
||||
// }
|
||||
|
||||
// // 更新目标角度
|
||||
// memcpy(&arm->stateVariable.targetJointAngles, target_angles, sizeof(Arm6dof_JointAngles_t));
|
||||
|
||||
// // 切换到关节位置控制模式
|
||||
// arm->control.mode = ARM_MODE_JOINT_POSITION;
|
||||
// arm->control.state = ARM_STATE_MOVING;
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 笛卡尔空间位置控制
|
||||
// */
|
||||
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose) {
|
||||
// if (arm == NULL || target_pose == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// // 求解逆运动学
|
||||
// Arm6dof_JointAngles_t ik_solution;
|
||||
// if (Arm_SolveIK(arm, target_pose, &ik_solution) != 0) {
|
||||
// return -1; // 逆运动学求解失败
|
||||
// }
|
||||
|
||||
// // 更新目标
|
||||
// memcpy(&arm->stateVariable.targetEndPose, target_pose, sizeof(Arm6dof_Pose_t));
|
||||
// memcpy(&arm->stateVariable.targetJointAngles, &ik_solution, sizeof(Arm6dof_JointAngles_t));
|
||||
|
||||
// // 切换到笛卡尔位置控制模式
|
||||
// arm->control.mode = ARM_MODE_CARTESIAN_POSITION;
|
||||
// arm->control.state = ARM_STATE_MOVING;
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 急停
|
||||
// */
|
||||
// int8_t Arm_Stop(Arm_t *arm) {
|
||||
// if (arm == NULL) {
|
||||
// return -1;
|
||||
// }
|
||||
|
||||
// // 将目标设置为当前位置
|
||||
// memcpy(&arm->stateVariable.targetJointAngles,
|
||||
// &arm->stateVariable.currentJointAngles,
|
||||
// sizeof(Arm6dof_JointAngles_t));
|
||||
|
||||
// arm->control.state = ARM_STATE_STOPPED;
|
||||
|
||||
// return 0;
|
||||
// }
|
||||
|
||||
// /**
|
||||
// * @brief 获取当前运动状态
|
||||
// */
|
||||
// Arm_MotionState_t Arm_GetState(Arm_t *arm) {
|
||||
// if (arm == NULL) {
|
||||
// return ARM_STATE_ERROR;
|
||||
// }
|
||||
// return arm->control.state;
|
||||
// }
|
||||
|
||||
@ -1,282 +0,0 @@
|
||||
// /*
|
||||
// * far♂蛇模块
|
||||
// */
|
||||
|
||||
// #pragma once
|
||||
|
||||
// #include <stdbool.h>
|
||||
// #include <stddef.h>
|
||||
// #include <stdint.h>
|
||||
// #ifdef __cplusplus
|
||||
// extern "C" {
|
||||
// #endif
|
||||
|
||||
// #include "main.h"
|
||||
// #include "component/pid.h"
|
||||
// #include "component/arm_kinematics/arm6dof.h"
|
||||
// #include "device/motor_dm.h"
|
||||
// #include "device/motor_lz.h"
|
||||
// /* Exported constants ------------------------------------------------------- */
|
||||
// #define ARM_JOINT_NUM 6 // 关节数量
|
||||
|
||||
// /* Exported macro ----------------------------------------------------------- */
|
||||
// /* Exported types ----------------------------------------------------------- */
|
||||
|
||||
// // 电机类型
|
||||
// typedef enum {
|
||||
// JOINT_MOTOR_TYPE_LZ = 0, // 凌控电机
|
||||
// JOINT_MOTOR_TYPE_DM, // 达妙电机
|
||||
// } Joint_MotorType_t;
|
||||
|
||||
// // 控制模式
|
||||
// typedef enum {
|
||||
// ARM_MODE_IDLE = 0, // 空闲模式（电机松弛）
|
||||
// ARM_MODE_JOINT_POSITION, // 关节空间位置控制
|
||||
// ARM_MODE_CARTESIAN_POSITION, // 笛卡尔空间位置控制
|
||||
// ARM_MODE_TRAJECTORY, // 轨迹跟踪模式
|
||||
// ARM_MODE_TEACH, // 示教模式（力控）
|
||||
// } Arm_ControlMode_t;
|
||||
|
||||
// // 运动状态
|
||||
// typedef enum {
|
||||
// ARM_STATE_STOPPED = 0, // 停止
|
||||
// ARM_STATE_MOVING, // 运动中
|
||||
// ARM_STATE_REACHED, // 到达目标
|
||||
// ARM_STATE_ERROR, // 错误
|
||||
// } Arm_MotionState_t;
|
||||
|
||||
// struct {
|
||||
// union {
|
||||
// MOTOR_LZ_Param_t lzMotor;
|
||||
// MOTOR_DM_Param_t dmMotor;
|
||||
// };
|
||||
// KPID_Params_t pid;
|
||||
// } *Joint_MotorParams_t;
|
||||
// typedef struct {
|
||||
// // 关节标识
|
||||
// uint8_t id; // 关节编号（0-5）
|
||||
// Joint_MotorType_t motor_type; // 电机类型
|
||||
|
||||
// // 运动学参数（DH参数）
|
||||
// Arm6dof_DHParams_t dh_params; // DH参数（包含限位）
|
||||
// float q_offset; // 零位偏移
|
||||
|
||||
// // 参数
|
||||
// *params;
|
||||
|
||||
// // 电机实例
|
||||
// union {
|
||||
// MOTOR_LZ_t lz_motor;
|
||||
// MOTOR_DM_t dm_motor;
|
||||
// } motor;
|
||||
|
||||
// // 控制器
|
||||
// KPID_t pid; // PID控制器
|
||||
|
||||
// // 状态变量
|
||||
// struct {
|
||||
// float current_angle; // 当前角度（去除零位偏移后）
|
||||
// float current_velocity; // 当前速度
|
||||
// float current_torque; // 当前力矩
|
||||
// float target_angle; // 目标角度
|
||||
// float target_velocity; // 目标速度
|
||||
// bool online; // 在线状态
|
||||
// } state;
|
||||
|
||||
// // 控制输出（根据motor_type选择使用）
|
||||
// union {
|
||||
// MOTOR_LZ_MotionParam_t lz_output;
|
||||
// MOTOR_MIT_Output_t dm_output;
|
||||
// } output;
|
||||
|
||||
// } Joint_t;
|
||||
|
||||
// // ============================================================================
|
||||
// // Arm（机械臂）- 由多个关节组成
|
||||
// // ============================================================================
|
||||
// typedef struct {
|
||||
// // 关节数组（面向对象：机械臂由6个关节对象组成）
|
||||
// Joint_t joints[ARM_JOINT_NUM];
|
||||
|
||||
// // 全局运动学参数
|
||||
// struct {
|
||||
// float x, y, z; // 工具中心偏移 (mm)
|
||||
// float roll, pitch, yaw; // 工具姿态偏移 (rad)
|
||||
// } tool_offset;
|
||||
|
||||
// // 时间管理
|
||||
// struct {
|
||||
// float now; // 当前时间（秒）
|
||||
// uint64_t last_wakeup; // 上次唤醒时间（微秒）
|
||||
// float dt; // 控制周期（秒）
|
||||
// } timer;
|
||||
|
||||
// // 末端状态（笛卡尔空间）
|
||||
// struct {
|
||||
// Arm6dof_Pose_t current; // 当前末端位姿（正运动学结果）
|
||||
// Arm6dof_Pose_t target; // 目标末端位姿
|
||||
// } end_effector;
|
||||
|
||||
// // 控制状态
|
||||
// struct {
|
||||
// Arm_ControlMode_t mode; // 控制模式
|
||||
// Arm_MotionState_t state; // 运动状态
|
||||
// float position_tolerance; // 位置到达容差（rad）
|
||||
// float velocity_tolerance; // 速度到达容差（rad/s）
|
||||
// bool enable; // 使能标志
|
||||
// } control;
|
||||
|
||||
// } Arm_t;
|
||||
|
||||
// // 兼容旧接口的参数结构体
|
||||
// typedef struct {
|
||||
// Arm6dof_Params_t arm6dof_params;
|
||||
// KPID_Params_t joint_pid_params[6];
|
||||
// MOTOR_LZ_Param_t jointMotor1_params;
|
||||
// MOTOR_LZ_Param_t jointMotor2_params;
|
||||
// MOTOR_LZ_Param_t jointMotor3_params;
|
||||
// MOTOR_DM_Param_t jointMotor4_params;
|
||||
// MOTOR_DM_Param_t jointMotor5_params;
|
||||
// MOTOR_DM_Param_t jointMotor6_params;
|
||||
// float q_offset[6];
|
||||
// } Arm_Params_t;
|
||||
|
||||
// /* Exported functions prototypes -------------------------------------------- */
|
||||
|
||||
// /**
|
||||
// * @brief 初始化机械臂（兼容旧接口）
|
||||
// */
|
||||
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);
|
||||
|
||||
// /**
|
||||
// * @brief 更新机械臂状态
|
||||
// */
|
||||
// int8_t Arm_Updata(Arm_t *arm);
|
||||
|
||||
// /**
|
||||
// * @brief 机械臂控制
|
||||
// */
|
||||
// int8_t Arm_Ctrl(Arm_t *arm);
|
||||
|
||||
// // ============================================================================
|
||||
// // 关节级别操作（面向对象接口）
|
||||
// // ============================================================================
|
||||
|
||||
// /**
|
||||
// * @brief 初始化单个关节
|
||||
// * @param joint 关节指针
|
||||
// * @param id 关节编号（0-5）
|
||||
// * @param motor_type 电机类型
|
||||
// * @param dh_params DH参数
|
||||
// * @param pid_params PID参数
|
||||
// * @param freq 控制频率
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
|
||||
// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq);
|
||||
|
||||
// /**
|
||||
// * @brief 更新关节状态（读取电机反馈）
|
||||
// * @param joint 关节指针
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Joint_Update(Joint_t *joint);
|
||||
|
||||
// /**
|
||||
// * @brief 关节位置控制
|
||||
// * @param joint 关节指针
|
||||
// * @param target_angle 目标角度（rad）
|
||||
// * @param dt 控制周期（s）
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt);
|
||||
|
||||
// /**
|
||||
// * @brief 关节松弛
|
||||
// * @param joint 关节指针
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Joint_Relax(Joint_t *joint);
|
||||
|
||||
// /**
|
||||
// * @brief 检查关节是否到达目标
|
||||
// * @param joint 关节指针
|
||||
// * @param tolerance 位置容差（rad）
|
||||
// * @return true: 已到达, false: 未到达
|
||||
// */
|
||||
// bool Joint_IsReached(const Joint_t *joint, float tolerance);
|
||||
|
||||
// // ============================================================================
|
||||
// // 机械臂级别操作（继续保持现有接口）
|
||||
// // ============================================================================
|
||||
|
||||
// /**
|
||||
// * @brief 设置控制模式
|
||||
// * @param arm 机械臂实例
|
||||
// * @param mode 控制模式
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode);
|
||||
|
||||
// /**
|
||||
// * @brief 使能/失能机械臂
|
||||
// * @param arm 机械臂实例
|
||||
// * @param enable true: 使能, false: 失能
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Arm_Enable(Arm_t *arm, bool enable);
|
||||
|
||||
// /**
|
||||
// * @brief 关节空间位置控制：设置目标关节角度
|
||||
// * @param arm 机械臂实例
|
||||
// * @param target_angles 目标关节角度（rad）
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles);
|
||||
|
||||
// /**
|
||||
// * @brief 笛卡尔空间位置控制：设置目标末端位姿
|
||||
// * @param arm 机械臂实例
|
||||
// * @param target_pose 目标末端位姿
|
||||
// * @return 0: 成功（含逆解成功）, -1: 失败（含逆解失败）
|
||||
// */
|
||||
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose);
|
||||
|
||||
// /**
|
||||
// * @brief 急停：立即停止所有运动
|
||||
// * @param arm 机械臂实例
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// */
|
||||
// int8_t Arm_Stop(Arm_t *arm);
|
||||
|
||||
// /**
|
||||
// * @brief 获取当前运动状态
|
||||
// * @param arm 机械臂实例
|
||||
// * @return 运动状态
|
||||
// */
|
||||
// Arm_MotionState_t Arm_GetState(Arm_t *arm);
|
||||
|
||||
// /**
|
||||
// * @brief ;
|
||||
// }output;
|
||||
// */
|
||||
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);
|
||||
// int8_t Arm_Updata(Arm_t *arm);
|
||||
// int8_t Arm_Ctrl(Arm_t *arm);
|
||||
|
||||
// /**
|
||||
// * @brief 逆运动学求解：根据目标末端位姿计算关节角度
|
||||
// * @param arm 机械臂实例
|
||||
// * @param target_pose 目标末端位姿
|
||||
// * @param result_angles 输出的关节角度解
|
||||
// * @return 0: 成功, -1: 失败
|
||||
// * @note 使用当前关节角度作为初始猜测值，迭代50次，误差容限1.0mm
|
||||
// */
|
||||
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles);
|
||||
|
||||
// #ifdef __cplusplus
|
||||
// }
|
||||
// #endif
|
||||
|
||||
|
||||
|
||||
@ -13,7 +13,7 @@
|
||||
#include "bsp/time.h"
|
||||
#include "component/arm_kinematics/arm6dof.h"
|
||||
#include "component/toolbox/robotics.h"
|
||||
#define qLimitTolerance 0.5// 关节角度误差容限（rad），用于IK解的有效性验证
|
||||
#define qLimitTolerance 0.75// 关节角度误差容限（rad），用于IK解的有效性验证
|
||||
namespace arm {
|
||||
|
||||
// 控制模式
|
||||
@ -21,6 +21,7 @@ enum class ControlMode {
|
||||
IDLE = 0,
|
||||
JOINT_POSITION,
|
||||
CARTESIAN_POSITION,
|
||||
CARTESIAN_ANALYTICAL,
|
||||
TRAJECTORY,
|
||||
TEACH, // 示教模式（重力补偿 + 零刚度）
|
||||
GRAVITY_COMP, // 重力补偿模式（带位置保持）
|
||||
@ -84,12 +85,61 @@ private:
|
||||
bool valid; // IK解是否有效
|
||||
} traj_;
|
||||
|
||||
// 坐标系目标缓存
|
||||
// 用途：MoveCartesianTool / MoveCartesianHeading 每帧都接收坐标系下的绝对目标，
|
||||
// 若每帧都用当前 end_effector_.current 做变换，机器人运动时位姿变化会导致
|
||||
// 世界系目标每帧漂移，MoveCartesian 检测到 goal 变化就重置 traj_.t=0，
|
||||
// t 永远无法到达 1。
|
||||
// 解决：仅当输入目标发生变化时才重新用当前位姿换算世界系目标并缓存；
|
||||
// 输入不变时复用缓存，轨迹可以连续推进直到 t=1。
|
||||
struct FrameCache_t {
|
||||
Arm6dof_Pose_t last_input; // 上次收到的坐标系输入
|
||||
Arm6dof_Pose_t world_goal; // 对应的世界系目标（换算结果）
|
||||
bool valid; // false=尚未初始化，首次调用强制换算
|
||||
};
|
||||
FrameCache_t tool_frame_cache_;
|
||||
FrameCache_t heading_frame_cache_;
|
||||
|
||||
// 时间管理
|
||||
struct {
|
||||
float now;
|
||||
uint64_t last_wakeup;
|
||||
float dt;
|
||||
} timer_;
|
||||
|
||||
static bool IsPoseChanged(const Arm6dof_Pose_t& lhs,
|
||||
const Arm6dof_Pose_t& rhs,
|
||||
float pos_eps,
|
||||
float ang_eps) {
|
||||
return fabsf(lhs.x - rhs.x) > pos_eps ||
|
||||
fabsf(lhs.y - rhs.y) > pos_eps ||
|
||||
fabsf(lhs.z - rhs.z) > pos_eps ||
|
||||
fabsf(lhs.roll - rhs.roll) > ang_eps ||
|
||||
fabsf(lhs.pitch - rhs.pitch) > ang_eps ||
|
||||
fabsf(lhs.yaw - rhs.yaw) > ang_eps;
|
||||
}
|
||||
|
||||
void RelaxAllJoints() {
|
||||
for (Joint* joint : joints_) {
|
||||
if (joint) {
|
||||
joint->Relax();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SetJointTargetsToCurrent() {
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (joints_[i]) {
|
||||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void FillCurrentJointAngles(Arm6dof_JointAngles_t& q) const {
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
// 构造函数
|
||||
@ -115,6 +165,11 @@ public:
|
||||
traj_.max_lin_vel = 0.15f; // 150 mm/s，可通过 SetLinVelLimit() 调整
|
||||
traj_.max_ang_vel = 1.0f; // ~57°/s，可通过 SetAngVelLimit() 调整
|
||||
|
||||
memset(&tool_frame_cache_, 0, sizeof(tool_frame_cache_));
|
||||
memset(&heading_frame_cache_, 0, sizeof(heading_frame_cache_));
|
||||
tool_frame_cache_.valid = false;
|
||||
heading_frame_cache_.valid = false;
|
||||
|
||||
timer_.now = 0.0f;
|
||||
timer_.last_wakeup = 0;
|
||||
timer_.dt = 0.001f;
|
||||
@ -163,9 +218,7 @@ public:
|
||||
|
||||
// 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取）
|
||||
Arm6dof_JointAngles_t q;
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||||
}
|
||||
FillCurrentJointAngles(q);
|
||||
|
||||
// 正运动学：计算当前末端位姿
|
||||
Arm6dof_ForwardKinematics(&q, &end_effector_.current);
|
||||
@ -194,6 +247,83 @@ public:
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 2.5D 解析降维逆运动学（专为该六轴机械臂的L型构型设计）
|
||||
* 特点：无奇异点、O(1)极速求解、绝对稳定。
|
||||
* 直接映射目标并进行安全边界裁剪，无需牛顿迭代。
|
||||
*/
|
||||
int8_t InverseKinematicsAnalytical(const Arm6dof_Pose_t* pose, Arm6dof_JointAngles_t* q_out) {
|
||||
if (!pose || !q_out) return -1;
|
||||
|
||||
float x = pose->x;
|
||||
float y = pose->y;
|
||||
float z = pose->z;
|
||||
|
||||
// 提取的宏观参数
|
||||
const float L1 = 0.3265f;
|
||||
const float dX = 0.0905f;
|
||||
const float dY = 0.05278f + 0.18467f; // 从 URDF 参数 J4 和 J5 中提取的垂直偏置积累
|
||||
const float L2_eff = sqrtf(dX*dX + dY*dY); // 虚拟小臂斜边
|
||||
const float theta_offset = atan2f(dY, dX); // 结构连杆的天然偏置仰角
|
||||
const float L3 = 0.10487f; // 腕部总伸出长度
|
||||
const float Z_J2 = 0.024f + 0.1015f; // 自基座到底盘的Z高度
|
||||
|
||||
// J1: 偏航直接通过 XY 夹角决定
|
||||
float target_q1 = atan2f(y, x);
|
||||
|
||||
// 面向R-Z平面的投影
|
||||
float R = sqrtf(x*x + y*y);
|
||||
float e_pitch = pose->pitch;
|
||||
|
||||
// 逆推动力学关节5 (虚拟手腕端点) 的位置
|
||||
float R5 = R - L3 * cosf(e_pitch);
|
||||
float Z5 = z - L3 * sinf(e_pitch);
|
||||
|
||||
// 相对于 J2 原点的距离
|
||||
float r_target = R5;
|
||||
float z_target = Z5 - Z_J2;
|
||||
|
||||
float D_sq = r_target*r_target + z_target*z_target;
|
||||
float D = sqrtf(D_sq);
|
||||
|
||||
// 死区保护层(极其重要)：限制最大前伸距离，强制拦下无效追踪，绝不当机
|
||||
float MAX_REACH = L1 + L2_eff - 0.005f;
|
||||
float MIN_REACH = fabsf(L1 - L2_eff) + 0.005f;
|
||||
if (D > MAX_REACH) { D = MAX_REACH; D_sq = D*D; }
|
||||
if (D < MIN_REACH) { D = MIN_REACH; D_sq = D*D; }
|
||||
|
||||
// 用余弦定理求 肘部内角 gamma
|
||||
float cos_gamma = (L1*L1 + L2_eff*L2_eff - D_sq) / (2.0f * L1 * L2_eff);
|
||||
cos_gamma = (cos_gamma > 1.0f) ? 1.0f : ((cos_gamma < -1.0f) ? -1.0f : cos_gamma);
|
||||
float gamma = acosf(cos_gamma);
|
||||
|
||||
// 用余弦定理求 目标线与大臂的夹角 beta
|
||||
float cos_beta = (L1*L1 + D_sq - L2_eff*L2_eff) / (2.0f * L1 * D);
|
||||
cos_beta = (cos_beta > 1.0f) ? 1.0f : ((cos_beta < -1.0f) ? -1.0f : cos_beta);
|
||||
float beta = acosf(cos_beta);
|
||||
|
||||
// 目标线相对于水平面的仰角 alpha
|
||||
float alpha = atan2f(z_target, r_target);
|
||||
|
||||
// 选择 Elbow Up（肘部向上）姿态：
|
||||
float target_q2 = alpha + beta;
|
||||
float q3_eff = -( (float)M_PI - gamma);
|
||||
float target_q3 = q3_eff - theta_offset;
|
||||
|
||||
// 手腕解耦补偿
|
||||
float arm_pitch_eff = target_q2 + q3_eff;
|
||||
float target_q5 = e_pitch - arm_pitch_eff;
|
||||
|
||||
q_out->q[0] = target_q1;
|
||||
q_out->q[1] = target_q2;
|
||||
q_out->q[2] = target_q3;
|
||||
q_out->q[3] = pose->roll;
|
||||
q_out->q[4] = target_q5;
|
||||
q_out->q[5] = pose->yaw;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 轨迹规划核心：基于固定的 start→goal 路径，由 traj_.t 驱动进度
|
||||
@ -255,12 +385,17 @@ public:
|
||||
if (traj_.valid) {
|
||||
q_init = traj_.angles;
|
||||
} else {
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||||
}
|
||||
FillCurrentJointAngles(q_init);
|
||||
}
|
||||
|
||||
if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
|
||||
bool ik_success = false;
|
||||
if (control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
|
||||
ik_success = (InverseKinematicsAnalytical(&interp, &traj_.angles) == 0);
|
||||
} else {
|
||||
ik_success = (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) == 0);
|
||||
}
|
||||
|
||||
if (!ik_success) {
|
||||
traj_.valid = false;
|
||||
traj_.active = false;
|
||||
control_.state = MotionState::ERROR;
|
||||
@ -271,7 +406,7 @@ public:
|
||||
// IK 可能返回与 q_init 差 2π 整数倍的等效解导致跳变，wrap 到最近分支
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (!joints_[i]) continue;
|
||||
const auto& dh = joints_[i]->GetDHParams();
|
||||
const auto& dh = joints_[i]->GetParams();
|
||||
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; // 范围 ≤ 2π，无等效分支
|
||||
float diff = traj_.angles.q[i] - q_init.q[i];
|
||||
diff -= roundf(diff / TWO_PI) * TWO_PI;
|
||||
@ -281,7 +416,7 @@ public:
|
||||
// 限位检查
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (joints_[i]) {
|
||||
const auto& dh = joints_[i]->GetDHParams();
|
||||
const auto& dh = joints_[i]->GetParams();
|
||||
if (traj_.angles.q[i] < (dh.qmin - qLimitTolerance) ||
|
||||
traj_.angles.q[i] > (dh.qmax + qLimitTolerance)) {
|
||||
traj_.valid = false;
|
||||
@ -339,9 +474,7 @@ public:
|
||||
|
||||
// 未使能则松弛
|
||||
if (!control_.enable) {
|
||||
for (Joint* joint : joints_) { // 遍历关节数组
|
||||
if (joint) joint->Relax();
|
||||
}
|
||||
RelaxAllJoints();
|
||||
control_.state = MotionState::STOPPED;
|
||||
return 0;
|
||||
}
|
||||
@ -349,21 +482,18 @@ public:
|
||||
// 根据控制模式执行
|
||||
switch (control_.mode) {
|
||||
case ControlMode::IDLE:
|
||||
for (Joint* joint : joints_) { // 遍历关节数组
|
||||
if (joint) joint->Relax();
|
||||
}
|
||||
RelaxAllJoints();
|
||||
control_.state = MotionState::STOPPED;
|
||||
break;
|
||||
|
||||
case ControlMode::JOINT_POSITION:
|
||||
case ControlMode::CARTESIAN_POSITION:
|
||||
case ControlMode::CARTESIAN_ANALYTICAL:
|
||||
case ControlMode::GRAVITY_COMP: {
|
||||
// 检查错误状态（不可达或超限）
|
||||
if (control_.state == MotionState::ERROR) {
|
||||
// 进入错误状态，松弛所有关节（安全停止）
|
||||
for (Joint* joint : joints_) {
|
||||
if (joint) joint->Relax();
|
||||
}
|
||||
RelaxAllJoints();
|
||||
control_.enable = false; // 自动禁用
|
||||
return -1;
|
||||
}
|
||||
@ -379,15 +509,11 @@ public:
|
||||
|
||||
// GRAVITY_COMP 模式：保持当前位置 + 重力补偿
|
||||
if (control_.mode == ControlMode::GRAVITY_COMP) {
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (joints_[i]) {
|
||||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||||
}
|
||||
}
|
||||
SetJointTargetsToCurrent();
|
||||
}
|
||||
|
||||
// CARTESIAN_POSITION 模式：每帧推进轨迹一小步，再应用IK解到关节
|
||||
if (control_.mode == ControlMode::CARTESIAN_POSITION) {
|
||||
// CARTESIAN_POSITION 相关的轨迹模式：每帧推进轨迹一小步，再应用IK解到关节
|
||||
if (control_.mode == ControlMode::CARTESIAN_POSITION || control_.mode == ControlMode::CARTESIAN_ANALYTICAL) {
|
||||
// 轨迹规划核心调用：插值 + IK，结果写入 traj_
|
||||
StepTrajectory();
|
||||
|
||||
@ -399,9 +525,7 @@ public:
|
||||
}
|
||||
} else {
|
||||
// IK解无效：松弛所有关节并进入错误状态
|
||||
for (Joint* joint : joints_) {
|
||||
if (joint) joint->Relax();
|
||||
}
|
||||
RelaxAllJoints();
|
||||
control_.state = MotionState::ERROR;
|
||||
return -1;
|
||||
}
|
||||
@ -432,9 +556,7 @@ public:
|
||||
}
|
||||
} else {
|
||||
// 未使能重力补偿时，示教模式下松弛
|
||||
for (Joint* joint : joints_) {
|
||||
if (joint) joint->Relax();
|
||||
}
|
||||
RelaxAllJoints();
|
||||
}
|
||||
control_.state = MotionState::MOVING;
|
||||
break;
|
||||
@ -506,7 +628,7 @@ public:
|
||||
// 1. 检查限位
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (joints_[i]) {
|
||||
const auto& dh = joints_[i]->GetDHParams();
|
||||
const auto& dh = joints_[i]->GetParams();
|
||||
if (target_angles[i] < dh.qmin || target_angles[i] > dh.qmax) {
|
||||
return -1; // 超限
|
||||
}
|
||||
@ -535,13 +657,7 @@ public:
|
||||
// 检测目标是否发生有效变化（避免每帧重置轨迹）
|
||||
constexpr float POS_THRESH = 0.001f; // 1mm
|
||||
constexpr float ANG_THRESH = 0.01f; // ~0.57°
|
||||
bool goal_changed =
|
||||
fabsf(goal.x - traj_.goal.x) > POS_THRESH ||
|
||||
fabsf(goal.y - traj_.goal.y) > POS_THRESH ||
|
||||
fabsf(goal.z - traj_.goal.z) > POS_THRESH ||
|
||||
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
|
||||
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
|
||||
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
|
||||
bool goal_changed = IsPoseChanged(goal, traj_.goal, POS_THRESH, ANG_THRESH);
|
||||
if (goal_changed) {
|
||||
// 从当前末端位姿出发重新规划轨迹
|
||||
traj_.start = end_effector_.current;
|
||||
@ -575,31 +691,39 @@ public:
|
||||
* .roll/.pitch/.yaw ：工具系下的目标姿态 (rad)
|
||||
*/
|
||||
void MoveCartesianTool(const Arm6dof_Pose_t& target_tool) {
|
||||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||||
// 仅当用户输入目标发生变化时才重新换算世界系目标
|
||||
// 若输入不变（用户未推杆），复用缓存的世界系目标，保证轨迹可以连续推进至 t=1
|
||||
constexpr float POS_EPS = 0.001f;
|
||||
constexpr float ANG_EPS = 0.01f;
|
||||
const FrameCache_t& tc = tool_frame_cache_;
|
||||
bool input_changed = !tc.valid || IsPoseChanged(target_tool, tc.last_input, POS_EPS, ANG_EPS);
|
||||
|
||||
// ── R_cur = Rz(yaw)*Ry(pitch)*Rx(roll) ──
|
||||
float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
|
||||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||
if (input_changed) {
|
||||
// 用当前末端位姿做坐标系变换（仅在用户改变目标时触发一次）
|
||||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||||
float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
|
||||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||
|
||||
// ── 位置：p_world = R_cur * p_tool（绝对目标，工具系坐标→世界系） ──
|
||||
Arm6dof_Pose_t goal;
|
||||
float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z};
|
||||
Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr);
|
||||
goal.x = p_world[0][0];
|
||||
goal.y = p_world[1][0];
|
||||
goal.z = p_world[2][0];
|
||||
Arm6dof_Pose_t goal;
|
||||
float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z};
|
||||
Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr);
|
||||
goal.x = p_world[0][0];
|
||||
goal.y = p_world[1][0];
|
||||
goal.z = p_world[2][0];
|
||||
|
||||
// ── 姿态：R_world = R_cur * R_target（工具系绝对姿态→世界系） ──
|
||||
float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll};
|
||||
Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||||
float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll};
|
||||
Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
|
||||
goal.yaw = rpy_new[0][0];
|
||||
goal.pitch = rpy_new[1][0];
|
||||
goal.roll = rpy_new[2][0];
|
||||
|
||||
// ── R_new → RPY，写入目标位姿 ──
|
||||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
|
||||
goal.yaw = rpy_new[0][0];
|
||||
goal.pitch = rpy_new[1][0];
|
||||
goal.roll = rpy_new[2][0];
|
||||
tool_frame_cache_.last_input = target_tool;
|
||||
tool_frame_cache_.world_goal = goal;
|
||||
tool_frame_cache_.valid = true;
|
||||
}
|
||||
|
||||
MoveCartesian(goal);
|
||||
MoveCartesian(tool_frame_cache_.world_goal);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -620,28 +744,35 @@ public:
|
||||
* 直观含义：yaw转动就是末端 yaw，pitch/roll 同理
|
||||
*/
|
||||
void MoveCartesianHeading(const Arm6dof_Pose_t& target_heading) {
|
||||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||||
// 仅当用户输入目标发生变化时才重新换算世界系目标（同 MoveCartesianTool 原理）
|
||||
constexpr float POS_EPS = 0.001f;
|
||||
constexpr float ANG_EPS = 0.01f;
|
||||
const FrameCache_t& hc = heading_frame_cache_;
|
||||
bool input_changed = !hc.valid || IsPoseChanged(target_heading, hc.last_input, POS_EPS, ANG_EPS);
|
||||
|
||||
// ── 位置：航向系→世界系，Rz(yaw) 旋转水平分量，Z 直接为世界Z ──
|
||||
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
|
||||
Arm6dof_Pose_t goal;
|
||||
goal.x = cy * target_heading.x - sy * target_heading.y; // 世界X
|
||||
goal.y = sy * target_heading.x + cy * target_heading.y; // 世界Y
|
||||
goal.z = target_heading.z; // 世界Z 直接赋值
|
||||
if (input_changed) {
|
||||
const Arm6dof_Pose_t& cur = end_effector_.current;
|
||||
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
|
||||
Arm6dof_Pose_t goal;
|
||||
goal.x = cy * target_heading.x - sy * target_heading.y;
|
||||
goal.y = sy * target_heading.x + cy * target_heading.y;
|
||||
goal.z = target_heading.z;
|
||||
|
||||
// ── 姿态：航向系下绝对姿态→世界系：R_world = Rz(yaw) * R_target ──
|
||||
float rpy_yaw_arr[3] = {cur.yaw, 0.0f, 0.0f};
|
||||
float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll};
|
||||
Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr))
|
||||
* robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||||
float rpy_yaw_arr[3] = {cur.yaw, 0.0f, 0.0f};
|
||||
float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll};
|
||||
Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr))
|
||||
* robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
|
||||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn);
|
||||
goal.yaw = rpy_new[0][0];
|
||||
goal.pitch = rpy_new[1][0];
|
||||
goal.roll = rpy_new[2][0];
|
||||
|
||||
// ── R_new → RPY，写入目标位姿 ──
|
||||
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
|
||||
goal.yaw = rpy_new[0][0];
|
||||
goal.pitch = rpy_new[1][0];
|
||||
goal.roll = rpy_new[2][0];
|
||||
heading_frame_cache_.last_input = target_heading;
|
||||
heading_frame_cache_.world_goal = goal;
|
||||
heading_frame_cache_.valid = true;
|
||||
}
|
||||
|
||||
MoveCartesian(goal);
|
||||
MoveCartesian(heading_frame_cache_.world_goal);
|
||||
}
|
||||
|
||||
// 获取末端位姿（FK结果，单位 m/rad）
|
||||
@ -680,14 +811,8 @@ public:
|
||||
traj_.active = false;
|
||||
|
||||
Arm6dof_JointAngles_t q;
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
|
||||
}
|
||||
for (size_t i = 0; i < JOINT_NUM; ++i) {
|
||||
if (joints_[i]) {
|
||||
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
|
||||
}
|
||||
}
|
||||
FillCurrentJointAngles(q);
|
||||
SetJointTargetsToCurrent();
|
||||
inverseKinematics_.angles = q;
|
||||
|
||||
|
||||
|
||||
@ -256,52 +256,72 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
|
||||
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
|
||||
static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
|
||||
/*
|
||||
* 左拨杆映射：
|
||||
* UP -> enable = false（失能）
|
||||
* MID -> enable = true，正常笛卡尔增量控制
|
||||
* DOWN -> enable = true，set_target_as_current = true（目标位姿吸附为当前位姿）
|
||||
* 遥控器控制机械臂末端位姿 —— 笛卡尔增量方案
|
||||
*
|
||||
* 左拨杆 SW_L (sw[0]) —— 整体使能:
|
||||
* UP → 失能（电机松弛/重力补偿）
|
||||
* MID → 使能，笛卡尔增量控制
|
||||
* DOWN → 使能（保留备用）
|
||||
*
|
||||
* 右拨杆 SW_R (sw[1]) —— 自由度分组:
|
||||
*
|
||||
* UP 【位置模式】3轴平移 + 偏航
|
||||
* 右摇杆X (RX) → X 平移
|
||||
* 右摇杆Y (RY) → Y 平移
|
||||
* 左摇杆Y (LY) → Z 平移（升降）
|
||||
* 左摇杆X (LX) → Yaw 偏航旋转
|
||||
*
|
||||
* MID 【姿态模式】俯仰/横滚/偏航 + 升降
|
||||
* 右摇杆X (RX) → Yaw 偏航旋转
|
||||
* 右摇杆Y (RY) → Pitch 俯仰旋转
|
||||
* 左摇杆X (LX) → Roll 横滚旋转
|
||||
* 左摇杆Y (LY) → Z 平移（升降）
|
||||
*
|
||||
* DOWN → set_target_as_current（目标吸附当前实际位姿，消除累积漂移）
|
||||
*/
|
||||
ctx->output.arm.cmd.set_target_as_current = false;
|
||||
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
|
||||
ctx->output.arm.cmd.set_target_as_current = true;
|
||||
if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
|
||||
ctx->output.arm.cmd.set_target_as_current = true;
|
||||
}
|
||||
|
||||
switch (ctx->input.rc.sw[0]) {
|
||||
case CMD_SW_MID:
|
||||
ctx->output.arm.cmd.enable = true;
|
||||
break;
|
||||
case CMD_SW_DOWN:
|
||||
ctx->output.arm.cmd.enable = true;
|
||||
break;
|
||||
default:
|
||||
ctx->output.arm.cmd.enable = false;
|
||||
goto end;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
case CMD_SW_DOWN:
|
||||
ctx->output.arm.cmd.enable = true;
|
||||
break;
|
||||
default:
|
||||
ctx->output.arm.cmd.enable = false;
|
||||
goto end;
|
||||
}
|
||||
|
||||
/* 遥控器模式使用笛卡尔位姿控制 */
|
||||
/* 遥控器模式使用笛卡尔位姿累积控制 */
|
||||
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
|
||||
|
||||
if (!ctx->output.arm.cmd.enable) return;
|
||||
/* set_target_as_current 置位时不叠加摇杆增量，由上层模块负责同步位姿 */
|
||||
/* set_target_as_current 置位时不叠加摇杆增量，由上层负责同步位姿基准 */
|
||||
if (ctx->output.arm.cmd.set_target_as_current) return;
|
||||
|
||||
/* 摆杆增量控制末端位姿 */
|
||||
float pos_speed = 0.3f * ctx->timer.dt; /* 位置平移速度 (m/s) */
|
||||
float rot_speed = 1.0f * ctx->timer.dt; /* 姿态旋转速度 (rad/s) */
|
||||
switch (ctx->input.rc.sw[1]) {
|
||||
case CMD_SW_UP:
|
||||
ctx->output.arm.cmd.target_pose.x += ctx->input.rc.joy_right.x * pos_speed;
|
||||
ctx->output.arm.cmd.target_pose.y += ctx->input.rc.joy_right.y * pos_speed;
|
||||
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_speed;
|
||||
ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x * rot_speed;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
ctx->output.arm.cmd.target_pose.roll += ctx->input.rc.joy_left.x * pos_speed;
|
||||
ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * pos_speed;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
/* 摇杆速度直接积分到 target_pose（坐标系语义由 arm_main 的 control_frame 决定） */
|
||||
const float pos_scale = 0.3f; /* 末端线速度上限 (m/s) */
|
||||
const float rot_scale = 1.0f; /* 末端角速度上限 (rad/s) */
|
||||
const float dt = ctx->timer.dt;
|
||||
|
||||
switch (ctx->input.rc.sw[1]) {
|
||||
case CMD_SW_UP:
|
||||
/* 位置模式：3轴平移 + 偏航 */
|
||||
ctx->output.arm.cmd.target_pose.x += ctx->input.rc.joy_right.x * pos_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.y += ctx->input.rc.joy_right.y * pos_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_left.x * rot_scale * dt;
|
||||
break;
|
||||
case CMD_SW_MID:
|
||||
/* 姿态模式：俯仰 + 横滚 + 偏航 + 升降 */
|
||||
ctx->output.arm.cmd.target_pose.yaw += ctx->input.rc.joy_right.x * rot_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.pitch += ctx->input.rc.joy_right.y * rot_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.roll += ctx->input.rc.joy_left.x * rot_scale * dt;
|
||||
ctx->output.arm.cmd.target_pose.z += ctx->input.rc.joy_left.y * pos_scale * dt;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
end:
|
||||
return;
|
||||
|
||||
@ -12,7 +12,6 @@ extern "C" {
|
||||
#include "component/pid.h"
|
||||
#include "device/motor.h"
|
||||
#include "device/motor_dm.h"
|
||||
#include "module/arm.h"
|
||||
#include "module/chassis.h"
|
||||
#include "module/cmd/cmd.h"
|
||||
#include "component/PowerControl.h"
|
||||
|
||||
@ -12,6 +12,13 @@
|
||||
|
||||
namespace arm {
|
||||
|
||||
struct JointControlParams {
|
||||
float qmin;
|
||||
float qmax;
|
||||
float kp;
|
||||
float kd;
|
||||
};
|
||||
|
||||
// ============================================================================
|
||||
// Joint 类 - 关节对象
|
||||
// ============================================================================
|
||||
@ -19,7 +26,7 @@ class Joint {
|
||||
private:
|
||||
uint8_t id_; // 关节编号
|
||||
IMotor* motor_; // 电机指针（多态）
|
||||
Arm6dof_DHParams_t dh_params_; // DH参数
|
||||
JointControlParams params_; // 关节控制参数
|
||||
float q_offset_; // 零位偏移
|
||||
const Joint* coupled_joint_; // 耦合源关节（J3 与 J2 耦合）
|
||||
|
||||
@ -35,9 +42,9 @@ private:
|
||||
|
||||
public:
|
||||
// 构造函数
|
||||
Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params,
|
||||
Joint(uint8_t id, IMotor* motor, const JointControlParams& params,
|
||||
float q_offset, float freq)
|
||||
: id_(id), motor_(motor), dh_params_(dh_params), q_offset_(q_offset),
|
||||
: id_(id), motor_(motor), params_(params), q_offset_(q_offset),
|
||||
coupled_joint_(nullptr) {
|
||||
|
||||
state_.current_angle = 0.0f;
|
||||
@ -47,6 +54,15 @@ public:
|
||||
state_.feedforward_torque = 0.0f;
|
||||
state_.online = false;
|
||||
}
|
||||
|
||||
// 兼容构造：允许直接传入机械臂DH参数，但仅提取关节层需要的控制字段
|
||||
Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params,
|
||||
float q_offset, float freq)
|
||||
: Joint(id,
|
||||
motor,
|
||||
JointControlParams{dh_params.qmin, dh_params.qmax, 0.0f, 0.0f},
|
||||
q_offset,
|
||||
freq) {}
|
||||
|
||||
// 禁止拷贝（电机资源唯一）
|
||||
Joint(const Joint&) = delete;
|
||||
@ -58,7 +74,7 @@ public:
|
||||
float GetCurrentVelocity() const { return state_.current_velocity; }
|
||||
float GetTargetAngle() const { return state_.target_angle; }
|
||||
bool IsOnline() const { return state_.online; }
|
||||
const Arm6dof_DHParams_t& GetDHParams() const { return dh_params_; }
|
||||
const JointControlParams& GetParams() const { return params_; }
|
||||
float GetOffset() const { return q_offset_; }
|
||||
|
||||
// Setters
|
||||
@ -101,7 +117,7 @@ public:
|
||||
// 零点不变：raw=0 时不触发任何折叠
|
||||
// 注意：j3 范围 [0,3]、j5 范围 [-1.75,1.75] 均在 [-π,π] 内，
|
||||
// 归一化后其实际运动段不会碰到 ±π 边界，计算安全
|
||||
const bool multi_turn = (dh_params_.qmax - dh_params_.qmin) > 2.0f * M_PI;
|
||||
const bool multi_turn = (params_.qmax - params_.qmin) > 2.0f * M_PI;
|
||||
if (!multi_turn) {
|
||||
if (raw > M_PI) raw -= 2.0f * M_PI;
|
||||
if (raw < -M_PI) raw += 2.0f * M_PI;
|
||||
@ -128,7 +144,7 @@ public:
|
||||
// 位置控制
|
||||
int8_t PositionControl(float target_angle, float dt) {
|
||||
// 检查限位
|
||||
if (target_angle < dh_params_.qmin || target_angle > dh_params_.qmax) {
|
||||
if (target_angle < params_.qmin || target_angle > params_.qmax) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -138,8 +154,8 @@ public:
|
||||
float total_feedforward = state_.feedforward_torque;
|
||||
|
||||
// MIT控制参数：优先使用DH参数中配置的kp/kd，否则使用默认值
|
||||
float kp = dh_params_.kp;
|
||||
float kd = dh_params_.kd;
|
||||
float kp = params_.kp;
|
||||
float kd = params_.kd;
|
||||
|
||||
// 如果未配置（=0），使用默认值（按电机类型）
|
||||
if (kp <= 0.0f) kp = (id_ < 3) ? 10.0f : 50.0f; // LZ vs DM默认值
|
||||
|
||||
@ -32,30 +32,28 @@ static MOTOR_DM_Param_t dm_params[3] = {
|
||||
{.can = BSP_CAN_1, .master_id = 0x16, .can_id = 0x06, .module = MOTOR_DM_J4310, .reverse = false,},
|
||||
};
|
||||
|
||||
// DH参数 (单位: m, rad, kg)
|
||||
// rc[3]: 质心在 DH 连杆坐标系下的坐标 (m)
|
||||
// 来源：URDF <inertial><origin xyz> 经完整RPY旋转矩阵逆变换到 DH 坐标系
|
||||
// 变换公式: rc_dh = R_rpy^(-1) * rc_urdf，其中R_rpy来自URDF joint的rpy参数
|
||||
// 数据来源：fix2026224/arm/urdf/arm.urdf (2026-02-24 重建模更新)
|
||||
//
|
||||
// MIT控制参数kp/kd调优建议：
|
||||
// - kp：位置刚度，值越大响应越快但易振荡。大关节（J2/J3）可适当降低避免抖动
|
||||
// - kd：阻尼系数，值越大越平稳但响应慢。根据实际测试调整
|
||||
// - 设置为0则使用默认值（LZ: kp=10,kd=0.5; DM: kp=50,kd=3）
|
||||
static Arm6dof_DHParams_t dh_params[6] = {
|
||||
// J1: 腰部旋转（LZ电机，垂直轴，负载小） j1 rpy=(0,0,0) → rc_dh=rc_urdf
|
||||
{.theta=0.0f, .d=0.0405f, .a=0.0014f, .alpha=-M_PI_2, .theta_offset=0.0f, .qmin=-15.7f, .qmax=15.7f, .m=2.3045f, .rc={ 0.00669710f, -0.00030098f, 0.04424100f}, .kp=10.0f, .kd=0.0f},
|
||||
// J2: 大臂俯仰（LZ电机，负载最大，降低kp避免抖动） j2 rpy=(-90,0,-90)
|
||||
{.theta=0.0f, .d=0.0f, .a=0.388f, .alpha=0.0f, .theta_offset=-M_PI_2, .qmin=-1.57f, .qmax=1.57f, .m=0.8903f, .rc={ 0.26586000f, -0.00044116f, 0.00000094f}, .kp=10.0f, .kd=0.0f},
|
||||
// J3: 小臂（LZ电机，负载中等） j3 rpy=(0,0,-90)
|
||||
{.theta=0.0f, .d=0.002795f, .a=0.047775f, .alpha=-M_PI_2, .theta_offset=-M_PI_2, .qmin=-1.0f, .qmax=3.0f, .m=0.72964f, .rc={ 0.08696299f, 0.00167372f, -0.02500400f}, .kp=20.0f, .kd=0.0f},
|
||||
// J4: 腕部旋转（DM电机，roll轴，0到2π循环） j4 rpy=(-90,0,-90)
|
||||
{.theta=0.0f, .d=0.241f, .a=0.0f, .alpha=M_PI_2, .theta_offset=M_PI, .qmin=0.0f, .qmax=6.3f, .m=0.60215f, .rc={ 0.00207070f, -0.14360000f, 0.00004920f}, .kp=3.0f, .kd=0.0f},
|
||||
// J5: 腕部俯仰（DM电机，负载小，可提高响应） j5 rpy=(90,0,-180)
|
||||
// {.theta=0.0f, .d=0.0f, .a=0.1065f, .alpha=-M_PI_2, .theta_offset=0.0f, .qmin=-1.9f, .qmax=1.9f, .m=0.21817f, .rc={ 0.00001750f, 0.00297341f, 0.05816899f}, .kp=15.0f, .kd=0.0f},
|
||||
{.theta=0.0f, .d=0.0f, .a=0.1065f, .alpha=-M_PI_2, .theta_offset=M_PI_2, .qmin=-1.9f, .qmax=1.9f, .m=0.21817f, .rc={ 0.00001750f, 0.00297341f, 0.05816899f}, .kp=5.0f, .kd=0.0f},
|
||||
// J6: 末端Roll（DM电机，最小负载，roll轴，0到2π循环） j6 rpy=(90,0,180)
|
||||
{.theta=0.0f, .d=0.0040025f, .a=0.0f, .alpha=0.0f, .theta_offset=M_PI, .qmin=0.0f, .qmax=6.3f, .m=0.57513f, .rc={-0.00002134f, 0.09993500f, 0.00053860f}, .kp=5.0f, .kd=0.0f},
|
||||
// J1: origin(0, 0, 0.024) rpy(0,0,0)
|
||||
{.theta=0.0f, .d=0.024f, .a=0.000f, .alpha=0.0f, .theta_offset=0.0f, .qmin=-15.7f, .qmax=15.7f, .m=2.2629f, .rc={-0.00718190f, 0.00031034f, 0.06159800f}},
|
||||
// J2: origin(-0.001395, 0, 0.1015) rpy(1.5708, 0, -1.5708)
|
||||
{.theta=0.0f, .d=0.1015f, .a=-0.00139f, .alpha=M_PI_2, .theta_offset=-M_PI_2, .qmin=-1.57f, .qmax=1.57f, .m=0.97482f, .rc={ 0.00316320f, -0.00227070f, -0.22947000f}},
|
||||
// J3: origin(0.3265, 0, -0.0071975) rpy(0, 0, 0)
|
||||
{.theta=0.0f, .d=-0.00719f, .a=0.3265f, .alpha=0.0f, .theta_offset=0.0f, .qmin=-1.0f, .qmax=3.0f, .m=0.72964f, .rc={ 0.08696300f, 0.00167340f, -0.01780600f}},
|
||||
// J4: origin(0.0905, 0.052775, 0.0058025) rpy(1.5708, 0, 3.1416)
|
||||
{.theta=0.0f, .d=0.05278f, .a=0.0905f, .alpha=M_PI_2, .theta_offset=M_PI, .qmin=0.0f, .qmax=6.3f, .m=0.54148f, .rc={-0.00005530f, 0.10972717f, -0.00230270f}},
|
||||
// J5: origin(0.001627, 0, 0.18467) rpy(1.5708, 0, 0)
|
||||
{.theta=0.0f, .d=0.18467f, .a=0.0016f, .alpha=M_PI_2, .theta_offset=0.0f, .qmin=-1.9f, .qmax=1.9f, .m=0.21817f, .rc={ 0.05654200f, 0.00102680f, -0.00131060f}},
|
||||
// J6: origin(0.10487, 0.0013347, 0) rpy(1.5708, 0, 1.5708)
|
||||
{.theta=0.0f, .d=0.00133f, .a=0.10487f, .alpha=M_PI_2, .theta_offset=M_PI_2, .qmin=0.0f, .qmax=6.3f, .m=0.56255f, .rc={ 0.00001277f, 0.10159000f, -0.00000780f}},
|
||||
};
|
||||
|
||||
static JointControlParams joint_params[6] = {
|
||||
{.qmin=-15.7f, .qmax=15.7f, .kp=10.0f, .kd=0.0f},
|
||||
{.qmin=-1.57f, .qmax=1.57f, .kp=10.0f, .kd=0.0f},
|
||||
{.qmin=-1.0f, .qmax=3.0f, .kp=20.0f, .kd=0.0f},
|
||||
{.qmin=0.0f, .qmax=6.3f, .kp=3.0f, .kd=0.0f},
|
||||
{.qmin=-1.9f, .qmax=1.9f, .kp=5.0f, .kd=0.0f},
|
||||
{.qmin=0.0f, .qmax=6.3f, .kp=5.0f, .kd=0.0f},
|
||||
};
|
||||
|
||||
static float q_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
|
||||
@ -65,6 +63,7 @@ MotorLZ* motors_lz[3] = {nullptr}; // 用于调试查看
|
||||
MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看
|
||||
Joint* joints[6] = {nullptr};
|
||||
RoboticArm* robot_arm = nullptr;
|
||||
|
||||
Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_target_as_current）
|
||||
|
||||
// ============================================================================
|
||||
@ -74,8 +73,9 @@ Arm_CMD_t arm_cmd; // 当前机械臂控制命令（含 target_pose / set_targe
|
||||
// 0 = 仅计算重力补偿力矩，全部电机松弛，观察 gravity_torques_dbg 是否合理
|
||||
// 1 = 仅关节6（Roll轴）输出纯力矩，其余松弛（Roll轴重力补偿接近0，最安全）
|
||||
// 2 = 全部六轴输出（完整重力补偿，GRAVITY_COMP模式）
|
||||
// 3 = 笛卡尔空间控制：控制末端目标位姿，逆运动学解算各关节角度
|
||||
uint8_t test_stage = 3;
|
||||
// 3 = 笛卡尔空间控制（旧版满秩库代数雅可比算法）
|
||||
// 4 = 2.5D 解析降维控制（专为比赛设计的无奇异点稳定版，推荐！）
|
||||
uint8_t test_stage = 1;
|
||||
Arm6dof_JointAngles_t current_angles; // 调试观察：当前各关节角度 (rad)
|
||||
// 重力补偿力矩观察数组（对应关节1~6，单位 N·m）
|
||||
float gravity_torques_dbg[6] = {0.0f};
|
||||
@ -107,6 +107,115 @@ int ik_test_ret = 0; // IK返回码：0=成功，-1=无
|
||||
// cmd task 中的机械臂命令指针（cmd.cpp 中定义），通过 extern 访问以便写回初始位姿
|
||||
extern Arm_CMD_t *cmd_for_arm;
|
||||
|
||||
/**
|
||||
* @brief 将当前世界系末端位姿转换到指定控制坐标系下的 target_pose，
|
||||
* 使 MoveCartesian*(result) 的目标恰好等于当前 cur_world（无初始跳变）
|
||||
* @param frame 1=世界系 2=工具系 3=航向系
|
||||
* @param cur 当前末端位姿（世界系，由 FK 计算得到）
|
||||
*/
|
||||
static Arm6dof_Pose_t SyncTargetToFrame(uint8_t frame, const Arm6dof_Pose_t& cur) {
|
||||
Arm6dof_Pose_t target;
|
||||
switch (frame) {
|
||||
case 2: {
|
||||
// 工具系：p_tool = R_cur^T * p_world，使 R_cur*p_tool = p_world
|
||||
// 姿态置零：R_tool = I，使 R_world = R_cur * I = R_cur（保持当前姿态）
|
||||
float rpy[3] = {cur.yaw, cur.pitch, cur.roll};
|
||||
Matrixf<3,3> RT = robotics::rpy2r(Matrixf<3,1>(rpy)).trans();
|
||||
float p[3] = {cur.x, cur.y, cur.z};
|
||||
Matrixf<3,1> pt = RT * Matrixf<3,1>(p);
|
||||
target.x = pt[0][0]; target.y = pt[1][0]; target.z = pt[2][0];
|
||||
target.yaw = 0.0f; target.pitch = 0.0f; target.roll = 0.0f;
|
||||
break;
|
||||
}
|
||||
case 3: {
|
||||
// 航向系：p_heading = Rz(-yaw)*[x,y]，z 直接赋值
|
||||
// 姿态：R_heading = Rz(-yaw)*R_cur，使 Rz(yaw)*R_heading = R_cur
|
||||
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
|
||||
target.x = cy * cur.x + sy * cur.y;
|
||||
target.y = -sy * cur.x + cy * cur.y;
|
||||
target.z = cur.z;
|
||||
float rpy_neg[3] = {-cur.yaw, 0.0f, 0.0f};
|
||||
float rpy_c[3] = { cur.yaw, cur.pitch, cur.roll};
|
||||
Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_neg))
|
||||
* robotics::rpy2r(Matrixf<3,1>(rpy_c));
|
||||
Matrixf<3,1> rn = robotics::r2rpy(Rn);
|
||||
target.yaw = rn[0][0]; target.pitch = rn[1][0]; target.roll = rn[2][0];
|
||||
break;
|
||||
}
|
||||
default:
|
||||
// 世界系：直接复制
|
||||
target = cur;
|
||||
break;
|
||||
}
|
||||
return target;
|
||||
}
|
||||
|
||||
static void SyncCommandTargetFromCurrent() {
|
||||
Arm6dof_Pose_t sync = SyncTargetToFrame(control_frame, robot_arm->GetEndPose());
|
||||
arm_cmd.target_pose = sync;
|
||||
if (cmd_for_arm) {
|
||||
cmd_for_arm->target_pose = sync;
|
||||
}
|
||||
}
|
||||
|
||||
static void HandleSetZeroRequest() {
|
||||
if (!setzero) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (setzero <= 3) {
|
||||
MOTOR_LZ_SetZero(&lz_params[setzero - 1]);
|
||||
} else if (setzero > 3 && setzero < 7) {
|
||||
MOTOR_DM_SetZero(&dm_params[setzero - 4]);
|
||||
} else if (setzero == 7) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
MOTOR_LZ_SetZero(&lz_params[i]);
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
MOTOR_DM_SetZero(&dm_params[i]);
|
||||
}
|
||||
}
|
||||
setzero = 0;
|
||||
}
|
||||
|
||||
static void RelaxAllMotors() {
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
motors[i]->Relax();
|
||||
}
|
||||
}
|
||||
|
||||
static void MoveBySelectedFrame(const Arm6dof_Pose_t& target) {
|
||||
switch (control_frame) {
|
||||
case 2:
|
||||
robot_arm->MoveCartesianTool(target);
|
||||
break;
|
||||
case 3:
|
||||
robot_arm->MoveCartesianHeading(target);
|
||||
break;
|
||||
default:
|
||||
robot_arm->MoveCartesian(target);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void HandleCartesianModeError() {
|
||||
if (robot_arm->GetState() == MotionState::ERROR) {
|
||||
robot_arm->ResetError();
|
||||
SyncCommandTargetFromCurrent();
|
||||
}
|
||||
}
|
||||
|
||||
static void RunCartesianControl(ControlMode mode) {
|
||||
robot_arm->SetLinVelLimit(traj_lin_vel);
|
||||
robot_arm->SetAngVelLimit(traj_ang_vel);
|
||||
robot_arm->SetMode(mode);
|
||||
MoveBySelectedFrame(arm_cmd.target_pose);
|
||||
robot_arm->Control();
|
||||
HandleCartesianModeError();
|
||||
ik_from_fk_result = robot_arm->GetIkAngles();
|
||||
traj_progress_dbg = robot_arm->GetTrajProgress();
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
void Task_arm_main(void* argument) {
|
||||
@ -138,7 +247,7 @@ void Task_arm_main(void* argument) {
|
||||
motors[4] = motors_dm[1];
|
||||
motors[5] = motors_dm[2];
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
joints[i] = new Joint(i, motors[i], dh_params[i], q_offset[i], ARM_MAIN_FREQ);
|
||||
joints[i] = new Joint(i, motors[i], joint_params[i], q_offset[i], ARM_MAIN_FREQ);
|
||||
}
|
||||
// 关节3与关节2存在机械耦合：J3电机读数包含J2的运动，需减去J2角度才是真实J3角度
|
||||
joints[2]->SetCoupledJoint(joints[1]);
|
||||
@ -148,7 +257,6 @@ void Task_arm_main(void* argument) {
|
||||
robot_arm->AddJoint(i, joints[i]);
|
||||
}
|
||||
robot_arm->Init();
|
||||
// robot_arm->Enable(true);
|
||||
|
||||
// 使能重力补偿
|
||||
robot_arm->EnableGravityCompensation(true);
|
||||
@ -160,96 +268,41 @@ void Task_arm_main(void* argument) {
|
||||
// JOINT_POSITION: 关节位置控制 + 重力补偿前馈
|
||||
robot_arm->SetMode(ControlMode::GRAVITY_COMP);
|
||||
|
||||
// 读取当前末端位姿，同步到 arm_cmd 和 cmd 侧累积 target_pose，防止上电时大范围跳变
|
||||
// 读取当前末端位姿，转换到 control_frame 后同步到 cmd，防止上电时大范围跳变
|
||||
osDelay(100);
|
||||
robot_arm->Update();
|
||||
arm_cmd.target_pose = robot_arm->GetEndPose();
|
||||
if (cmd_for_arm) {
|
||||
cmd_for_arm->target_pose = arm_cmd.target_pose;
|
||||
{
|
||||
SyncCommandTargetFromCurrent();
|
||||
}
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/*---------------------------零点校准---------------------------*/
|
||||
if (setzero) {
|
||||
if (setzero <= 3) {
|
||||
MOTOR_LZ_SetZero(&lz_params[setzero-1]);
|
||||
} else if (setzero > 3&&setzero < 7) {
|
||||
MOTOR_DM_SetZero(&dm_params[setzero-4]);
|
||||
} else if (setzero == 7) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
MOTOR_LZ_SetZero(&lz_params[i]);
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
MOTOR_DM_SetZero(&dm_params[i]);
|
||||
}
|
||||
}
|
||||
setzero = 0;
|
||||
}
|
||||
HandleSetZeroRequest();
|
||||
/*--------------------------------------------------------------*/
|
||||
|
||||
// 更新机械臂状态（读取关节角度、FK、重力补偿计算）
|
||||
robot_arm->Update();
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
|
||||
|
||||
robot_arm->Enable(arm_cmd.enable);
|
||||
|
||||
|
||||
// set_target_as_current：将目标位姿同步为当前实际末端位姿
|
||||
// 同时写回 cmd_for_arm->target_pose，使 cmd 侧增量累积从正确基准继续
|
||||
if (arm_cmd.set_target_as_current) {
|
||||
const Arm6dof_Pose_t& ep = robot_arm->GetEndPose();
|
||||
Arm6dof_Pose_t pose;
|
||||
switch (control_frame) {
|
||||
case 1: { // 世界系
|
||||
pose = ep;
|
||||
arm_cmd.target_pose = pose;
|
||||
if (cmd_for_arm) {
|
||||
cmd_for_arm->target_pose = pose;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2: { // 工具系（航向系：Rz(-yaw)旋转水平分量）
|
||||
float cy = cosf(ep.yaw), sy = sinf(ep.yaw);
|
||||
pose.x = cy * ep.x + sy * ep.y;
|
||||
pose.y = -sy * ep.x + cy * ep.y;
|
||||
pose.z = ep.z;
|
||||
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
||||
float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw)
|
||||
Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr))
|
||||
* robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||
Matrixf<3,1> rpy_h = robotics::r2rpy(R_h);
|
||||
pose.yaw = rpy_h[0][0];
|
||||
pose.pitch = rpy_h[1][0];
|
||||
pose.roll = rpy_h[2][0];
|
||||
arm_cmd.target_pose = pose;
|
||||
if (cmd_for_arm) {
|
||||
cmd_for_arm->target_pose = pose;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 3: { // 工具系（R_cur^T * p_world，姿态置零）
|
||||
float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll};
|
||||
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
|
||||
float p_arr[3] = {ep.x, ep.y, ep.z};
|
||||
Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr);
|
||||
pose.x = p_tool[0][0];
|
||||
pose.y = p_tool[1][0];
|
||||
pose.z = p_tool[2][0];
|
||||
pose.yaw = 0.0f;
|
||||
pose.pitch = 0.0f;
|
||||
pose.roll = 0.0f;
|
||||
arm_cmd.target_pose = pose;
|
||||
if (cmd_for_arm) {
|
||||
cmd_for_arm->target_pose = pose;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
// 使能状态变化沿处理：仅在变化时下发使能，减少冗余调用
|
||||
static bool last_enable = false;
|
||||
if (arm_cmd.enable != last_enable) {
|
||||
robot_arm->Enable(arm_cmd.enable);
|
||||
}
|
||||
}
|
||||
|
||||
// 使能上升沿自动同步：避免使能时 target_pose 与当前实际位姿不一致导致初始跳变
|
||||
if (arm_cmd.enable && !last_enable) {
|
||||
SyncCommandTargetFromCurrent();
|
||||
}
|
||||
last_enable = arm_cmd.enable;
|
||||
|
||||
// set_target_as_current：将 target_pose 同步到当前末端位姿（转换到当前 control_frame）
|
||||
if (arm_cmd.set_target_as_current) {
|
||||
SyncCommandTargetFromCurrent();
|
||||
}
|
||||
|
||||
|
||||
// 每帧同步调试观察变量
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
@ -262,16 +315,24 @@ void Task_arm_main(void* argument) {
|
||||
end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f,
|
||||
ep.roll, ep.pitch, ep.yaw };
|
||||
|
||||
|
||||
osMessageQueueGet(task_runtime.msgq.arm.cmd, &arm_cmd, NULL, 0);
|
||||
|
||||
switch (test_stage) {
|
||||
case 0:
|
||||
// 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
|
||||
for (int i = 0; i < 6; ++i) motors[i]->Relax();
|
||||
//检测电机正电流旋转方向与urdf规定是否一致
|
||||
static float rotationDirectionCheck[6] = {0.0f};
|
||||
joints[0]->TorqueControl(rotationDirectionCheck[0]);
|
||||
joints[1]->TorqueControl(rotationDirectionCheck[1]);
|
||||
joints[2]->TorqueControl(rotationDirectionCheck[2]);
|
||||
joints[3]->TorqueControl(rotationDirectionCheck[3]);
|
||||
joints[4]->TorqueControl(rotationDirectionCheck[4]);
|
||||
joints[5]->TorqueControl(rotationDirectionCheck[5]);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
// 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理
|
||||
RelaxAllMotors();
|
||||
break;
|
||||
|
||||
case 2:
|
||||
// 阶段1：测试单个关节力矩输出（用于验证力矩计算）
|
||||
// 修改下面的索引来测试不同关节：j2=1, j3=2, j5=4
|
||||
motors[0]->Relax();
|
||||
@ -288,31 +349,26 @@ void Task_arm_main(void* argument) {
|
||||
// joints[3]->TorqueControl(gravity_torques_dbg[3]);
|
||||
// joints[5]->TorqueControl(gravity_torques_dbg[5]);
|
||||
break;
|
||||
case 2:
|
||||
case 3:
|
||||
default:
|
||||
// 阶段4：全部六轴 GRAVITY_COMP 模式（位置保持 + 重力补偿前馈）
|
||||
robot_arm->SetMode(ControlMode::GRAVITY_COMP);
|
||||
robot_arm->Control();
|
||||
break;
|
||||
|
||||
case 3:
|
||||
// 笛卡尔空间轨迹规划控制
|
||||
// 修改 target_pose 后，机械臂自动从当前位姿平滑运动到目标位姿
|
||||
robot_arm->SetLinVelLimit(traj_lin_vel);
|
||||
robot_arm->SetAngVelLimit(traj_ang_vel);
|
||||
robot_arm->SetMode(ControlMode::CARTESIAN_POSITION);
|
||||
// robot_arm->MoveCartesianTool(target_pose);
|
||||
// robot_arm->MoveCartesianHeading(target_pose);
|
||||
robot_arm->MoveCartesian(arm_cmd.target_pose);
|
||||
|
||||
|
||||
// robot_arm->StepTrajectory();
|
||||
robot_arm->Control();
|
||||
// for (int i = 0; i < 6; ++i) motors[i]->Relax();
|
||||
|
||||
ik_from_fk_result = robot_arm->GetIkAngles();
|
||||
traj_progress_dbg = robot_arm->GetTrajProgress();
|
||||
case 4:
|
||||
// 笛卡尔空间轨迹规划控制：基于数值逆解(雅可比)
|
||||
RunCartesianControl(ControlMode::CARTESIAN_POSITION);
|
||||
break;
|
||||
|
||||
case 5: {
|
||||
// =================================================================================
|
||||
// ★ 2.5D 解析降维控制算法 ★
|
||||
// 已经集成到底层 arm_oop 的 CARTESIAN_ANALYTICAL 模式中，完全与原轨迹规划流程接轨
|
||||
// =================================================================================
|
||||
RunCartesianControl(ControlMode::CARTESIAN_ANALYTICAL); // 启用降维解析数学模式
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// IK测试：以当前角度为初始猜测，对 target_pose 求逆运动学
|
||||
|
||||
@ -9,6 +9,7 @@
|
||||
/* USER INCLUDE BEGIN */
|
||||
#include "bsp/pwm.h"
|
||||
#include <math.h>
|
||||
#include "device/buzzer.h"
|
||||
/* USER INCLUDE END */
|
||||
|
||||
/* Private typedef ---------------------------------------------------------- */
|
||||
@ -16,7 +17,9 @@
|
||||
/* Private macro ------------------------------------------------------------ */
|
||||
/* Private variables -------------------------------------------------------- */
|
||||
/* USER STRUCT BEGIN */
|
||||
|
||||
BUZZER_t buzzer;
|
||||
static uint16_t count;
|
||||
bool reset=0;
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -36,13 +39,33 @@ void Task_blink(void *argument) {
|
||||
BSP_PWM_Start(BSP_PWM_TIM5_CH1);
|
||||
BSP_PWM_SetComp(BSP_PWM_TIM5_CH2, 0.0f);
|
||||
BSP_PWM_Start(BSP_PWM_TIM5_CH2);
|
||||
BSP_PWM_SetComp(BSP_PWM_TIM5_CH3, 0.0f);
|
||||
BSP_PWM_SetComp(BSP_PWM_TIM5_CH3, 0.0f);
|
||||
BSP_PWM_Start(BSP_PWM_TIM5_CH3);
|
||||
|
||||
BUZZER_Init(&buzzer, BSP_PWM_BUZZER);
|
||||
BUZZER_PlayMusic(&buzzer, MUSIC_NOKIA);
|
||||
|
||||
/* USER CODE INIT END */
|
||||
|
||||
while (1) {
|
||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||
/* USER CODE BEGIN */
|
||||
count++;
|
||||
uint16_t phase = count % 1000;
|
||||
if (count == 1001) count = 1;
|
||||
if (phase == 0) {
|
||||
/* 每秒开始播放C4音符 */
|
||||
BUZZER_Set(&buzzer, 261.63f, 0.5f); // C4音符频率约261.63Hz
|
||||
BUZZER_Start(&buzzer);
|
||||
} else if (phase == 50) {
|
||||
/* 播放100ms后停止 (50/500Hz = 0.1s) */
|
||||
BUZZER_Stop(&buzzer);
|
||||
}
|
||||
if (reset) {
|
||||
__set_FAULTMASK(1); /* 关闭所有中断 */
|
||||
NVIC_SystemReset(); /* 系统复位 */
|
||||
}
|
||||
|
||||
// 呼吸灯 - 基于tick的正弦波
|
||||
float dutyB = (sinf(tick * 0.003f) + 1.0f) * 0.25f; // 0到1之间的正弦波，加快频率
|
||||
float dutyG = (sinf(tick * 0.000f) + 1.0f) * 0.25f; // 0到1之间的正弦波，加快频率
|
||||
|
||||
@ -28,10 +28,14 @@
|
||||
AT9S_t at9s;
|
||||
#endif
|
||||
#ifdef DR16
|
||||
// 将DR16变量放到RAM_D1 (0x24000000)，用于DMA传输
|
||||
// 将DR16变量放到RAM_D1，用于DMA传输并保证32字节对齐以支持Cache操作
|
||||
// __attribute__((section(".ram_d1"))) __attribute__((aligned(32)))
|
||||
DR16_t dr16;
|
||||
static DR16_SwitchPos_t last_sw_l = DR16_SW_ERR; /* 记录左拨杆上一次状态 */
|
||||
#endif
|
||||
|
||||
extern bool reset;
|
||||
#include "stm32f4xx.h"
|
||||
/* USER STRUCT END */
|
||||
|
||||
/* Private function --------------------------------------------------------- */
|
||||
@ -78,6 +82,15 @@ void Task_rc(void *argument) {
|
||||
osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0);
|
||||
// 启动下一次DMA接收
|
||||
DR16_StartDmaRecv(&dr16);
|
||||
if (dr16.header.online) {
|
||||
/* 拨杆从非UP状态切换到UP状态，且复位功能已使能，触发系统复位 */
|
||||
if (
|
||||
dr16.data.sw_l == DR16_SW_UP &&
|
||||
last_sw_l != DR16_SW_UP && last_sw_l != DR16_SW_ERR) {
|
||||
reset=!reset;
|
||||
}
|
||||
last_sw_l = dr16.data.sw_l; /* 更新拨杆状态 */
|
||||
}
|
||||
#endif
|
||||
/* USER CODE END */
|
||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||
|
||||
27
arm.ioc
27
arm.ioc
@ -45,9 +45,10 @@ Mcu.IP3=FREERTOS
|
||||
Mcu.IP4=NVIC
|
||||
Mcu.IP5=RCC
|
||||
Mcu.IP6=SYS
|
||||
Mcu.IP7=TIM5
|
||||
Mcu.IP8=USART3
|
||||
Mcu.IPNb=9
|
||||
Mcu.IP7=TIM4
|
||||
Mcu.IP8=TIM5
|
||||
Mcu.IP9=USART3
|
||||
Mcu.IPNb=10
|
||||
Mcu.Name=STM32F407I(E-G)Hx
|
||||
Mcu.Package=UFBGA176
|
||||
Mcu.Pin0=PB5
|
||||
@ -55,9 +56,11 @@ Mcu.Pin1=PA14
|
||||
Mcu.Pin10=PH12
|
||||
Mcu.Pin11=PH11
|
||||
Mcu.Pin12=PH10
|
||||
Mcu.Pin13=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin14=VP_SYS_VS_Systick
|
||||
Mcu.Pin15=VP_TIM5_VS_ClockSourceINT
|
||||
Mcu.Pin13=PD14
|
||||
Mcu.Pin14=VP_FREERTOS_VS_CMSIS_V2
|
||||
Mcu.Pin15=VP_SYS_VS_Systick
|
||||
Mcu.Pin16=VP_TIM4_VS_ClockSourceINT
|
||||
Mcu.Pin17=VP_TIM5_VS_ClockSourceINT
|
||||
Mcu.Pin2=PA13
|
||||
Mcu.Pin3=PB6
|
||||
Mcu.Pin4=PD0
|
||||
@ -66,7 +69,7 @@ Mcu.Pin6=PC10
|
||||
Mcu.Pin7=PD1
|
||||
Mcu.Pin8=PH0-OSC_IN
|
||||
Mcu.Pin9=PH1-OSC_OUT
|
||||
Mcu.PinsNb=16
|
||||
Mcu.PinsNb=18
|
||||
Mcu.ThirdPartyNb=0
|
||||
Mcu.UserConstants=
|
||||
Mcu.UserName=STM32F407IGHx
|
||||
@ -119,6 +122,8 @@ PD0.Signal=CAN1_RX
|
||||
PD1.Locked=true
|
||||
PD1.Mode=CAN_Activate
|
||||
PD1.Signal=CAN1_TX
|
||||
PD14.Locked=true
|
||||
PD14.Signal=S_TIM4_CH3
|
||||
PH0-OSC_IN.Mode=HSE-External-Oscillator
|
||||
PH0-OSC_IN.Signal=RCC_OSC_IN
|
||||
PH1-OSC_OUT.Mode=HSE-External-Oscillator
|
||||
@ -197,12 +202,18 @@ RCC.VCOI2SOutputFreq_Value=384000000
|
||||
RCC.VCOInputFreq_Value=2000000
|
||||
RCC.VCOOutputFreq_Value=336000000
|
||||
RCC.VcooutputI2S=192000000
|
||||
SH.S_TIM4_CH3.0=TIM4_CH3,PWM Generation3 CH3
|
||||
SH.S_TIM4_CH3.ConfNb=1
|
||||
SH.S_TIM5_CH1.0=TIM5_CH1,PWM Generation1 CH1
|
||||
SH.S_TIM5_CH1.ConfNb=1
|
||||
SH.S_TIM5_CH2.0=TIM5_CH2,PWM Generation2 CH2
|
||||
SH.S_TIM5_CH2.ConfNb=1
|
||||
SH.S_TIM5_CH3.0=TIM5_CH3,PWM Generation3 CH3
|
||||
SH.S_TIM5_CH3.ConfNb=1
|
||||
TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||
TIM4.IPParameters=Channel-PWM Generation3 CH3,Prescaler,Period
|
||||
TIM4.Period=2000
|
||||
TIM4.Prescaler=17-1
|
||||
TIM5.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
|
||||
TIM5.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
|
||||
TIM5.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||
@ -218,6 +229,8 @@ VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
|
||||
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
|
||||
VP_SYS_VS_Systick.Mode=SysTick
|
||||
VP_SYS_VS_Systick.Signal=SYS_VS_Systick
|
||||
VP_TIM4_VS_ClockSourceINT.Mode=Internal
|
||||
VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
|
||||
VP_TIM5_VS_ClockSourceINT.Mode=Internal
|
||||
VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
|
||||
board=custom
|
||||
|
||||
14
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/CMakeLists.txt
Normal file
14
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/CMakeLists.txt
Normal file
@ -0,0 +1,14 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
|
||||
project(xin)
|
||||
|
||||
find_package(catkin REQUIRED)
|
||||
|
||||
catkin_package()
|
||||
|
||||
find_package(roslaunch)
|
||||
|
||||
foreach(dir config launch meshes urdf)
|
||||
install(DIRECTORY ${dir}/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||
endforeach(dir)
|
||||
@ -0,0 +1 @@
|
||||
controller_joint_names: ['', 'j1', 'j2', 'j3', 'j4', 'j5', 'j6', ]
|
||||
1617
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/export.log
Normal file
1617
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/export.log
Normal file
File diff suppressed because one or more lines are too long
@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
<arg
|
||||
name="model" />
|
||||
<param
|
||||
name="robot_description"
|
||||
textfile="$(find xin)/urdf/xin.urdf" />
|
||||
<node
|
||||
name="joint_state_publisher_gui"
|
||||
pkg="joint_state_publisher_gui"
|
||||
type="joint_state_publisher_gui" />
|
||||
<node
|
||||
name="robot_state_publisher"
|
||||
pkg="robot_state_publisher"
|
||||
type="robot_state_publisher" />
|
||||
<node
|
||||
name="rviz"
|
||||
pkg="rviz"
|
||||
type="rviz"
|
||||
args="-d $(find xin)/urdf.rviz" />
|
||||
</launch>
|
||||
@ -0,0 +1,20 @@
|
||||
<launch>
|
||||
<include
|
||||
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||
<node
|
||||
name="tf_footprint_base"
|
||||
pkg="tf"
|
||||
type="static_transform_publisher"
|
||||
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||
<node
|
||||
name="spawn_model"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-file $(find xin)/urdf/xin.urdf -urdf -model xin"
|
||||
output="screen" />
|
||||
<node
|
||||
name="fake_joint_calibration"
|
||||
pkg="rostopic"
|
||||
type="rostopic"
|
||||
args="pub /calibrated std_msgs/Bool true" />
|
||||
</launch>
|
||||
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link1.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link1.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link2.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link2.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link3.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link3.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link4.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link4.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link5.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link5.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link6.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/Link6.STL
Normal file
Binary file not shown.
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/base_link.STL
Normal file
BIN
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/meshes/base_link.STL
Normal file
Binary file not shown.
21
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/package.xml
Normal file
21
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/package.xml
Normal file
@ -0,0 +1,21 @@
|
||||
<package format="2">
|
||||
<name>xin</name>
|
||||
<version>1.0.0</version>
|
||||
<description>
|
||||
<p>URDF Description package for xin</p>
|
||||
<p>This package contains configuration data, 3D models and launch files
|
||||
for xin robot</p>
|
||||
</description>
|
||||
<author>TODO</author>
|
||||
<maintainer email="TODO@email.com" />
|
||||
<license>BSD</license>
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>roslaunch</depend>
|
||||
<depend>robot_state_publisher</depend>
|
||||
<depend>rviz</depend>
|
||||
<depend>joint_state_publisher_gui</depend>
|
||||
<depend>gazebo</depend>
|
||||
<export>
|
||||
<architecture_independent />
|
||||
</export>
|
||||
</package>
|
||||
8
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.csv
Normal file
8
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.csv
Normal file
@ -0,0 +1,8 @@
|
||||
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||
base_link,2.2062E-06,-0.089452,0.015312,0,0,0,1.0633,0.0031488,-7.846E-08,1.0021E-07,0.00202,-1.6521E-08,0.0051396,0,0,0,0,0,0,package://xin/meshes/base_link.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/base_link.STL,,大Y模块-2,坐标系base,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||
Link1,-0.0071819,0.00031034,0.061598,0,0,0,2.2629,0.0035533,1.4877E-06,-3.269E-05,0.0024578,-1.066E-06,0.0036303,0,0,0,0,0,0,package://xin/meshes/Link1.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/Link1.STL,,一号臂驱动单元-1;3.1-二号臂驱动单元-1,坐标系1,基准轴1,j1,continuous,0,0,0.024,0,0,0,base_link,0,0,1,60,3.14,,,,,,,,,,
|
||||
Link2,0.22947,-0.0031632,-0.0022707,0,0,0,0.97482,0.00042566,0.00027677,-1.1303E-15,0.0043535,-6.9832E-13,0.0047666,0,0,0,0,0,0,package://xin/meshes/Link2.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/Link2.STL,,3.1-一号臂-1,坐标系2,基准轴2,j2,revolute,-0.001395,0,0.1015,1.5708,0,-1.5708,Link1,0,0,1,60,3.14,0,3.14,,,,,,,,
|
||||
Link3,0.086963,0.0016734,-0.017806,0,0,0,0.72964,0.00028636,1.1866E-05,-4.7151E-08,0.00040609,8.3967E-07,0.00030158,0,0,0,0,0,0,package://xin/meshes/Link3.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/Link3.STL,,3.1-二号臂主臂-1,坐标系3,基准轴3,j3,revolute,0.3265,0,-0.0071975,0,0,0,Link2,0,0,1,20,3.14,0,1.57,,,,,,,,
|
||||
Link4,5.52951427810755E-05,-0.00230270444841696,0.109727165961792,0,0,0,0.541483189887007,0.000263059603979983,1.23868002229671E-08,-1.24134280810698E-08,0.000284201134854818,1.12515113310631E-06,0.000145605109171895,0,0,0,0,0,0,package://xin/meshes/Link4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://xin/meshes/Link4.STL,,3.1-二号臂回转臂-1,坐标系4,基准轴4,j4,continuous,0.0905,0.052775,0.0058025,1.5708,0,3.1416,Link3,0,0,1,20,3.14,,,,,,,,,,
|
||||
Link5,0.056542,0.0013106,0.0010268,0,0,0,0.21817,8.1275E-05,1.143E-08,5.0118E-07,8.1233E-05,1.1372E-08,8.3976E-05,0,0,0,0,0,0,package://xin/meshes/Link5.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/Link5.STL,,三号臂-1,坐标系5,基准轴5,j5,revolute,0.001627,0,0.18467,1.5708,0,0,Link4,0,0,1,20,3.14,0,3.14,,,,,,,,
|
||||
Link6,-7.7997E-06,1.2767E-05,0.10159,0,0,0,0.56255,0.00025673,-1.884E-06,-1.1047E-07,0.00016698,-7.6179E-08,0.00024233,0,0,0,0,0,0,package://xin/meshes/Link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://xin/meshes/Link6.STL,,机械爪3.1-2,坐标系6,基准轴6,j6,revolute,0.10487,0.0013347,0,1.5708,0,1.5708,Link5,0,0,1,20,3.14,0,3.66,,,,,,,,
|
||||
|
391
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf
Normal file
391
arm_model/机械臂URDF六坐标系不带夹爪/缩小版2026315/xin/urdf/xin.urdf
Normal file
@ -0,0 +1,391 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="xin">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="2.2062E-06 -0.089452 0.015312"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.0633" />
|
||||
<inertia
|
||||
ixx="0.0031488"
|
||||
ixy="-7.846E-08"
|
||||
ixz="1.0021E-07"
|
||||
iyy="0.00202"
|
||||
iyz="-1.6521E-08"
|
||||
izz="0.0051396" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="Link1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.0071819 0.00031034 0.061598"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="2.2629" />
|
||||
<inertia
|
||||
ixx="0.0035533"
|
||||
ixy="1.4877E-06"
|
||||
ixz="-3.269E-05"
|
||||
iyy="0.0024578"
|
||||
iyz="-1.066E-06"
|
||||
izz="0.0036303" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j1"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0 0 0.024"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="Link1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.22947 -0.0031632 -0.0022707"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.97482" />
|
||||
<inertia
|
||||
ixx="0.00042566"
|
||||
ixy="0.00027677"
|
||||
ixz="-1.1303E-15"
|
||||
iyy="0.0043535"
|
||||
iyz="-6.9832E-13"
|
||||
izz="0.0047666" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="-0.001395 0 0.1015"
|
||||
rpy="1.5708 0 -1.5708" />
|
||||
<parent
|
||||
link="Link1" />
|
||||
<child
|
||||
link="Link2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="3.14"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.086963 0.0016734 -0.017806"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.72964" />
|
||||
<inertia
|
||||
ixx="0.00028636"
|
||||
ixy="1.1866E-05"
|
||||
ixz="-4.7151E-08"
|
||||
iyy="0.00040609"
|
||||
iyz="8.3967E-07"
|
||||
izz="0.00030158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.3265 0 -0.0071975"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="Link2" />
|
||||
<child
|
||||
link="Link3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="1.57"
|
||||
effort="20"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="5.52951427810755E-05 -0.00230270444841696 0.109727165961792"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.541483189887007" />
|
||||
<inertia
|
||||
ixx="0.000263059603979983"
|
||||
ixy="1.23868002229671E-08"
|
||||
ixz="-1.24134280810698E-08"
|
||||
iyy="0.000284201134854818"
|
||||
iyz="1.12515113310631E-06"
|
||||
izz="0.000145605109171895" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j4"
|
||||
type="continuous">
|
||||
<origin
|
||||
xyz="0.0905 0.052775 0.0058025"
|
||||
rpy="1.5708 0 3.1416" />
|
||||
<parent
|
||||
link="Link3" />
|
||||
<child
|
||||
link="Link4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
effort="20"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.056542 0.0013106 0.0010268"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.21817" />
|
||||
<inertia
|
||||
ixx="8.1275E-05"
|
||||
ixy="1.143E-08"
|
||||
ixz="5.0118E-07"
|
||||
iyy="8.1233E-05"
|
||||
iyz="1.1372E-08"
|
||||
izz="8.3976E-05" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.001627 0 0.18467"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="Link4" />
|
||||
<child
|
||||
link="Link5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="3.14"
|
||||
effort="20"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="Link6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-7.7997E-06 1.2767E-05 0.10159"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.56255" />
|
||||
<inertia
|
||||
ixx="0.00025673"
|
||||
ixy="-1.884E-06"
|
||||
ixz="-1.1047E-07"
|
||||
iyy="0.00016698"
|
||||
iyz="-7.6179E-08"
|
||||
izz="0.00024233" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="0.75294 0.75294 0.75294 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://xin/meshes/Link6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="j6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0.10487 0.0013347 0"
|
||||
rpy="1.5708 0 1.5708" />
|
||||
<parent
|
||||
link="Link5" />
|
||||
<child
|
||||
link="Link6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="0"
|
||||
upper="3.66"
|
||||
effort="20"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
</robot>
|
||||
@ -1,18 +1,18 @@
|
||||
|
||||
|
||||
|
||||
Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:290, State=BP_STATE_ON
|
||||
GraphedExpression="ik_test_ret", DisplayFormat=DISPLAY_FORMAT_DEC, Color=#e56a6f, Show=0
|
||||
OpenDocument="robotics.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.h", Line=309
|
||||
GraphedExpression="((robot_arm)->traj_).t", Color=#35792b, Show=0
|
||||
GraphedExpression="(((robot_arm)->end_effector_).target).z", Color=#769dda, Show=0
|
||||
GraphedExpression="((arm_cmd).target_pose).z", Color=#b14f0d
|
||||
OpenDocument="main.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/main.c", Line=45
|
||||
OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=265
|
||||
OpenDocument="memcpy.c", FilePath="/build/gnu-tools-for-stm32_13.3.rel1.20250523-0900/src/newlib/newlib/libc/string/memcpy.c", Line=0
|
||||
OpenDocument="stm32f4xx_it.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/stm32f4xx_it.c", Line=77
|
||||
OpenDocument="init.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/init.c", Line=6
|
||||
OpenDocument="ctrl_chassis.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/ctrl_chassis.c", Line=6
|
||||
OpenDocument="cmd.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/cmd.cpp", Line=22
|
||||
OpenDocument="cmd.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/cmd/cmd.c", Line=278
|
||||
OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32Projects_HAL/board-F4/arm/startup_stm32f407xx.s", Line=48
|
||||
OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=266
|
||||
OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=256
|
||||
OpenDocument="motor_base.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/motor_base.hpp", Line=121
|
||||
OpenDocument="joint.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/joint.hpp", Line=29
|
||||
OpenDocument="robotics.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.cpp", Line=251
|
||||
@ -20,7 +20,7 @@ OpenDocument="arm6dof.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/compo
|
||||
OpenDocument="can.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/can.c", Line=0
|
||||
OpenDocument="arm6dof.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.cpp", Line=175
|
||||
OpenDocument="matrix.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/matrix.h", Line=24
|
||||
OpenDocument="main.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/main.c", Line=45
|
||||
OpenDocument="robotics.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/toolbox/robotics.h", Line=309
|
||||
OpenDocument="rc.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/rc.c", Line=20
|
||||
OpenToolbar="Debug", Floating=0, x=0, y=0
|
||||
OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=2, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
|
||||
@ -30,17 +30,17 @@ OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0
|
||||
OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="201;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1143;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
|
||||
OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="201;0", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1148;-71", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0"
|
||||
OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
|
||||
SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
|
||||
TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;639]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
|
||||
TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Class";"Source"], ColWidths=[1594;104;100;100;27;100]
|
||||
TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
|
||||
TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
|
||||
TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret"], ColWidths=[100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;100;100;100;100;110;126;126]
|
||||
TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;279;100]
|
||||
TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ik_test_ret";" ((robot_arm)->traj_).t";" (((robot_arm)->end_effector_).target).z";" ((arm_cmd).target_pose).z"], ColWidths=[100;100;100;100;100;100]
|
||||
TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;144;144;124;154;110;144;126]
|
||||
TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340]
|
||||
WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1
|
||||
@ -53,4 +53,5 @@ WatchedExpression="sync_tool_to_current", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="cmd", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ik_test_enable", RefreshRate=1, Window=Watched Data 1
|
||||
WatchedExpression="ik_test_result", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ik_test_ret", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="ik_test_ret", RefreshRate=5, Window=Watched Data 1
|
||||
WatchedExpression="arm_cmd", RefreshRate=5, Window=Watched Data 1
|
||||
Loading…
Reference in New Issue
Block a user