diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 6fe7936..d54ba96 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 13e23ff..58058e2 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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 && diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 153d235..5dc40f2 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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; diff --git a/application/robot.c b/application/robot.c index 7b4c41e..3102179 100644 --- a/application/robot.c +++ b/application/robot.c @@ -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) diff --git a/application/robot_def.h b/application/robot_def.h index 1f88f6a..44cf236 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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) diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e19af5c..b75959d 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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); diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index e9345cc..06f1722 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -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); } } diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index bd07c1a..1130754 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -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; diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index d73476e..868cc97 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -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