全向轮基本调试完毕
This commit is contained in:
parent
f9077ccee2
commit
98bf20d4bc
|
|
@ -64,8 +64,8 @@ void ChassisInit() {
|
|||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 4.0f, // 4.5
|
||||
.Ki = 8.0f, // 1.5
|
||||
.Kd = 0.0015, // 0
|
||||
.Ki = 3.0f, // 1.5
|
||||
.Kd = 0, // 0
|
||||
.IntegralLimit = 3000,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.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);
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
chassis_cmd_recv.wz = 4000;
|
||||
chassis_cmd_recv.wz = 20000;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -248,18 +248,18 @@ void ChassisTask() {
|
|||
LimitChassisOutput();
|
||||
|
||||
// float vofa_send_data[2];
|
||||
// vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
|
||||
// vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
|
||||
// vofa_justfloat_output(vofa_send_data,8,&huart1);
|
||||
// vofa_send_data[0] = motor_lb->motor_controller.speed_PID.Ref;
|
||||
// vofa_send_data[1] = motor_lb->motor_controller.speed_PID.Measure;
|
||||
// vofa_justfloat_output(vofa_send_data, 8, &huart1);
|
||||
|
||||
// 根据电机的反馈速度和IMU(如果有)计算真实速度
|
||||
EstimateSpeed();
|
||||
|
||||
//todo: 裁判系统信息移植到消息中心发送
|
||||
// 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||||
// 发送敌方方颜色id
|
||||
chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
// 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
|
||||
// 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||||
// 发送敌方方颜色id
|
||||
chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
// 当前只做了17mm热量的数据获取,后续根据robot_def中的宏切换双枪管和英雄42mm的情况
|
||||
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
|
||||
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
|
||||
|
||||
|
|
|
|||
|
|
@ -125,8 +125,13 @@ static void RemoteControlSet() {
|
|||
chassis_cmd_send.chassis_mode = CHASSIS_NO_FOLLOW;
|
||||
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) ||
|
||||
(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 &&
|
||||
|
|
|
|||
|
|
@ -27,9 +27,9 @@ void GimbalInit() {
|
|||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 0.5, // 8
|
||||
.Kp = 1.5, // 8
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.Kd = 0.005,
|
||||
.DeadBand = 0.1,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 100,
|
||||
|
|
@ -37,14 +37,14 @@ void GimbalInit() {
|
|||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 12000, // 50
|
||||
.Ki = 3000, // 200
|
||||
.Kd = 0,
|
||||
.Kp = 2000, // 50
|
||||
.Ki = 100, // 200
|
||||
.Kd = 0.03,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 3000,
|
||||
.IntegralLimit = 1200,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
|
||||
.other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
|
||||
// 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
|
||||
.other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
|
||||
},
|
||||
|
|
@ -55,16 +55,18 @@ void GimbalInit() {
|
|||
.close_loop_type = SPEED_LOOP | ANGLE_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020};
|
||||
.motor_type = GM6020,
|
||||
.motor_control_type = CURRENT_CONTROL
|
||||
};
|
||||
// PITCH
|
||||
Motor_Init_Config_s pitch_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 4,
|
||||
.tx_id = 1,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
.Kp = 7.5, // 10
|
||||
.Kp = 3, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0.05,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
|
|
@ -72,7 +74,7 @@ void GimbalInit() {
|
|||
.MaxOut = 500,
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = -300, // 50
|
||||
.Kp = -800, // 50
|
||||
.Ki = 0, // 350
|
||||
.Kd = 0, // 0
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
|
|
@ -91,6 +93,7 @@ void GimbalInit() {
|
|||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = GM6020,
|
||||
.motor_control_type = CURRENT_CONTROL
|
||||
};
|
||||
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
|
||||
yaw_motor = DJIMotorInit(&yaw_config);
|
||||
|
|
@ -144,12 +147,13 @@ void GimbalTask() {
|
|||
// 根据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[1] = pitch_motor->motor_controller.speed_PID.Measure;
|
||||
// vofa_send_data[2] = pitch_motor->motor_controller.angle_PID.Ref;
|
||||
// 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[1] = yaw_motor->motor_controller.speed_PID.Measure;
|
||||
// vofa_send_data[2] = yaw_motor->motor_controller.angle_PID.Ref;
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ void RobotInit()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDInit();
|
||||
GimbalInit();
|
||||
// ShootInit();
|
||||
ShootInit();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
@ -50,7 +50,7 @@ void RobotTask()
|
|||
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
|
||||
RobotCMDTask();
|
||||
GimbalTask();
|
||||
//ShootTask();
|
||||
ShootTask();
|
||||
#endif
|
||||
|
||||
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
|
||||
|
|
|
|||
|
|
@ -26,15 +26,15 @@
|
|||
|
||||
/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.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 PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
|
||||
#define PITCH_MAX_ANGLE 30 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
|
||||
#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 NUM_PER_CIRCLE 10 // 拨盘一圈的装载量
|
||||
#define NUM_PER_CIRCLE 8 // 拨盘一圈的装载量
|
||||
// 机器人底盘修改的参数,单位为m(米)
|
||||
#define WHEEL_BASE 0.3f // 纵向轴距(前进后退方向)
|
||||
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
|
||||
|
|
@ -203,6 +203,7 @@ typedef struct
|
|||
{
|
||||
// code to go here
|
||||
// ...
|
||||
uint8_t stalled_flag; //堵转标志
|
||||
} Shoot_Upload_Data_s;
|
||||
|
||||
#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
|
||||
|
|
|
|||
|
|
@ -16,98 +16,123 @@ static Subscriber_t *shoot_sub;
|
|||
static Shoot_Upload_Data_s shoot_feedback_data; // 来自cmd的发射控制信息
|
||||
|
||||
// dwt定时,计算冷却用
|
||||
static float hibernate_time = 0, dead_time = 0;
|
||||
static float hibernate_time = 0,last_hibernate_time = 0, dead_time = 0;
|
||||
|
||||
void ShootInit()
|
||||
{
|
||||
// 左摩擦轮
|
||||
Motor_Init_Config_s friction_config = {
|
||||
.can_init_config = {
|
||||
.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,
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
},
|
||||
.current_PID = {
|
||||
.Kp = 0, // 0.7
|
||||
.Ki = 0, // 0.1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 10000,
|
||||
.MaxOut = 15000,
|
||||
.controller_param_init_config = {
|
||||
.speed_PID = {
|
||||
.Kp = 1.5f, // 20
|
||||
.Ki = 0.2f, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.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 = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.controller_setting_init_config = {
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP | CURRENT_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
friction_config.can_init_config.tx_id = 1,
|
||||
friction_l = DJIMotorInit(&friction_config);
|
||||
.outer_loop_type = SPEED_LOOP,
|
||||
.close_loop_type = SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_REVERSE,
|
||||
},
|
||||
.motor_type = M3508};
|
||||
friction_config.can_init_config.tx_id = 2,
|
||||
friction_l = DJIMotorInit(&friction_config);
|
||||
|
||||
friction_config.can_init_config.tx_id = 2; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
friction_config.can_init_config.tx_id = 3; // 右摩擦轮,改txid和方向就行
|
||||
friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
friction_r = DJIMotorInit(&friction_config);
|
||||
|
||||
// 拨盘电机
|
||||
Motor_Init_Config_s loader_config = {
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 3,
|
||||
},
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 0, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.MaxOut = 200,
|
||||
.can_init_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 1
|
||||
},
|
||||
.speed_PID = {
|
||||
.Kp = 0, // 10
|
||||
.Ki = 0, // 1
|
||||
.Kd = 0,
|
||||
.Improve = PID_Integral_Limit,
|
||||
.IntegralLimit = 5000,
|
||||
.MaxOut = 5000,
|
||||
.controller_param_init_config = {
|
||||
.angle_PID = {
|
||||
// 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降
|
||||
.Kp = 0, // 10
|
||||
.Ki = 0,
|
||||
.Kd = 0,
|
||||
.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 = {
|
||||
.Kp = 0, // 0.7
|
||||
.Ki = 0, // 0.1
|
||||
.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, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
|
||||
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
|
||||
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
|
||||
},
|
||||
},
|
||||
.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
|
||||
.motor_type = M2006 // 英雄使用m3508
|
||||
};
|
||||
loader = DJIMotorInit(&loader_config);
|
||||
|
||||
shoot_pub = PubRegister("shoot_feed", sizeof(Shoot_Upload_Data_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()
|
||||
{
|
||||
// 从cmd获取控制数据
|
||||
//从cmd获取控制数据
|
||||
SubGetMessage(shoot_sub, &shoot_cmd_recv);
|
||||
|
||||
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
|
||||
|
|
@ -126,71 +151,76 @@ void ShootTask()
|
|||
|
||||
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
|
||||
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
|
||||
// if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||||
// return;
|
||||
if (hibernate_time + dead_time > DWT_GetTimeline_ms())
|
||||
return;
|
||||
|
||||
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
|
||||
switch (shoot_cmd_recv.load_mode)
|
||||
{
|
||||
// 停止拨盘
|
||||
case LOAD_STOP:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
|
||||
break;
|
||||
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
|
||||
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 150; // 完成1发弹丸发射的时间
|
||||
break;
|
||||
// 三连发,如果不需要后续可能删除
|
||||
case LOAD_3_BULLET:
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 300; // 完成3发弹丸发射的时间
|
||||
break;
|
||||
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
|
||||
case LOAD_BURSTFIRE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
|
||||
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
|
||||
break;
|
||||
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
|
||||
// 也有可能需要从switch-case中独立出来
|
||||
case LOAD_REVERSE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
// ...
|
||||
break;
|
||||
default:
|
||||
while (1)
|
||||
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
|
||||
// 停止拨盘
|
||||
case LOAD_STOP:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
|
||||
break;
|
||||
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
|
||||
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 150; // 完成1发弹丸发射的时间
|
||||
break;
|
||||
// 三连发,如果不需要后续可能删除
|
||||
case LOAD_3_BULLET:
|
||||
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
|
||||
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 300; // 完成3发弹丸发射的时间
|
||||
break;
|
||||
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
|
||||
case LOAD_BURSTFIRE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
|
||||
|
||||
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
|
||||
break;
|
||||
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
|
||||
// 也有可能需要从switch-case中独立出来
|
||||
case LOAD_REVERSE:
|
||||
DJIMotorOuterLoop(loader, SPEED_LOOP);
|
||||
DJIMotorSetRef(loader, 8 * 360 * REDUCTION_RATIO_LOADER / 8); // 固定速度反转
|
||||
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
|
||||
dead_time = 500; // 翻转500ms
|
||||
shoot_feedback_data.stalled_flag = 0 ; //清除堵转标志
|
||||
// ...
|
||||
break;
|
||||
default:
|
||||
while (1)
|
||||
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
|
||||
}
|
||||
|
||||
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
|
||||
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
|
||||
{
|
||||
// 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
|
||||
switch (shoot_cmd_recv.bullet_speed)
|
||||
{
|
||||
case SMALL_AMU_15:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
case SMALL_AMU_18:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
case SMALL_AMU_30:
|
||||
DJIMotorSetRef(friction_l, 0);
|
||||
DJIMotorSetRef(friction_r, 0);
|
||||
break;
|
||||
default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 30000);
|
||||
DJIMotorSetRef(friction_r, 30000);
|
||||
break;
|
||||
}
|
||||
// switch (shoot_cmd_recv.bullet_speed)
|
||||
// {
|
||||
// case SMALL_AMU_15:
|
||||
// DJIMotorSetRef(friction_l, 0);
|
||||
// DJIMotorSetRef(friction_r, 0);
|
||||
// break;
|
||||
// case SMALL_AMU_18:
|
||||
// DJIMotorSetRef(friction_l, 0);
|
||||
// DJIMotorSetRef(friction_r, 0);
|
||||
// break;
|
||||
// case SMALL_AMU_30:
|
||||
// DJIMotorSetRef(friction_l, 0);
|
||||
// DJIMotorSetRef(friction_r, 0);
|
||||
// break;
|
||||
//default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速.
|
||||
DJIMotorSetRef(friction_l, 30000);
|
||||
DJIMotorSetRef(friction_r, 30000);
|
||||
// break;
|
||||
// }
|
||||
}
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -19,13 +19,27 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
|
|||
* can1: [0]:0x1FF,[1]:0x200,[2]:0x2FF
|
||||
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
|
||||
*/
|
||||
static CANInstance sender_assignment[6] = {
|
||||
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .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 = {0}},
|
||||
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {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}},
|
||||
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}},
|
||||
[1] = {.can_handle = &hcan1, .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 = {
|
||||
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,
|
||||
* 并对电机进行分组以便处理多电机控制命令
|
||||
*/
|
||||
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_send_num;
|
||||
uint8_t motor_grouping;
|
||||
|
||||
switch (motor->motor_type)
|
||||
{
|
||||
case M2006:
|
||||
case M3508:
|
||||
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)
|
||||
switch (motor->motor_type) {
|
||||
case M2006:
|
||||
case M3508:
|
||||
if (motor_id < 4) // 根据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);
|
||||
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;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case GM6020:
|
||||
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;
|
||||
}
|
||||
// 计算接收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;
|
||||
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为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.");
|
||||
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)
|
||||
{
|
||||
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)
|
||||
case GM6020:
|
||||
if(motor->motor_control_type == CURRENT_CONTROL) //6020电流控制帧id 0x1fe 0x2fe
|
||||
{
|
||||
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);
|
||||
if (motor_id < 4)
|
||||
{
|
||||
motor_send_num = motor_id;
|
||||
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
|
||||
while (1)
|
||||
LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册
|
||||
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
|
||||
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
|
||||
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,通过遍历与所有电机进行对比以选择正确的实例
|
||||
*/
|
||||
static void DecodeDJIMotor(CANInstance *_instance)
|
||||
{
|
||||
static void DecodeDJIMotor(CANInstance *_instance) {
|
||||
// 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
|
||||
// _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
|
||||
uint8_t *rxbuff = _instance->rx_buff;
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)_instance->id;
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *) _instance->id;
|
||||
DJI_Motor_Measure_s *measure = &motor->measure; // measure要多次使用,保存指针减小访存开销
|
||||
|
||||
DaemonReload(motor->daemon);
|
||||
|
|
@ -132,12 +155,12 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
|||
|
||||
// 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
|
||||
measure->last_ecd = measure->ecd;
|
||||
measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd;
|
||||
measure->ecd = ((uint16_t) rxbuff[0]) << 8 | rxbuff[1];
|
||||
measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float) measure->ecd;
|
||||
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 +
|
||||
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];
|
||||
|
||||
// 多圈角度计算,前提是假设两次采样间电机转过的角度小于180°,自己画个图就清楚计算过程了
|
||||
|
|
@ -148,22 +171,21 @@ static void DecodeDJIMotor(CANInstance *_instance)
|
|||
measure->total_angle = measure->total_round * 360 + measure->angle_single_round;
|
||||
}
|
||||
|
||||
static void DJIMotorLostCallback(void *motor_ptr)
|
||||
{
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *)motor_ptr;
|
||||
static void DJIMotorLostCallback(void *motor_ptr) {
|
||||
DJIMotorInstance *motor = (DJIMotorInstance *) motor_ptr;
|
||||
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);
|
||||
}
|
||||
|
||||
// 电机初始化,返回一个电机实例
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config)
|
||||
{
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *)malloc(sizeof(DJIMotorInstance));
|
||||
DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) {
|
||||
DJIMotorInstance *instance = (DJIMotorInstance *) malloc(sizeof(DJIMotorInstance));
|
||||
memset(instance, 0, sizeof(DJIMotorInstance));
|
||||
|
||||
// motor basic setting 电机基本设置
|
||||
instance->motor_type = config->motor_type; // 6020 or 2006 or 3508
|
||||
instance->motor_settings = config->controller_setting_init_config; // 正反转,闭环类型等
|
||||
instance->motor_control_type = config->motor_control_type; //电流控制or电压控制
|
||||
|
||||
// motor controller init 电机控制器初始化
|
||||
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 = {
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
.callback = DJIMotorLostCallback,
|
||||
.owner_id = instance,
|
||||
.reload_count = 2, // 20ms未收到数据则丢失
|
||||
};
|
||||
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)
|
||||
motor->motor_settings.angle_feedback_source = type;
|
||||
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类型,或发生了指针越界
|
||||
}
|
||||
|
||||
void DJIMotorStop(DJIMotorInstance *motor)
|
||||
{
|
||||
void DJIMotorStop(DJIMotorInstance *motor) {
|
||||
motor->stop_flag = MOTOR_STOP;
|
||||
}
|
||||
|
||||
void DJIMotorEnable(DJIMotorInstance *motor)
|
||||
{
|
||||
void DJIMotorEnable(DJIMotorInstance *motor) {
|
||||
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;
|
||||
}
|
||||
|
||||
// 设置参考值
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
|
||||
{
|
||||
void DJIMotorSetRef(DJIMotorInstance *motor, float ref) {
|
||||
motor->motor_controller.pid_ref = ref;
|
||||
}
|
||||
|
||||
// 为所有电机实例计算三环PID,发送控制报文
|
||||
void DJIMotorControl()
|
||||
{
|
||||
void DJIMotorControl() {
|
||||
// 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性
|
||||
uint8_t group, num; // 电机组号和组内编号
|
||||
int16_t set; // 电机控制CAN发送设定值
|
||||
|
|
@ -242,8 +258,7 @@ void DJIMotorControl()
|
|||
float pid_measure, pid_ref; // 电机PID测量值和设定值
|
||||
|
||||
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
|
||||
for (size_t i = 0; i < idx; ++i)
|
||||
{ // 减小访存开销,先保存指针引用
|
||||
for (size_t i = 0; i < idx; ++i) { // 减小访存开销,先保存指针引用
|
||||
motor = dji_motor_instance[i];
|
||||
motor_setting = &motor->motor_settings;
|
||||
motor_controller = &motor->motor_controller;
|
||||
|
|
@ -254,8 +269,7 @@ void DJIMotorControl()
|
|||
|
||||
// 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)
|
||||
pid_measure = *motor_controller->other_angle_feedback_ptr;
|
||||
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)
|
||||
pid_ref += *motor_controller->speed_feedforward_ptr;
|
||||
|
||||
|
|
@ -281,22 +295,22 @@ void DJIMotorControl()
|
|||
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
|
||||
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor_controller->current_feedforward_ptr;
|
||||
if (motor_setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
if (motor_setting->close_loop_type & CURRENT_LOOP) {
|
||||
//现在电调都有内置电流环，无需pid计算
|
||||
//pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
|
||||
pid_ref *= -1;
|
||||
|
||||
// 获取最终输出
|
||||
set = (int16_t)pid_ref;
|
||||
set = (int16_t) pid_ref;
|
||||
|
||||
// 分组填入发送数据
|
||||
group = motor->sender_group;
|
||||
num = motor->message_num;
|
||||
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] = (uint8_t) (set >> 8); // 低八位
|
||||
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t) (set & 0x00ff); // 高八位
|
||||
|
||||
// 若该电机处于停止状态,直接将buff置零
|
||||
if (motor->stop_flag == MOTOR_STOP)
|
||||
|
|
@ -304,10 +318,8 @@ void DJIMotorControl()
|
|||
}
|
||||
|
||||
// 遍历flag,检查是否要发送这一帧报文
|
||||
for (size_t i = 0; i < 6; ++i)
|
||||
{
|
||||
if (sender_enable_flag[i])
|
||||
{
|
||||
for (size_t i = 0; i < 10; ++i) {
|
||||
if (sender_enable_flag[i]) {
|
||||
CANTransmit(&sender_assignment[i], 1);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@ typedef struct
|
|||
uint8_t message_num;
|
||||
|
||||
Motor_Type_e motor_type; // 电机类型
|
||||
Motor_Control_Type_e motor_control_type;//电机控制方式
|
||||
Motor_Working_Type_e stop_flag; // 启停标志
|
||||
|
||||
DaemonInstance* daemon;
|
||||
|
|
|
|||
|
|
@ -81,6 +81,15 @@ typedef struct
|
|||
|
||||
} Motor_Control_Setting_s;
|
||||
|
||||
/* 电机控制方式枚举 */
|
||||
typedef enum
|
||||
{
|
||||
CONTROL_TYPE_NONE = 0,
|
||||
CURRENT_CONTROL,
|
||||
VOLTAGE_CONTROL,
|
||||
} Motor_Control_Type_e;
|
||||
|
||||
|
||||
/* 电机控制器,包括其他来源的反馈数据指针,3环控制器和电机的参考输入*/
|
||||
// 后续增加前馈数据指针
|
||||
typedef struct
|
||||
|
|
@ -133,6 +142,7 @@ typedef struct
|
|||
Motor_Control_Setting_s controller_setting_init_config;
|
||||
Motor_Type_e motor_type;
|
||||
CAN_Init_Config_s can_init_config;
|
||||
Motor_Control_Type_e motor_control_type;
|
||||
} Motor_Init_Config_s;
|
||||
|
||||
#endif // !MOTOR_DEF_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue