HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

全向轮基本调试完毕

This commit is contained in:
shmily744 2024-01-27 09:58:56 +08:00
parent f9077ccee2
commit 98bf20d4bc
9 changed files with 333 additions and 262 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
application/chassis/chassis.c
View File

@ -64,8 +64,8 @@ void ChassisInit() {
.controller_param_init_config = { .controller_param_init_config = {
.speed_PID = { .speed_PID = {
.Kp = 4.0f, // 4.5 .Kp = 4.0f, // 4.5
.Ki = 8.0f, // 1.5 .Ki = 3.0f, // 1.5
.Kd = 0.0015, // 0 .Kd = 0, // 0
.IntegralLimit = 3000, .IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000, .MaxOut = 12000,
@ -224,7 +224,7 @@ void ChassisTask() {
chassis_cmd_recv.wz = 10.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle); chassis_cmd_recv.wz = 10.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
break; break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 4000; chassis_cmd_recv.wz = 20000;
break; break;
default: default:
break; break;
@ -248,18 +248,18 @@ void ChassisTask() {
LimitChassisOutput(); LimitChassisOutput();
// float vofa_send_data[2]; // float vofa_send_data[2];
// vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref; // vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref;
// vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure; // vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure;
// vofa_justfloat_output(vofa_send_data,8,&huart1); // vofa_justfloat_output(vofa_send_data, 8, &huart1);
// 根据电机的反馈速度和IMU(如果有)计算真实速度 // 根据电机的反馈速度和IMU(如果有)计算真实速度
EstimateSpeed(); EstimateSpeed();
//todo: 裁判系统信息移植到消息中心发送 //todo: 裁判系统信息移植到消息中心发送
// 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送 // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// 发送敌方方颜色id // 发送敌方方颜色id
chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color; chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
// 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况 // 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit; // chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0; // chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;

7
application/cmd/robot_cmd.c
View File

@ -125,8 +125,13 @@ static void RemoteControlSet() {
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW; chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE; gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
} }
// else if (switch_is_up(rc_data[TEMP].rc.switch_right))// 右侧开关状态[上],小陀螺
// {
// chassis_cmd_send.chassis_mode = CHASSIS_ROTATE;
// gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
// }
// 云台参数,确定云台控制数据 // 云台参数,确定云台控制数据
if (switch_is_mid(rc_data[TEMP].rc.switch_left) || if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
(vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0 (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&

30
application/gimbal/gimbal.c
View File

@ -27,9 +27,9 @@ void GimbalInit() {
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 0.5, // 8 .Kp = 1.5, // 8
.Ki = 0, .Ki = 0,
.Kd = 0, .Kd = 0.005,
.DeadBand = 0.1, .DeadBand = 0.1,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 100, .IntegralLimit = 100,
@ -37,14 +37,14 @@ void GimbalInit() {
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = 12000, // 50 .Kp = 2000, // 50
.Ki = 3000, // 200 .Ki = 100, // 200
.Kd = 0, .Kd = 0.03,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000, .IntegralLimit = 1200,
.MaxOut = 20000, .MaxOut = 20000,
}, },
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle, .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明 // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2], .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
}, },
@ -55,16 +55,18 @@ void GimbalInit() {
.close_loop_type = SPEED_LOOP | ANGLE_LOOP, .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = GM6020}; .motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL
};
// PITCH // PITCH
Motor_Init_Config_s pitch_config = { Motor_Init_Config_s pitch_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
.tx_id = 4, .tx_id = 1,
}, },
.controller_param_init_config = { .controller_param_init_config = {
.angle_PID = { .angle_PID = {
.Kp = 7.5, // 10 .Kp = 3, // 10
.Ki = 0, .Ki = 0,
.Kd = 0.05, .Kd = 0.05,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
@ -72,7 +74,7 @@ void GimbalInit() {
.MaxOut = 500, .MaxOut = 500,
}, },
.speed_PID = { .speed_PID = {
.Kp = -300, // 50 .Kp = -800, // 50
.Ki = 0, // 350 .Ki = 0, // 350
.Kd = 0, // 0 .Kd = 0, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
@ -91,6 +93,7 @@ void GimbalInit() {
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
}, },
.motor_type = GM6020, .motor_type = GM6020,
.motor_control_type = CURRENT_CONTROL
}; };
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config); yaw_motor = DJIMotorInit(&yaw_config);
@ -144,12 +147,13 @@ void GimbalTask() {
// 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩 // 根据IMU姿态/pitch电机角度反馈计算出当前配重下的重力矩
// ... // ...
//float vofa_send_data[4]; // float vofa_send_data[4];
// vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref; // vofa_send_data[0] = pitch_motor->motor_controller.speed_PID.Ref;
// vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure; // vofa_send_data[1] = pitch_motor->motor_controller.speed_PID.Measure;
// vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref; // vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref;
// vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure; // vofa_send_data[3] = pitch_motor->motor_controller.angle_PID.Measure;
// // vofa_justfloat_output(vofa_send_data, 16, &huart1);
// float vofa_send_data[4];
// vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref; // vofa_send_data[0] = yaw_motor->motor_controller.speed_PID.Ref;
// vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure; // vofa_send_data[1] = yaw_motor->motor_controller.speed_PID.Measure;
// vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref; // vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref;

4
application/robot.c
View File

@ -32,7 +32,7 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit(); RobotCMDInit();
GimbalInit(); GimbalInit();
// ShootInit(); ShootInit();
#endif #endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
@ -50,7 +50,7 @@ void RobotTask()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD) #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDTask(); RobotCMDTask();
GimbalTask(); GimbalTask();
//ShootTask(); ShootTask();
#endif #endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD) #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)

9
application/robot_def.h
View File

@ -26,15 +26,15 @@
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */ /* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
// 云台参数 // 云台参数
#define YAW_CHASSIS_ALIGN_ECD 1995 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改 #define YAW_CHASSIS_ALIGN_ECD 3055 // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度 #define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
#define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
#define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
#define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
// 发射参数 // 发射参数
#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define ONE_BULLET_DELTA_ANGLE 45 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 #define NUM_PER_CIRCLE 8 // 拨盘一圈的装载量
// 机器人底盘修改的参数,单位为m(米) // 机器人底盘修改的参数,单位为m(米)
#define WHEEL_BASE 0.3f // 纵向轴距(前进后退方向) #define WHEEL_BASE 0.3f // 纵向轴距(前进后退方向)
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向) #define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
@ -203,6 +203,7 @@ typedef struct
{ {
// code to go here // code to go here
// ... // ...
uint8_t stalled_flag; //堵转标志
} Shoot_Upload_Data_s; } Shoot_Upload_Data_s;
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1) #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)

280
application/shoot/shoot.c
View File

@ -16,98 +16,123 @@ static Subscriber_t *shoot_sub;
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息 static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
// dwt定时,计算冷却用 // dwt定时,计算冷却用
static float hibernate_time = 0, dead_time = 0; static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;
void ShootInit() void ShootInit()
{ {
// 左摩擦轮 // 左摩擦轮
Motor_Init_Config_s friction_config = { Motor_Init_Config_s friction_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
},
.controller_param_init_config = {
.speed_PID = {
.Kp = 0, // 20
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
.MaxOut = 15000,
}, },
.current_PID = { .controller_param_init_config = {
.Kp = 0, // 0.7 .speed_PID = {
.Ki = 0, // 0.1 .Kp = 1.5f, // 20
.Kd = 0, .Ki = 0.2f, // 1
.Improve = PID_Integral_Limit, .Kd = 0,
.IntegralLimit = 10000, .Improve = PID_Integral_Limit,
.MaxOut = 15000, .IntegralLimit = 10000,
.MaxOut = 15000,
},
.current_PID = {
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 10000,
.MaxOut = 15000,
},
}, },
}, .controller_setting_init_config = {
.controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED,
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, .outer_loop_type = SPEED_LOOP,
.close_loop_type = SPEED_LOOP | CURRENT_LOOP, .close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, .motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
}, },
.motor_type = M3508}; .motor_type = M3508};
friction_config.can_init_config.tx_id = 1, friction_config.can_init_config.tx_id = 2,
friction_l = DJIMotorInit(&friction_config); friction_l = DJIMotorInit(&friction_config);
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行 friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
friction_r = DJIMotorInit(&friction_config); friction_r = DJIMotorInit(&friction_config);
// 拨盘电机 // 拨盘电机
Motor_Init_Config_s loader_config = { Motor_Init_Config_s loader_config = {
.can_init_config = { .can_init_config = {
.can_handle = &hcan2, .can_handle = &hcan2,
.tx_id = 3, .tx_id = 1
},
.controller_param_init_config = {
.angle_PID = {
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kp = 0, // 10
.Ki = 0,
.Kd = 0,
.MaxOut = 200,
}, },
.speed_PID = { .controller_param_init_config = {
.Kp = 0, // 10 .angle_PID = {
.Ki = 0, // 1 // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
.Kd = 0, .Kp = 0, // 10
.Improve = PID_Integral_Limit, .Ki = 0,
.IntegralLimit = 5000, .Kd = 0,
.MaxOut = 5000, .MaxOut = 200,
},
.speed_PID = {
.Kp = 3, // 10
.Ki = 0, // 1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 10000,
},
.current_PID = {
.Kp = 0, // 0.7
.Ki = 0, // 0.1
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 5000,
},
}, },
.current_PID = { .controller_setting_init_config = {
.Kp = 0, // 0.7 .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.Ki = 0, // 0.1 .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.Kd = 0, .close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.Improve = PID_Integral_Limit, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
.IntegralLimit = 5000,
.MaxOut = 5000,
}, },
}, .motor_type = M2006 // 英雄使用m3508
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
},
.motor_type = M2006 // 英雄使用m3508
}; };
loader = DJIMotorInit(&loader_config); loader = DJIMotorInit(&loader_config);
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s)); shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s)); shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
} }
//卡弹检测
void stalled_detect()
{
static float last_detect_time = 0,detect_time = 0;
static float last_total_angle = 0;
static uint8_t stalled_cnt = 0;
last_detect_time = detect_time;
detect_time = DWT_GetTimeline_ms();
if(detect_time - last_detect_time < 200) // 200ms 检测一次
return;
if(shoot_cmd_recv.load_mode == LOAD_BURSTFIRE)
{
float reference_angle = (detect_time - last_detect_time) * loader->motor_controller.pid_ref * 1e3f;
float real_angle = loader->measure.total_angle - last_total_angle;
if(real_angle < reference_angle * 0.2f)
{
//stalled_cnt ++;
shoot_feedback_data.stalled_flag = 1;
}
}
}
/* 机器人发射机构控制核心任务 */ /* 机器人发射机构控制核心任务 */
void ShootTask() void ShootTask()
{ {
// 从cmd获取控制数据 //从cmd获取控制数据
SubGetMessage(shoot_sub, &shoot_cmd_recv); SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止) // 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
@ -126,71 +151,76 @@ void ShootTask()
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
// if (hibernate_time + dead_time > DWT_GetTimeline_ms()) if (hibernate_time + dead_time > DWT_GetTimeline_ms())
// return; return;
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
switch (shoot_cmd_recv.load_mode) switch (shoot_cmd_recv.load_mode)
{ {
// 停止拨盘 // 停止拨盘
case LOAD_STOP: case LOAD_STOP:
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环 DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break; break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间 dead_time = 150; // 完成1发弹丸发射的时间
break; break;
// 三连发,如果不需要后续可能删除 // 三连发,如果不需要后续可能删除
case LOAD_3_BULLET: case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环 DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发 DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成3发弹丸发射的时间 dead_time = 300; // 完成3发弹丸发射的时间
break; break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE: case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP); DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8); DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
break; // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流) break;
// 也有可能需要从switch-case中独立出来 // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
case LOAD_REVERSE: // 也有可能需要从switch-case中独立出来
DJIMotorOuterLoop(loader, SPEED_LOOP); case LOAD_REVERSE:
// ... DJIMotorOuterLoop(loader, SPEED_LOOP);
break; DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
default: hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
while (1) dead_time = 500; // 翻转500ms
; // 未知模式,停止运行,检查指针越界,内存溢出等问题 shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
// ...
break;
default:
while (1)
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
} }
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON) if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{ {
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入 // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
switch (shoot_cmd_recv.bullet_speed) // switch (shoot_cmd_recv.bullet_speed)
{ // {
case SMALL_AMU_15: // case SMALL_AMU_15:
DJIMotorSetRef(friction_l, 0); // DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0); // DJIMotorSetRef(friction_r, 0);
break; // break;
case SMALL_AMU_18: // case SMALL_AMU_18:
DJIMotorSetRef(friction_l, 0); // DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0); // DJIMotorSetRef(friction_r, 0);
break; // break;
case SMALL_AMU_30: // case SMALL_AMU_30:
DJIMotorSetRef(friction_l, 0); // DJIMotorSetRef(friction_l, 0);
DJIMotorSetRef(friction_r, 0); // DJIMotorSetRef(friction_r, 0);
break; // break;
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. //default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
DJIMotorSetRef(friction_l, 30000); DJIMotorSetRef(friction_l, 30000);
DJIMotorSetRef(friction_r, 30000); DJIMotorSetRef(friction_r, 30000);
break; // break;
} // }
} }
else // 关闭摩擦轮 else // 关闭摩擦轮
{ {
@ -207,6 +237,14 @@ void ShootTask()
{ {
//... //...
} }
//卡弹检测
stalled_detect();
// DJIMotorEnable(friction_l);
// DJIMotorEnable(friction_r);
// DJIMotorSetRef(friction_l, 30000);
// DJIMotorSetRef(friction_r, 30000);
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈 // 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data); PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);

234
modules/motor/DJImotor/dji_motor.c
View File

@ -19,13 +19,27 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF * can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF * can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/ */
static CANInstance sender_assignment[6] = { static CANInstance sender_assignment[10] = {
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, 0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, 0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, 0}},
[3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[6] = {.can_handle = &hcan1, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[7] = {.can_handle = &hcan1, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[8] = {.can_handle = &hcan2, .txconf.StdId = 0x1fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}},
[9] = {.can_handle = &hcan2, .txconf.StdId = 0x2fe, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {
0}}
}; };
/** /**
@ -38,78 +52,88 @@ static uint8_t sender_enable_flag[6] = {0};
* @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID, * @brief 根据电调/拨码开关上的ID,根据说明书的默认id分配方式计算发送ID和接收ID,
* 并对电机进行分组以便处理多电机控制命令 * 并对电机进行分组以便处理多电机控制命令
*/ */
static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config) static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *config) {
{
uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值 uint8_t motor_id = config->tx_id - 1; // 下标从零开始,先减一方便赋值
uint8_t motor_send_num; uint8_t motor_send_num;
uint8_t motor_grouping; uint8_t motor_grouping;
switch (motor->motor_type) switch (motor->motor_type) {
{ case M2006:
case M2006: case M3508:
case M3508: if (motor_id < 4) // 根据ID分组
if (motor_id < 4) // 根据ID分组
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
// 计算接收id并设置分组发送id
config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
// 检查是否发生id冲突
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{ {
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); motor_send_num = motor_id;
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2; motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) } else {
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus); motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
} }
}
break;
case GM6020: // 计算接收id并设置分组发送id
if (motor_id < 4) config->rx_id = 0x200 + motor_id + 1; // 把ID+1,进行分组设置
{ sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧
motor_send_num = motor_id; motor->message_num = motor_send_num;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3; motor->sender_group = motor_grouping;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置 // 检查是否发生id冲突
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册 for (size_t i = 0; i < idx; ++i) {
motor->message_num = motor_send_num; if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
motor->sender_group = motor_grouping; dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
LOGERROR(
"[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
for (size_t i = 0; i < idx; ++i) case GM6020:
{ if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{ {
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information."); if (motor_id < 4)
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2; {
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是) motor_send_num = motor_id;
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus); motor_grouping = config->can_handle == &hcan1 ? 6 : 8;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 7 : 9;
}
}else{
if (motor_id < 4)
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
} }
}
break;
default: // other motors should not be registered here config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
while (1) sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册 motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i) {
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle &&
dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id) {
LOGERROR(
"[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
default: // other motors should not be registered here
while (1)
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
} }
} }
@ -119,12 +143,11 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
* *
* @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例 * @param _instance 收到数据的instance,通过遍历与所有电机进行对比以选择正确的实例
*/ */
static void DecodeDJIMotor(CANInstance *_instance) static void DecodeDJIMotor(CANInstance *_instance) {
{
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址 // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针 // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
uint8_t *rxbuff = _instance->rx_buff; uint8_t *rxbuff = _instance->rx_buff;
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id; DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id;
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销 DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
DaemonReload(motor->daemon); DaemonReload(motor->daemon);
@ -132,12 +155,12 @@ static void DecodeDJIMotor(CANInstance *_instance)
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册 // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
measure->last_ecd = measure->ecd; measure->last_ecd = measure->ecd;
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1]; measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1];
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd; measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd;
measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps +
RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])); RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float) ((int16_t) (rxbuff[2] << 8 | rxbuff[3]));
measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current +
CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); CURRENT_SMOOTH_COEF * (float) ((int16_t) (rxbuff[4] << 8 | rxbuff[5]));
measure->temperature = rxbuff[6]; measure->temperature = rxbuff[6];
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了 // 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
@ -148,22 +171,21 @@ static void DecodeDJIMotor(CANInstance *_instance)
measure->total_angle = measure->total_round * 360 + measure->angle_single_round; measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
} }
static void DJIMotorLostCallback(void *motor_ptr) static void DJIMotorLostCallback(void *motor_ptr) {
{ DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr;
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2; uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2;
LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id); LOGWARNING("[dji_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id);
} }
// 电机初始化,返回一个电机实例 // 电机初始化,返回一个电机实例
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
{ DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance));
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
memset(instance, 0, sizeof(DJIMotorInstance)); memset(instance, 0, sizeof(DJIMotorInstance));
// motor basic setting 电机基本设置 // motor basic setting 电机基本设置
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508 instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等 instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
// motor controller init 电机控制器初始化 // motor controller init 电机控制器初始化
PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID); PIDInit(&instance->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
@ -185,9 +207,9 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
// 注册守护线程 // 注册守护线程
Daemon_Init_Config_s daemon_config = { Daemon_Init_Config_s daemon_config = {
.callback = DJIMotorLostCallback, .callback = DJIMotorLostCallback,
.owner_id = instance, .owner_id = instance,
.reload_count = 2, // 20ms未收到数据则丢失 .reload_count = 2, // 20ms未收到数据则丢失
}; };
instance->daemon = DaemonRegister(&daemon_config); instance->daemon = DaemonRegister(&daemon_config);
@ -197,8 +219,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
} }
/* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */ /* 电流只能通过电机自带传感器监测,后续考虑加入力矩传感器应变片等 */
void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback_Source_e type) {
{
if (loop == ANGLE_LOOP) if (loop == ANGLE_LOOP)
motor->motor_settings.angle_feedback_source = type; motor->motor_settings.angle_feedback_source = type;
else if (loop == SPEED_LOOP) else if (loop == SPEED_LOOP)
@ -207,31 +228,26 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback
LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界 LOGERROR("[dji_motor] loop type error, check memory access and func param"); // 检查是否传入了正确的LOOP类型,或发生了指针越界
} }
void DJIMotorStop(DJIMotorInstance *motor) void DJIMotorStop(DJIMotorInstance *motor) {
{
motor->stop_flag = MOTOR_STOP; motor->stop_flag = MOTOR_STOP;
} }
void DJIMotorEnable(DJIMotorInstance *motor) void DJIMotorEnable(DJIMotorInstance *motor) {
{
motor->stop_flag = MOTOR_ENALBED; motor->stop_flag = MOTOR_ENALBED;
} }
/* 修改电机的实际闭环对象 */ /* 修改电机的实际闭环对象 */
void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop) void DJIMotorOuterLoop(DJIMotorInstance *motor, Closeloop_Type_e outer_loop) {
{
motor->motor_settings.outer_loop_type = outer_loop; motor->motor_settings.outer_loop_type = outer_loop;
} }
// 设置参考值 // 设置参考值
void DJIMotorSetRef(DJIMotorInstance *motor, float ref) void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
{
motor->motor_controller.pid_ref = ref; motor->motor_controller.pid_ref = ref;
} }
// 为所有电机实例计算三环PID,发送控制报文 // 为所有电机实例计算三环PID,发送控制报文
void DJIMotorControl() void DJIMotorControl() {
{
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
uint8_t group, num; // 电机组号和组内编号 uint8_t group, num; // 电机组号和组内编号
int16_t set; // 电机控制CAN发送设定值 int16_t set; // 电机控制CAN发送设定值
@ -242,8 +258,7 @@ void DJIMotorControl()
float pid_measure, pid_ref; // 电机PID测量值和设定值 float pid_measure, pid_ref; // 电机PID测量值和设定值
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < idx; ++i) for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用
{ // 减小访存开销,先保存指针引用
motor = dji_motor_instance[i]; motor = dji_motor_instance[i];
motor_setting = &motor->motor_settings; motor_setting = &motor->motor_settings;
motor_controller = &motor->motor_controller; motor_controller = &motor->motor_controller;
@ -254,8 +269,7 @@ void DJIMotorControl()
// pid_ref会顺次通过被启用的闭环充当数据的载体 // pid_ref会顺次通过被启用的闭环充当数据的载体
// 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出
if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) {
{
if (motor_setting->angle_feedback_source == OTHER_FEED) if (motor_setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor_controller->other_angle_feedback_ptr; pid_measure = *motor_controller->other_angle_feedback_ptr;
else else
@ -265,8 +279,8 @@ void DJIMotorControl()
} }
// 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环
if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) if ((motor_setting->close_loop_type & SPEED_LOOP) &&
{ (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) {
if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD) if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD)
pid_ref += *motor_controller->speed_feedforward_ptr; pid_ref += *motor_controller->speed_feedforward_ptr;
@ -281,22 +295,22 @@ void DJIMotorControl()
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈 // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD) if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr; pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP) if (motor_setting->close_loop_type & CURRENT_LOOP) {
{ //现在电调都有内置电流环，无需pid计算
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref); //pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
} }
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1; pid_ref *= -1;
// 获取最终输出 // 获取最终输出
set = (int16_t)pid_ref; set = (int16_t) pid_ref;
// 分组填入发送数据 // 分组填入发送数据
group = motor->sender_group; group = motor->sender_group;
num = motor->message_num; num = motor->message_num;
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位 sender_assignment[group].tx_buff[2 * num] = (uint8_t) (set >> 8); // 低八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位 sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t) (set & 0x00ff); // 高八位
// 若该电机处于停止状态,直接将buff置零 // 若该电机处于停止状态,直接将buff置零
if (motor->stop_flag == MOTOR_STOP) if (motor->stop_flag == MOTOR_STOP)
@ -304,10 +318,8 @@ void DJIMotorControl()
} }
// 遍历flag,检查是否要发送这一帧报文 // 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 6; ++i) for (size_t i = 0; i < 10; ++i) {
{ if (sender_enable_flag[i]) {
if (sender_enable_flag[i])
{
CANTransmit(&sender_assignment[i], 1); CANTransmit(&sender_assignment[i], 1);
} }
} }

1
modules/motor/DJImotor/dji_motor.h
View File

@ -58,6 +58,7 @@ typedef struct
uint8_t message_num; uint8_t message_num;
Motor_Type_e motor_type; // 电机类型 Motor_Type_e motor_type; // 电机类型
Motor_Control_Type_e motor_control_type;//电机控制方式
Motor_Working_Type_e stop_flag; // 启停标志 Motor_Working_Type_e stop_flag; // 启停标志
DaemonInstance* daemon; DaemonInstance* daemon;

10
modules/motor/motor_def.h
View File

@ -81,6 +81,15 @@ typedef struct
} Motor_Control_Setting_s; } Motor_Control_Setting_s;
/* 电机控制方式枚举 */
typedef enum
{
CONTROL_TYPE_NONE = 0,
CURRENT_CONTROL,
VOLTAGE_CONTROL,
} Motor_Control_Type_e;
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/ /* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
// 后续增加前馈数据指针 // 后续增加前馈数据指针
typedef struct typedef struct
@ -133,6 +142,7 @@ typedef struct
Motor_Control_Setting_s controller_setting_init_config; Motor_Control_Setting_s controller_setting_init_config;
Motor_Type_e motor_type; Motor_Type_e motor_type;
CAN_Init_Config_s can_init_config; CAN_Init_Config_s can_init_config;
Motor_Control_Type_e motor_control_type;
} Motor_Init_Config_s; } Motor_Init_Config_s;
#endif // !MOTOR_DEF_H #endif // !MOTOR_DEF_H