Merge remote-tracking branch 'origin/master'

# Conflicts:
#	application/cmd/robot_cmd.c
#	engineering.jdebug.user

application/chassis/chassis.c

@@ -45,7 +45,7 @@ static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数
 
 
 static SuperCapInstance *cap;                                       // 超级电容
-static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
+static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb;
 float chassis_power = 0;
 static first_order_filter_type_t vx_filter;
 static first_order_filter_type_t vy_filter;
@@ -111,7 +111,6 @@ void ChassisInit()
     first_order_filter_init(&vy_filter,0.002f,&chassis_y_order_filter);
     first_order_filter_init(&vx_filter,0.002f,&chassis_x_order_filter);
 
-
     // 发布订阅初始化,如果为双板,则需要can comm来传递消息
 #ifdef CHASSIS_BOARD
     Chassis_IMU_data = INS_Init(); // 底盘IMU初始化

application/cmd/robot_cmd.c

@@ -116,6 +116,13 @@ static void CalcOffsetAngle() {
 #endif
 }
 
+static void death_check()
+{
+    if(referee_data->GameRobotState.remain_HP)
+    {
+        gimbal_cmd_send.MotorEnableFlag=5;
+    }
+}
 /**
  * @brief 控制输入为遥控器(调试时)的模式和控制量设置
  *
@@ -128,7 +135,7 @@ static void update_ui_data() {
  // 出招表
 
 static void RemoteControlSet() {
-    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],底盘运动,伸缩保持不动
+    if (switch_is_down(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[下],底盘运动,伸缩保持不动
     {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_KEEP;
@@ -137,8 +144,11 @@ static void RemoteControlSet() {
         chassis_cmd_send.vx = 15.0f * (float) rc_data[TEMP].rc.rocker_r_; // _水平方向
         chassis_cmd_send.vy = 15.0f * (float) rc_data[TEMP].rc.rocker_r1; // 1数值方向
         chassis_cmd_send.wz = -1.0f * (float) rc_data[TEMP].rc.rocker_l_; //旋转
-
+        to_stretch_cmd_send.tc += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//图传
-    } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩
+        // 图传限位
+        if (to_stretch_cmd_send.tc >= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MAX_ANGLE;
+        if (to_stretch_cmd_send.tc <= TUCHUAN_MAX_ANGLE) to_stretch_cmd_send.tc = TUCHUAN_MIN_ANGLE;
+    } else if (switch_is_mid(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[中],底盘平移，伸缩运动
     {
         chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
         to_stretch_cmd_send.to_stretch_mode = TO_STRETCH_MODE;
@@ -149,6 +159,7 @@ static void RemoteControlSet() {
 
         to_stretch_cmd_send.ud += 0.0025F * (float) rc_data[TEMP].rc.rocker_l_;//抬升
         to_stretch_cmd_send.fb += 0.0025F * (float) rc_data[TEMP].rc.rocker_l1;//前伸
+
         //伸缩限位待添加
 //        if (gimbal_cmd_send.pitch >= PITCH_MAX_ANGLE) gimbal_cmd_send.pitch = PITCH_MAX_ANGLE;
 //        if (gimbal_cmd_send.pitch <= PITCH_MIN_ANGLE) gimbal_cmd_send.pitch = PITCH_MIN_ANGLE;

application/gimbal/gimbal.cpp

@@ -253,12 +253,22 @@ void GimbalInit()
     };
     PIDInit(&diff_roll_spd_loop,&diff_roll_spd_config);
 
-
     gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
     // YAW
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 }
+//static void DMMotroEnable()
+//{
+//    if(gimbal_cmd_recv.MotorEnableFlag)
+//    {
+//        DMMotorSetMode(DM_CMD_MOTOR_MODE,yaw_motor);
+//        DMMotorSetMode(DM_CMD_MOTOR_MODE,pitch_motor);
+//        DMMotorSetMode(DM_CMD_MOTOR_MODE,roll_motor);
+//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_r_motor );
+//        DMMotorSetMode(DM_CMD_MOTOR_MODE,diff_l_motor );
+//    }
+//}
 
 /* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()

application/robot.c

@@ -31,8 +31,8 @@ void RobotInit()
 
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDInit();
-    GimbalInit();
+    //GimbalInit();
-    //To_stretchInit();
+    To_stretchInit();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
@@ -49,8 +49,8 @@ void RobotTask()
 {
 #if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
     RobotCMDTask();
-    GimbalTask();
+    //GimbalTask();
-    //To_stretchTask();
+    To_stretchTask();
 #endif
 
 #if defined(ONE_BOARD) || defined(CHASSIS_BOARD)

application/robot_def.h

@@ -35,15 +35,14 @@
 
 #define YAW 60
 
-
-
-
 // 云台参数
 #define YAW_CHASSIS_ALIGN_ECD 0  // 云台和底盘对齐指向相同方向时的电机position值,若对云台有机械改动需要修改
 #define YAW_ECD_GREATER_THAN_4096 1 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
 #define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
 #define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
 #define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
+#define TUCHUAN_MAX_ANGLE 180         // 图传电机最大角度
+#define TUCHUAN_MIN_ANGLE 0         // 图传电机最小角度
 // 发射参数
 #define ONE_BULLET_DELTA_ANGLE 1933.272    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
 #define REDUCTION_RATIO_LOADER 27.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f//初始是49，增加为27
@@ -148,13 +147,14 @@ typedef struct
     float diff_pitch; //差速器pitch
     float diff_roll;  //差速器roll
     float chassis_rotate_wz;
-
+    uint8_t MotorEnableFlag;
     gimbal_mode_e gimbal_mode;
 } Gimbal_Ctrl_Cmd_s;
 
 // cmd发布的伸缩控制数据,由to_stretch订阅
 typedef struct
 {
+    float tc;
     float ud;
     float fb;
     to_stretch_mode_e to_stretch_mode;
@@ -190,7 +190,9 @@ typedef struct
 
 typedef struct
 {
-
+    float gravity_feedforward;
+    float protract_x;  //前伸距离
+    float lift_z;  //抬升高度
 } To_stretch_Upload_Data_s;
 
 #pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)

application/to_stretch/to_stretch.c

@@ -16,7 +16,7 @@ static Subscriber_t *to_stretch_sub;                   // 用于订阅伸缩的
 static To_stretch_Ctrl_Cmd_s to_stretch_cmd_recv;         // 伸缩接收到的控制命令
 static To_stretch_Upload_Data_s to_stretch_feedback_data; // 伸缩回传的反馈数据
 
-static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd;
+static DJIMotorInstance *motor_lu, *motor_ru, *motor_ld, *motor_rd, *tuchuan;
 
 float gravity_feedforward = 3000;
 float l_protract = 0,r_protract = 0;
@@ -25,6 +25,7 @@ float l_lift = 0,r_lift = 0;
 float lift_z = 0;  //抬升高度
 float ld_offset = 0,rd_offset = 0;
 float lu_offset = 0,ru_offset = 0;
+float kp=4,ki=1,kd=0;//调试用
 
 PIDInstance protract_position_loop;//前伸位置环
 PIDInstance lift_position_loop;//抬升位置环
@@ -45,24 +46,23 @@ void To_stretchInit() {
                             .MaxOut = 500,
                     },
                     .speed_PID = {
-                            .Kp = 2,
+                            .Kp = 4,
                             .Ki = 1,
                             .Kd = 0,
                             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                             .IntegralLimit = 2500,
-                            .MaxOut = 5000,
+                            .MaxOut = 13000,
                     },
                     .current_feedforward_ptr = &gravity_feedforward,
             },
             .controller_setting_init_config = {
                     .angle_feedback_source = MOTOR_FEED,
                     .speed_feedback_source = MOTOR_FEED,
-                    .outer_loop_type = SPEED_LOOP,//ANGLE_LOOP,
+                    .outer_loop_type = SPEED_LOOP,
-                    .close_loop_type = SPEED_LOOP,//SPEED_LOOP | ANGLE_LOOP,
+                    .close_loop_type = SPEED_LOOP,
                     .feedforward_flag = CURRENT_FEEDFORWARD,
             },
             .motor_type = M3508,
-
     };
     // 前后
     Motor_Init_Config_s fb_config = {
@@ -96,7 +96,6 @@ void To_stretchInit() {
             },
             .motor_type = M3508,
     };
-    // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
     //上下
     ud_config.can_init_config.tx_id = 5;
     ud_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
@@ -124,7 +123,7 @@ void To_stretchInit() {
             .Kd = 0,
             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
             .IntegralLimit = 100,
-            .MaxOut = 20000,
+            .MaxOut = 15000,
     };
     PIDInit(&protract_position_loop,&protract_pid_config);
 
@@ -134,10 +133,43 @@ void To_stretchInit() {
             .Kd = 0,
             .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
             .IntegralLimit = 100,
-            .MaxOut = 20000,
+            .MaxOut = 15000,
     };
     PIDInit(&lift_position_loop,&lift_pid_config);
 
+    // 图传电机
+    Motor_Init_Config_s tuchuan_config = {
+            .can_init_config = {
+                    .can_handle = &hcan1,
+                    .tx_id = 1,
+            },
+            .controller_param_init_config = {
+                    .angle_PID = {
+                            .Kp = 20,
+                            .Ki = 10,
+                            .Kd = 0,
+                            .MaxOut = 5000,
+                    },
+                    .speed_PID = {
+                            .Kp = 2.0f,
+                            .Ki = 1.0f,
+                            .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 = ANGLE_LOOP,
+                    .close_loop_type =  ANGLE_LOOP | SPEED_LOOP,
+                    .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            },
+            .motor_type = M2006
+    };
+    tuchuan = DJIMotorInit(&tuchuan_config);
+
     to_stretch_sub = SubRegister("to_stretch_cmd", sizeof(To_stretch_Ctrl_Cmd_s));
     to_stretch_pub = PubRegister("to_stretch_feed", sizeof(To_stretch_Upload_Data_s));
 
@@ -146,6 +178,10 @@ void To_stretchInit() {
 /* 机器人伸缩控制核心任务 */
 void To_stretchTask()
 {
+    motor_lu->motor_controller.speed_PID.Kp=motor_ru->motor_controller.speed_PID.Kp=kp;
+    motor_lu->motor_controller.speed_PID.Ki=motor_ru->motor_controller.speed_PID.Ki=ki;
+    motor_lu->motor_controller.speed_PID.Kd=motor_ru->motor_controller.speed_PID.Kd=kd;
+
     static uint8_t init_flag = FALSE;
     if(init_flag == FALSE)
     {
@@ -177,6 +213,7 @@ void To_stretchTask()
         DJIMotorStop(motor_ru);
         DJIMotorStop(motor_ld);
         DJIMotorStop(motor_rd);
+        DJIMotorStop(tuchuan);
     }
     else
     { // 正常工作
@@ -184,9 +221,10 @@ void To_stretchTask()
         DJIMotorEnable(motor_ru);
         DJIMotorEnable(motor_ld);
         DJIMotorEnable(motor_rd);
+        DJIMotorEnable(tuchuan);
     }
 
-    // 根据模式设定运动形式
+    // 根据模式设定伸缩+图传运动形式
     switch (to_stretch_cmd_recv.to_stretch_mode)
     {
         case TO_STRETCH_ZERO_FORCE:
@@ -194,6 +232,7 @@ void To_stretchTask()
             DJIMotorStop(motor_ru);
             DJIMotorStop(motor_ld);
             DJIMotorStop(motor_rd);
+            DJIMotorStop(tuchuan);
             break;
         case TO_STRETCH_MODE:
             DJIMotorEnable(motor_lu);
@@ -201,8 +240,6 @@ void To_stretchTask()
             DJIMotorEnable(motor_ld);
             DJIMotorEnable(motor_rd);
 
-//            DJIMotorSetRef(motor_ld, 1000);
-//            DJIMotorSetRef(motor_ld, 1000);
             DJIMotorSetRef(motor_lu, ud_speed_set);
             DJIMotorSetRef(motor_ru, -ud_speed_set);
             DJIMotorSetRef(motor_ld, -fb_speed_set);
@@ -213,6 +250,8 @@ void To_stretchTask()
             DJIMotorEnable(motor_ru);
             DJIMotorEnable(motor_ld);
             DJIMotorEnable(motor_rd);
+            DJIMotorEnable(tuchuan);
+            DJIMotorSetRef(tuchuan, to_stretch_cmd_recv.tc);
             break;
         default:
             break;

modules/motor/DMmotor/dmmotor.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

modules/motor/DMmotor/dmmotor.h

@@ -68,7 +68,7 @@ void DMMotorSetRef(DMMotorInstance *motor, float ref);
 void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
 
 void DMMotorEnable(DMMotorInstance *motor);
-
+void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor);
 void DMMotorStop(DMMotorInstance *motor);
 void DMMotorCaliEncoder(DMMotorInstance *motor);
 void DMMotorControlInit();