508 lines
18 KiB
C++
508 lines
18 KiB
C++
|
||
|
||
// /* 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;
|
||
// }
|
||
|