354 lines
14 KiB
C++
354 lines
14 KiB
C++
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
#include "arm.h"
|
|
#include "robot_def.h"
|
|
#include "dji_motor.h"
|
|
#include "dmmotor.h"
|
|
#include "ins_task.h"
|
|
#include "message_center.h"
|
|
#include "general_def.h"
|
|
#include "bmi088.h"
|
|
#include "user_lib.h"
|
|
#include "bsp_log.h"
|
|
#include "referee_VT.h"
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
#include "matrix.h"
|
|
#include "robotics.h"
|
|
|
|
static DJIMotorInstance *Joint1LeftMotor,*Joint1RightMotor,*Joint2Motor,*Joint56Motor,*Joint65Motor;//定义电机
|
|
static DMMotorInstance *Joint3Motor,*Joint4Motor;
|
|
float arm_gravity_feedforward = 0;
|
|
float q_set[6] ; //六个关节的目标值
|
|
//滤波初始化
|
|
first_order_filter_type_t Qset_filter[6]; //关节目标值滤波
|
|
//机械臂参数初始化
|
|
robotics::Link link[6];
|
|
|
|
float debug_kp=0,debug_ki=0,debug_kd=0,debug_maxout=6000;
|
|
|
|
static Publisher_t *arm_pub; // 云台应用消息发布者(云台反馈给cmd)
|
|
static Subscriber_t *arm_sub; // cmd控制消息订阅者
|
|
static Arm_Upload_Data_s arm_feedback_data; // 回传给cmd的云台状态信息
|
|
static Arm_Ctrl_Cmd_s arm_cmd_recv; // 来自cmd的控制信息
|
|
|
|
void ArmInit()
|
|
{
|
|
Motor_Init_Config_s Joint1LeftMotor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 3.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0,
|
|
.MaxOut = 16384,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 13.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = M3508,
|
|
};
|
|
Motor_Init_Config_s Joint1RightMotor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 3.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0,
|
|
.MaxOut = 16384,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 13.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = M3508,
|
|
};
|
|
|
|
Motor_Init_Config_s Joint2Motor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 3.5f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0,
|
|
.MaxOut = 16384,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 13.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 16000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = M3508,
|
|
};
|
|
|
|
Motor_Init_Config_s Joint3Motor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 1.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 50.0f,
|
|
.Ki = 0.3f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 20,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = DM4340,
|
|
};
|
|
|
|
Motor_Init_Config_s Joint4Motor_Config = {
|
|
.controller_param_init_config = {
|
|
.current_feedforward_ptr = &arm_gravity_feedforward,
|
|
.speed_PID = {
|
|
.Kp = 1.5f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 5.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
|
|
PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
|
|
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.feedforward_flag = CURRENT_FEEDFORWARD,
|
|
},
|
|
.motor_type = DM6006,
|
|
};
|
|
|
|
Motor_Init_Config_s Joint56Motor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 2.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 8.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.1f,
|
|
.MaxOut = 4000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = M2006,
|
|
};
|
|
Motor_Init_Config_s Joint65Motor_Config = {
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 2.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.0f,
|
|
.MaxOut = 10000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
.angle_PID = {
|
|
.Kp = 8.0f,
|
|
.Ki = 0.0f,
|
|
.Kd = 0.1f,
|
|
.MaxOut = 4000,
|
|
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement),
|
|
.IntegralLimit = 10.0F,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.outer_loop_type = ANGLE_LOOP,
|
|
.close_loop_type = static_cast<Closeloop_Type_e>(ANGLE_LOOP | SPEED_LOOP),
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
},
|
|
.motor_type = M2006,
|
|
};
|
|
|
|
Joint1LeftMotor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint1LeftMotor_Config.can_init_config.tx_id = 1;
|
|
Joint1LeftMotor = DJIMotorInit(&Joint1LeftMotor_Config);
|
|
|
|
Joint1RightMotor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint1RightMotor_Config.can_init_config.tx_id = 2;
|
|
Joint1RightMotor = DJIMotorInit(&Joint1RightMotor_Config);
|
|
|
|
Joint2Motor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint2Motor_Config.can_init_config.tx_id = 3;
|
|
Joint2Motor = DJIMotorInit(&Joint2Motor_Config);
|
|
|
|
Joint56Motor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint56Motor_Config.can_init_config.tx_id = 4;
|
|
Joint56Motor = DJIMotorInit(&Joint56Motor_Config);
|
|
|
|
Joint65Motor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint65Motor_Config.can_init_config.tx_id = 5;
|
|
Joint65Motor = DJIMotorInit(&Joint65Motor_Config);
|
|
|
|
Joint3Motor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint3Motor_Config.can_init_config.tx_id = 6;
|
|
Joint3Motor_Config.can_init_config.rx_id = 7;
|
|
Joint3Motor = DMMotorInit(&Joint3Motor_Config);
|
|
|
|
Joint4Motor_Config.can_init_config.fdcan_handle = &hfdcan1;
|
|
Joint4Motor_Config.can_init_config.tx_id = 8;
|
|
Joint4Motor_Config.can_init_config.rx_id = 9;
|
|
Joint4Motor = DMMotorInit(&Joint4Motor_Config);
|
|
|
|
//上传并接收控制中心的命令
|
|
arm_pub = PubRegister("arm_feed", sizeof(Arm_Upload_Data_s));
|
|
arm_sub = SubRegister("arm_cmd", sizeof(Arm_Ctrl_Cmd_s));
|
|
|
|
|
|
|
|
}
|
|
|
|
void ArmTask()
|
|
{
|
|
// 获取云台控制数据
|
|
// 后续增加未收到数据的处理
|
|
SubGetMessage(arm_sub, &arm_cmd_recv);
|
|
|
|
//debug控制参数
|
|
// Joint1LeftMotor->motor_controller.angle_PID.Kp=Joint1RightMotor->motor_controller.angle_PID.Kp = debug_kp;
|
|
// Joint1LeftMotor->motor_controller.angle_PID.Ki=Joint1RightMotor->motor_controller.angle_PID.Ki = debug_ki;
|
|
// Joint1LeftMotor->motor_controller.angle_PID.Kd=Joint1RightMotor->motor_controller.angle_PID.Kd = debug_kd;
|
|
// Joint1LeftMotor->motor_controller.angle_PID.MaxOut=Joint1RightMotor->motor_controller.angle_PID.MaxOut = debug_maxout;
|
|
/* 控制参数计算 ------------------------------------------------------------------------*/
|
|
//腕部pitch重力补偿
|
|
arm_gravity_feedforward = 1.5f * arm_sin_f32(Joint4Motor->measure.position);
|
|
if(arm_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
|
|
{
|
|
DMMotorStop(Joint3Motor);DMMotorStop(Joint4Motor);
|
|
DJIMotorStop(Joint1LeftMotor);DJIMotorStop(Joint1RightMotor);
|
|
DJIMotorStop(Joint56Motor);DJIMotorStop(Joint65Motor);
|
|
DJIMotorStop(Joint2Motor);
|
|
//不使能模式下 目标值为当前值 防止使能甩大臂
|
|
q_set[3] = Joint4Motor->measure.position;
|
|
}
|
|
else
|
|
{
|
|
DMMotorEnable(Joint3Motor);DMMotorEnable(Joint4Motor);
|
|
DJIMotorEnable(Joint1LeftMotor);DJIMotorEnable(Joint1RightMotor);
|
|
DJIMotorEnable(Joint56Motor);DJIMotorEnable(Joint65Motor);
|
|
DJIMotorEnable(Joint2Motor);
|
|
}
|
|
|
|
if(arm_cmd_recv.gimbal_mode == ARM_MANUAL_MODE) //各关节分开控制
|
|
{
|
|
q_set[0] += arm_cmd_recv.joint1 * DEGREE_2_RAD;
|
|
q_set[1] += arm_cmd_recv.joint2 * DEGREE_2_RAD;
|
|
q_set[2] += arm_cmd_recv.joint3 * DEGREE_2_RAD;
|
|
q_set[3] += arm_cmd_recv.joint4 * DEGREE_2_RAD;
|
|
q_set[4] += arm_cmd_recv.joint5 * DEGREE_2_RAD;
|
|
q_set[5] -= arm_cmd_recv.joint6 * DEGREE_2_RAD;
|
|
}
|
|
|
|
|
|
|
|
//先暂时随便限位一下
|
|
|
|
|
|
q_set[2] = float_constrain(q_set[2],JOINT3_MIN* DEGREE_2_RAD,JOINT3_MAX* DEGREE_2_RAD);
|
|
q_set[3] = float_constrain(q_set[3],JOINT4_MIN* DEGREE_2_RAD,JOINT4_MAX* DEGREE_2_RAD);
|
|
|
|
|
|
//q_set[0]需要映射到3508的角度上
|
|
DJIMotorSetRef(Joint1LeftMotor,q_set[0]);
|
|
DJIMotorSetRef(Joint1RightMotor,q_set[0]);
|
|
|
|
//q_set[1]也需要映射到3508的角度上
|
|
DJIMotorSetRef(Joint2Motor,q_set[1]);
|
|
|
|
|
|
DMMotorSetRef(Joint3Motor,q_set[2]);
|
|
DMMotorSetRef(Joint4Motor,q_set[3]);
|
|
|
|
DJIMotorSetRef(Joint56Motor,q_set[4]+q_set[5]);
|
|
DJIMotorSetRef(Joint65Motor,q_set[4]-q_set[5]);
|
|
|
|
|
|
// 设置反馈数据
|
|
|
|
|
|
|
|
// 推送消息
|
|
PubPushMessage(arm_pub, (void *)&arm_feedback_data);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|