上电和复活设置大yaw轴电机使能

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)) // 遥控器左侧开关状态为[上],自动模式
         {

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;
 

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;

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)
 {

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);