283 lines
11 KiB
C
283 lines
11 KiB
C
#include "to_stretch.h"
|
|
#include "robot_def.h"
|
|
#include "dji_motor.h"
|
|
#include "message_center.h"
|
|
#include "referee_task.h"
|
|
#include "general_def.h"
|
|
#include "bsp_dwt.h"
|
|
#include "referee_UI.h"
|
|
#include "arm_math.h"
|
|
#include "user_lib.h"
|
|
|
|
//to_stretch
|
|
|
|
static Publisher_t *to_stretch_pub; // 用于发布伸缩的数据
|
|
static Subscriber_t *to_stretch_sub; // 用于订阅伸缩的控制命令
|
|
static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv; // 伸缩接收到的控制命令
|
|
static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据
|
|
|
|
static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan;
|
|
|
|
float gravity_feedforward = 5000;
|
|
float l_protract = 0,r_protract = 0;
|
|
float protract_x = 0; //前伸距离
|
|
float l_lift = 0,r_lift = 0;
|
|
float lift_z = 0; //抬升高度
|
|
float ld_offset = 0,rd_offset = 0;
|
|
float lu_offset = 0,ru_offset = 0;
|
|
float kp=3,ki=2,kd=0;//调试用
|
|
|
|
first_order_filter_type_t lu_filter,ru_filter;
|
|
static float lu_spd,ru_spd;
|
|
|
|
PIDInstance protract_position_loop;//前伸位置环
|
|
PIDInstance lift_position_loop;//抬升位置环
|
|
|
|
void To_stretchInit() {
|
|
// 上下
|
|
Motor_Init_Config_s ud_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan1,
|
|
},
|
|
.controller_param_init_config = {
|
|
.speed_PID = {
|
|
.Kp = 3,
|
|
.Ki = 2,
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 5000,
|
|
.MaxOut = 13000,
|
|
},
|
|
.current_feedforward_ptr = &gravity_feedforward,
|
|
},
|
|
.controller_setting_init_config = {
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.outer_loop_type = SPEED_LOOP,
|
|
.close_loop_type = SPEED_LOOP,
|
|
.feedforward_flag = CURRENT_FEEDFORWARD,
|
|
},
|
|
.motor_type = M3508,
|
|
};
|
|
// 前后
|
|
Motor_Init_Config_s fb_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan1,
|
|
},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 10.0f,
|
|
.Ki = 0,
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 100,
|
|
.MaxOut = 5000,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 2,
|
|
.Ki = 0,
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 2500,
|
|
.MaxOut = 5000,
|
|
},
|
|
.other_angle_feedback_ptr = &protract_x,
|
|
},
|
|
.controller_setting_init_config = {
|
|
.angle_feedback_source = OTHER_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.outer_loop_type = SPEED_LOOP,
|
|
.close_loop_type = SPEED_LOOP,
|
|
},
|
|
.motor_type = M3508,
|
|
};
|
|
//上下
|
|
ud_config.can_init_config.tx_id = 5;
|
|
ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
|
ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
|
|
ud_config.controller_param_init_config.other_speed_feedback_ptr = &lu_spd;
|
|
motor_lu = DJIMotorInit(&ud_config);
|
|
|
|
ud_config.can_init_config.tx_id = 6;
|
|
ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
|
ud_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
|
|
ud_config.controller_param_init_config.other_speed_feedback_ptr = &ru_spd;
|
|
motor_ru = DJIMotorInit(&ud_config);
|
|
//前后
|
|
fb_config.can_init_config.tx_id = 7;
|
|
fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
|
fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL;
|
|
motor_ld = DJIMotorInit(&fb_config);
|
|
|
|
fb_config.can_init_config.tx_id = 8;
|
|
fb_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
|
fb_config.controller_setting_init_config.feedback_reverse_flag = FEEDBACK_DIRECTION_REVERSE;
|
|
motor_rd = DJIMotorInit(&fb_config);
|
|
|
|
const float spd_filter_num = 0.02f;
|
|
first_order_filter_init(&lu_filter,5e-3f,&spd_filter_num);
|
|
first_order_filter_init(&ru_filter,5e-3f,&spd_filter_num);
|
|
|
|
PID_Init_Config_s protract_pid_config = {
|
|
.Kp = 2500.0f,
|
|
.Ki = 0,
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 100,
|
|
.MaxOut = 15000,
|
|
};
|
|
PIDInit(&protract_position_loop,&protract_pid_config);
|
|
|
|
PID_Init_Config_s lift_pid_config = {
|
|
.Kp = 2500.0f,
|
|
.Ki = 0,
|
|
.Kd = 0,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
|
.IntegralLimit = 100,
|
|
.MaxOut = 15000,
|
|
};
|
|
PIDInit(&lift_position_loop,&lift_pid_config);
|
|
|
|
// 图传电机
|
|
Motor_Init_Config_s tuchuan_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan2,
|
|
.tx_id = 1,
|
|
},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 20,
|
|
.Ki = 10,
|
|
.Kd = 0,
|
|
.MaxOut = 5000,
|
|
},
|
|
.speed_PID = {
|
|
.Kp = 2.0f,
|
|
.Ki = 1.0f,
|
|
.Kd = 0,
|
|
.Improve = PID_Integral_Limit,
|
|
.IntegralLimit = 5000,
|
|
.MaxOut = 5000,
|
|
},
|
|
},
|
|
.controller_setting_init_config = {
|
|
.angle_feedback_source = MOTOR_FEED,
|
|
.speed_feedback_source = MOTOR_FEED,
|
|
.outer_loop_type = SPEED_LOOP,
|
|
.close_loop_type = SPEED_LOOP,
|
|
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
|
},
|
|
.motor_type = M2006
|
|
};
|
|
tuchuan = DJIMotorInit(&tuchuan_config);
|
|
|
|
to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
|
|
to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));
|
|
|
|
}
|
|
|
|
/* 机器人伸缩控制核心任务 */
|
|
void To_stretchTask()
|
|
{
|
|
motor_lu->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp;
|
|
motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki;
|
|
motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd;
|
|
|
|
motor_ru->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp;
|
|
motor_ru->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki;
|
|
motor_ru->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd;
|
|
|
|
static uint8_t init_flag = FALSE;
|
|
if(init_flag == FALSE)
|
|
{
|
|
ld_offset = motor_ld->measure.total_angle ;
|
|
rd_offset = motor_rd->measure.total_angle ;
|
|
|
|
lu_offset = motor_lu->measure.total_angle ;
|
|
ru_offset = motor_ru->measure.total_angle ;
|
|
|
|
init_flag = TRUE;
|
|
}
|
|
l_protract = -(motor_ld->measure.total_angle - ld_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
|
|
r_protract = (motor_rd->measure.total_angle - rd_offset) / 19.0f * DEGREE_2_RAD * 20.0f;
|
|
|
|
l_lift = (motor_lu->measure.total_angle - lu_offset) / 19.0f * DEGREE_2_RAD * 20.0f; //单位mm
|
|
r_lift = -(motor_ru->measure.total_angle - ru_offset) / 19.0f * DEGREE_2_RAD * 20.0f;
|
|
|
|
protract_x = (l_protract+r_protract)/2;
|
|
lift_z = (l_lift+r_lift)/2;
|
|
|
|
SubGetMessage(to_stretch_sub, &to_stretch_cmd_recv);
|
|
|
|
first_order_filter_cali(&lu_filter,motor_lu->measure.speed_aps);
|
|
lu_spd = lu_filter.out;
|
|
first_order_filter_cali(&ru_filter, -motor_ru->measure.speed_aps);
|
|
ru_spd = ru_filter.out;
|
|
|
|
float fb_speed_set = PIDCalculate(&protract_position_loop,protract_x,to_stretch_cmd_recv.fb);
|
|
float ud_speed_set = PIDCalculate(&lift_position_loop,lift_z,to_stretch_cmd_recv.ud);
|
|
|
|
if (to_stretch_cmd_recv.to_stretch_mode == TO_STRETCH_ZERO_FORCE)
|
|
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
|
DJIMotorStop(motor_lu);
|
|
DJIMotorStop(motor_ru);
|
|
DJIMotorStop(motor_ld);
|
|
DJIMotorStop(motor_rd);
|
|
DJIMotorStop(tuchuan);
|
|
}
|
|
else
|
|
{ // 正常工作
|
|
DJIMotorEnable(motor_lu);
|
|
DJIMotorEnable(motor_ru);
|
|
DJIMotorEnable(motor_ld);
|
|
DJIMotorEnable(motor_rd);
|
|
DJIMotorEnable(tuchuan);
|
|
}
|
|
|
|
// 根据模式设定伸缩+图传运动形式
|
|
switch (to_stretch_cmd_recv.to_stretch_mode)
|
|
{
|
|
case TO_STRETCH_ZERO_FORCE:
|
|
DJIMotorStop(motor_lu);
|
|
DJIMotorStop(motor_ru);
|
|
DJIMotorStop(motor_ld);
|
|
DJIMotorStop(motor_rd);
|
|
DJIMotorStop(tuchuan);
|
|
break;
|
|
case TO_STRETCH_MODE:
|
|
DJIMotorEnable(motor_lu);
|
|
DJIMotorEnable(motor_ru);
|
|
DJIMotorEnable(motor_ld);
|
|
DJIMotorEnable(motor_rd);
|
|
|
|
DJIMotorSetRef(motor_lu, ud_speed_set);
|
|
DJIMotorSetRef(motor_ru, -ud_speed_set);
|
|
DJIMotorSetRef(motor_ld, -fb_speed_set);
|
|
DJIMotorSetRef(motor_rd, fb_speed_set);
|
|
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
|
|
break;
|
|
case TO_STRETCH_KEEP:
|
|
DJIMotorEnable(motor_lu);
|
|
DJIMotorEnable(motor_ru);
|
|
DJIMotorEnable(motor_ld);
|
|
DJIMotorEnable(motor_rd);
|
|
DJIMotorEnable(tuchuan);
|
|
DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// 推送反馈消息
|
|
|
|
to_stretch_feedback_data.protract_x = protract_x;
|
|
to_stretch_feedback_data.lift_z = lift_z;
|
|
#ifdef ONE_BOARD
|
|
PubPushMessage(to_stretch_pub, (void *)&to_stretch_feedback_data);
|
|
#endif
|
|
#ifdef CHASSIS_BOARD
|
|
CANCommSend(chasiss_can_comm, (void *)&chassis_feedback_data);
|
|
#endif // CHASSIS_BOARD
|
|
}
|
|
|