加裁判系统前保留内容，导航待测试版本

User/device/remote_control.c

@@ -10,7 +10,7 @@
 #include "bsp/calc_lib.h"
 
 #define R12DS
-//#define DR16
+// #define DR16
 /*
 两个遥控器都是
              x                
@@ -85,25 +85,25 @@ static bool DR16_DataCorrupted(const DR16_t *dr16) {
 
   if ((dr16->data.ch_r_x < REMOTE_CH_VALUE_MIN) ||
       (dr16->data.ch_r_x > REMOTE_CH_VALUE_MAX))
-    return true;
+    return false;
 
   if ((dr16->data.ch_r_y < REMOTE_CH_VALUE_MIN) ||
       (dr16->data.ch_r_y > REMOTE_CH_VALUE_MAX))
-    return true;
+    return false;
 
   if ((dr16->data.ch_l_x < REMOTE_CH_VALUE_MIN) ||
       (dr16->data.ch_l_x > REMOTE_CH_VALUE_MAX))
-    return true;
+    return false;
 
   if ((dr16->data.ch_l_y < REMOTE_CH_VALUE_MIN) ||
       (dr16->data.ch_l_y > REMOTE_CH_VALUE_MAX))
-    return true;
+    return false;
 
-  if (dr16->data.sw_l == 0) return true;
+  if (dr16->data.sw_l == 0) return false;
 
-  if (dr16->data.sw_r == 0) return true;
+  if (dr16->data.sw_r == 0) return false;
 
-  return false;
+  return true;
 }
 
 int8_t DR16_ParseRaw(DR16_t *dr16){

User/module/chassis.c

@@ -83,7 +83,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_mode_t mode ,uint32_t now)
     PID_Reset(&c->pid.chassis_6020anglePid[i]);
     PID_Reset(&c->pid.chassis_3508DAOHANG_pid[i]);
   }
-
+   PID_Reset(&c->pid.daohang_chassis_follow_gimbal_pid);
   c->mode = mode;
   return CHASSIS_OK;
 }
@@ -148,6 +148,7 @@ int8_t chassis_init(Chassis_t *c, Chassis_Param_t *param, float target_freq)
     PID_Init(&c->pid.chassis_3508DAOHANG_pid[i], KPID_MODE_CALC_D, target_freq, &c->param->M3508DAOHANG_param);
   }
   PID_Init(&c->pid.chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->chassis_follow_gimbalPID); 
+  PID_Init(&c->pid.daohang_chassis_follow_gimbal_pid, KPID_MODE_CALC_D, target_freq, &c->param->daohang_chassis_follow_gimbalPID); 
 
   LowPassFilter2p_Init(&c->filled[0], target_freq, 20.0f); // vx
   LowPassFilter2p_Init(&c->filled[1], target_freq, 20.0f); // vy
@@ -411,7 +412,7 @@ case CHASSIS_MODE_DAOHANG:
     // 导航模式
     c->move_vec.Vx = -c_cmd->Vx/1;
     c->move_vec.Vy = c_cmd->Vy/1;
-    c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);;
+    c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 1.44215584f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);;
     break;
 
   default:
@@ -450,6 +451,7 @@ case CHASSIS_MODE_DAOHANG:
                                              c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
     c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
   }
+  break;
 
   case CHASSIS_MODE_DAOHANG:
 for(int i=0;i<4;i++)
@@ -469,6 +471,8 @@ for(int i=0;i<4;i++)
                                              c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt);
     c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
 }
+break;
+
 }
   return CHASSIS_OK;
 }

User/module/chassis.h

@@ -59,7 +59,7 @@ extern "C"
 		KPID_Params_t M3508DAOHANG_param;
 
 		KPID_Params_t chassis_follow_gimbalPID;
-
+		KPID_Params_t daohang_chassis_follow_gimbalPID;
 		MOTOR_RM_Param_t motor_3508_param[4]; // 四个3508电机
 		MOTOR_RM_Param_t motor_6020_param[4]; // 四个6020电机
 
@@ -132,7 +132,7 @@ extern "C"
 			KPID_t chassis_3508VPID[4];
 
 			KPID_t chassis_3508DAOHANG_pid[4];
-			
+			KPID_t daohang_chassis_follow_gimbal_pid;
 			KPID_t chassis_follow_gimbal_pid;
 		} pid;
 

User/module/cmd.c

@@ -155,7 +155,7 @@ int8_t CMD_CtrlSet(Chassis_CMD_t *c_cmd, const CMD_RC_t *rc, Gimbal_CMD_t *g_cmd
             // c_cmd->mode = CHASSIS_MODE_FOLLOW_GIMBAL;
             break;  
         case CMD_SW_DOWN:
-            g_cmd->ctrl_mode = GIMBAL_MODE_AI;
+            g_cmd->ctrl_mode = GIMBAL_MODE_DAOHANG;
             s_cmd->control_mode = SHOOT_AI;
             c_cmd->mode = CHASSIS_MODE_DAOHANG;
             break;

User/module/cmd.h

@@ -106,12 +106,14 @@ typedef struct
         GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
         GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
         GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+          GIMBAL_MODE_DAOHANG, /* 导航模式，云台根据导航数据进行移动 */
     } Gimbal_Mode_t;
     typedef enum
 {
     GIMBAL_MODE_REMOTE,
     GIMBAL_MODE_AI,
   
+
 }GIMBAL_Ctrl_mode_t;
 typedef struct {
   Gimbal_Mode_t mode;

User/module/config.c

@@ -57,7 +57,7 @@ static const Config_Param_t config = {
             // .d_cutoff_freq= -1.0f,
             // .range=-1.0f
             .k = 1.0f,
-            .p = 4.0f,
+            .p = 3.0f,
             .i = 0.0f,
             .d = 0.0f,
             .i_limit = 1.0f,
@@ -68,7 +68,7 @@ static const Config_Param_t config = {
 
         .M3508DAOHANG_param={
             .k = 1.0f,
-            .p = 2.0f,
+            .p = 1.0f,
             .i = 0.0f,
             .d = 0.0f,
             .i_limit = 1.0f,
@@ -76,14 +76,24 @@ static const Config_Param_t config = {
             .d_cutoff_freq = -1.0f,
             .range = -1.0f
         },
-
+        .daohang_chassis_follow_gimbalPID={
-        .chassis_follow_gimbalPID = {
+            .k = 1.0f, 
-            .k = 0.7f, 
+            .p = 2.5f, 
-            .p = 0.5f, 
+            .i = 0.0f, 
-            .i = 0.1f, 
             .d = 0.0f, 
             .i_limit = 1.0f, 
-            .out_limit = 0.5f, 
+            .out_limit = 1.0f, 
+            .d_cutoff_freq = -1.0f, 
+            .range = M_2PI
+        },
+
+        .chassis_follow_gimbalPID = {
+            .k = 0.5f, 
+            .p = 0.5f, 
+            .i = 0.0f, 
+            .d = 0.0f, 
+            .i_limit = 1.0f, 
+            .out_limit = 1.0f, 
             .d_cutoff_freq = -1.0f, 
             .range = M_2PI
         },
@@ -235,6 +245,27 @@ static const Config_Param_t config = {
             .range = -1.0f
         },
 
+.pid.daohang_6020_motor_angle={
+            .k = 1.0f, 
+            .p = 2.0f, 
+            .i = 0.0f, 
+            .d = 0.0f, 
+            .i_limit = 1.0f, 
+            .out_limit = 1.0f, 
+            .d_cutoff_freq = 20.0f, 
+            .range = M_2PI
+        },
+.pid.daohang_4310_motor_angle={
+            .k = 2.0f, 
+            .p = 5.0f, 
+            .i = 0.0f, 
+            .d = 1.0f, 
+            .i_limit = 0.0f, 
+            .out_limit = 2.0f, 
+            .d_cutoff_freq = 20.0f, 
+            .range = M_2PI
+        },
+
         .mech_zero = {
             .yaw_6020 = 1.31f,/*1.31*/
             .yaw_4310 = 2.06f, /*大yaw零点*/

User/module/gimbal.c

@@ -74,6 +74,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
     PID_Reset(&g->pid.pitch_4310_omega);
     PID_Reset(&g->pid.yaw_6020_angle);
     PID_Reset(&g->pid.yaw_6020_omega);
+    PID_Reset(&g->pid.daohang_6020_angle);
+    PID_Reset(&g->pid.daohang_4310_angle);
     LowPassFilter2p_Reset(&g->filter_out.yaw_4310,0.0f);
     LowPassFilter2p_Reset(&g->filter_out.pitch_4310,0.0f);
     LowPassFilter2p_Reset(&g->filter_out.yaw_6020,0.0f);
@@ -119,6 +121,9 @@ int8_t Gimbal_Init(Gimbal_t *g,  Gimbal_Param_t *param,float target_freq)
   /*4310 pitch pid初始化 单环*/
   PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
   PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
+/*导航PID*/
+  PID_Init(&g->pid.daohang_6020_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_6020_motor_angle );
+  PID_Init(&g->pid.daohang_4310_angle,KPID_MODE_CALC_D, target_freq,&g->param->pid.daohang_4310_motor_angle );
   /*输出滤波器初始化*/
   LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out);
   LowPassFilter2p_Init(&g->filter_out.gyro_yaw, target_freq, g->param->low_pass_cutoff_freq.gyro);
@@ -364,6 +369,16 @@ g->setpoint.pit_accl = g_cmd->pit_accl;
     g->out.yaw_6020 = 0.0f;
     break;
 
+    case GIMBAL_MODE_DAOHANG:
+     /*导航模式，NUC直接给出目标角度，云台控制环跟随即可*/
+      g->setpoint.eulr.yaw = g->param->mech_zero.yaw_6020+g->param->travel.yaw_6020*0.5f; // 这里可以直接设置为NUC传来的目标角度
+      g->setpoint.yaw_4310 = 1.44215584f;
+
+      g->out.yaw_6020 = PID_Calc(&g->pid.daohang_6020_angle,g->setpoint.eulr.yaw,
+                                    g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,0.0f,g->dt);
+      g->out.yaw_4310 = PID_Calc(&g->pid.daohang_4310_angle,g->setpoint.yaw_4310,
+                                g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt);
+
     /* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
     // g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
     g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);

User/module/gimbal.h

@@ -42,6 +42,9 @@ extern "C"
             KPID_Params_t yaw_4310_motor_angle;   /* yaw轴4310角位置环PID参数 */
             KPID_Params_t pitch_4310_motor_omega; /* pitch轴角速度环PID参数 */
             KPID_Params_t pitch_4310_motor_angle; /* pitch轴角位置环PID参数 */
+
+            KPID_Params_t daohang_6020_motor_angle; /* 导航模式小yaw角位置环PID参数 */
+            KPID_Params_t daohang_4310_motor_angle; /* 导航模式大yaw角位置环PID参数 */
         } pid;
 
         /* 低通滤波器截止频率 */
@@ -134,6 +137,9 @@ extern "C"
             KPID_t yaw_4310_omega;   /* yaw轴4310角速度环PID */
             KPID_t pitch_4310_angle; /* pitch轴4310角位置环PID */
             KPID_t pitch_4310_omega; /* pitch轴4310角速度环PID */
+
+            KPID_t daohang_6020_angle; /* 导航模式小yaw角位置环PID */
+            KPID_t daohang_4310_angle; /* 导航模式大yaw角位置环PID */
         } pid;
 
         struct

User/task/gimbal.c

@@ -66,7 +66,8 @@ osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
 
 if(ai_gimbal_result_cmd.mode==0){
       cmd_gimbal.set_pitch =gimbal_imu.eulr.pit;
-      cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;			
+  cmd_gimbal.set_yaw=gimbal_imu.eulr.yaw;
+  
     }
     if(ai_gimbal_result_cmd.mode==1)
     {
@@ -76,6 +77,7 @@ if(ai_gimbal_result_cmd.mode==0){
       cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
       cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
       cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
+      
     }
     if(ai_gimbal_result_cmd.mode==2){
       cmd_gimbal.set_pitch=ai_gimbal_result_cmd.gimbal_t.setpoint.pit;
@@ -84,7 +86,10 @@ if(ai_gimbal_result_cmd.mode==0){
       cmd_gimbal.yaw_accl=ai_gimbal_result_cmd.gimbal_t.accl.yaw;
       cmd_gimbal.pit_vel=ai_gimbal_result_cmd.gimbal_t.vel.pit;
       cmd_gimbal.pit_accl=ai_gimbal_result_cmd.gimbal_t.accl.pit;
+      
     }
+
+
 Gimbal_UpdateFeedback(&gimbal);
 
 osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);