diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 0deeca3..ce02b92 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -382,6 +382,8 @@ static void EmergencyHandler() { uint8_t sentry_behave(); /* 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率) */ +static uint8_t is_died; //死亡标志位 +static uint8_t enable_count; void RobotCMDTask() { // 从其他应用获取回传数据 #ifdef ONE_BOARD @@ -396,10 +398,34 @@ void RobotCMDTask() { // 根据gimbal的反馈值计算云台和底盘正方向的夹角,不需要传参,通过static私有变量完成 CalcOffsetAngle(); + //gimbal_cmd_send.enable_motor = 0; + if(referee_data->GameRobotState.current_HP <= 4 ){ + is_died = 1; + } +// if(is_died == 1 && referee_data->GameRobotState.current_HP > 4 ){ +// is_died = 0; +// DWT_Delay(0.08); +// gimbal_cmd_send.enable_motor = 1; +// enable_count++; +// } +// if(enable_count>0 && enable_count <20){ +// DWT_Delay(0.08); +// gimbal_cmd_send.enable_motor = 1; +// enable_count++; +// }else{ +// enable_count = 0; +// } + + if(is_died == 1 && referee_data->GameRobotState.current_HP > 4 ){ + is_died = 0; + gimbal_cmd_send.enable_motor = 500; + } + if(gimbal_cmd_send.enable_motor >0){ + gimbal_cmd_send.enable_motor--; + } + // 根据遥控器左侧开关,确定当前使用的控制模式为遥控器调试还是键鼠 static control_mode_e last_gimbal_mode = 0; - - if (robot_state != ROBOT_STOP && rc_data[TEMP].rc.dial < 300) { if (switch_is_up(rc_data[TEMP].rc.switch_left)) // 遥控器左侧开关状态为[上],自动模式 { diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index af18fa0..446e106 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -176,7 +176,10 @@ void GimbalTask() // 后续增加未收到数据的处理 SubGetMessage(gimbal_sub, &gimbal_cmd_recv); SubGetMessage(chassis_sub, &chassis_cmd_recv); - + if(gimbal_cmd_recv.enable_motor > 0){ + LOGINFO("enabling DMmoter"); + DMMotorSetMode(DM_CMD_MOTOR_MODE, big_yaw_motor); + } //gimbal_cmd_recv.big_yaw = 0; diff --git a/application/robot_def.h b/application/robot_def.h index f039ab2..b462c8d 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -168,6 +168,7 @@ typedef struct float yaw; float pitch; float chassis_rotate_wz; + uint8_t enable_motor; //重新使能电机 control_mode_e control_mode; gimbal_mode_e gimbal_mode; shoot_mode_e shoot_mode; diff --git a/modules/motor/DMmotor/DM4310.c b/modules/motor/DMmotor/DM4310.c index ac2a55e..3e9424e 100644 --- a/modules/motor/DMmotor/DM4310.c +++ b/modules/motor/DMmotor/DM4310.c @@ -25,7 +25,7 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; } -static void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) +void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor) { memset(motor->motor_can_instance->tx_buff, 0xff, 7); // 发送电机指令的时候前面7bytes都是0xff motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id @@ -43,6 +43,8 @@ static void DMMotorDecode(CANInstance *motor_can) measure->last_position = measure->position; + measure->state = 0; + tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16); @@ -71,6 +73,10 @@ static void DMMotorLostCallback(void *motor_ptr) DMMotorInstance *motor = (DMMotorInstance *)motor_ptr; uint16_t can_bus = motor->motor_can_instance->can_handle == &hcan1 ? 1 : 2; LOGWARNING("[dm_motor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instance->tx_id); + + DMMotorEnable(motor); + DMMotorSetMode(DM_CMD_MOTOR_MODE, motor); + LOGWARNING("[dm_motor] Tring to restart motor, can bus [%d] , id [%d]\"", can_bus, motor->motor_can_instance->tx_id); } void DMMotorCaliEncoder(DMMotorInstance *motor) { diff --git a/modules/motor/DMmotor/DM4310.h b/modules/motor/DMmotor/DM4310.h index 3081e92..8050ad3 100644 --- a/modules/motor/DMmotor/DM4310.h +++ b/modules/motor/DMmotor/DM4310.h @@ -70,7 +70,7 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config); void DMMotorSetRef(DMMotorInstance *motor, float ref); void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type); - +void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor); void DMMotorEnable(DMMotorInstance *motor); void DMMotorStop(DMMotorInstance *motor);