简化了电机测量的命名,完成初版平衡底盘的功能编写,待测试方向和符号的正确性

application/balance_chassis/balance.c

@@ -26,12 +26,8 @@ static attitude_t *imu_data;
 static SuperCapInstance *super_cap;
 static referee_info_t *referee_data; // 裁判系统的数据会被不断更新
 // 电机
-static HTMotorInstance *lf;
+static HTMotorInstance *lf, *rf, *lb, *rb;
-static HTMotorInstance *rf;
+static LKMotorInstance *l_driven, *r_driven;
-static HTMotorInstance *lb;
-static HTMotorInstance *rb;
-static LKMotorInstance *l_driven;
-static LKMotorInstance *r_driven;
 // 底盘板和云台板通信
 static CANCommInstance *chassis_comm; // 由于采用多板架构,即使使用C板也有空余串口,可以使用串口通信以获得更高的通信速率并降低阻塞
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;
@@ -40,27 +36,51 @@ static Chassis_Upload_Data_s chassis_feed_send;
 static LinkNPodParam left_side, right_side;
 static PIDInstance swerving_pid;        // 转向PID,有转向指令时使用IMU的加速度反馈积分以获取速度和位置状态量
 static PIDInstance anti_crash_pid;      // 抗劈叉,将输出以相反的方向叠加到左右腿的上
-static PIDInstance leg_length_pid;      // 用PD模拟弹簧的传递函数,不需要积分项(弹簧是一个无积分的二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
+static PIDInstance leg_length_pid;      // 用PD模拟弹簧,不要积分(弹簧是无积分二阶系统),增益不可过大否则抗外界冲击响应时太"硬"
 static PIDInstance roll_compensate_pid; // roll轴补偿,用于保持机体水平
 
 /* ↓↓↓分割出这些函数是为了提高可读性,使得阅读者的逻辑更加顺畅;但实际上这些函数都不长,可以以注释的形式直接加到BalanceTask里↓↓↓*/
 
 /**
  * @brief 将电机和imu的数据组装为LinkNPodParam结构体
+ * @note 由于两个连杆和轮子连接时,只有一个使用了轴承,另外一个直接和电机的定子连接,
+ *       所以轮子的转速和电机的转速不一致,因此和地面的速度应为电机转速减去杆的转速,得到的才是轮子的转速
  *
+ * @todo angle direction to be verified
  */
 static void ParamAssemble()
+{ // 电机的角度是逆时针为正,左侧全部取反
+    left_side.phi1 = PI2 - LIMIT_LINK_RAD - lb->measure.total_angle;
+    left_side.phi4 = -lf->measure.total_angle + LIMIT_LINK_RAD;
+    left_side.phi1_w = -lb->measure.speed_rads;
+    left_side.phi4_w = -lf->measure.speed_rads;
+    left_side.wheel_dist = -l_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
+    left_side.wheel_w = -l_driven->measure.speed_rads;
+
+    right_side.phi1 = PI2 - LIMIT_LINK_RAD + rb->measure.total_angle;
+    right_side.phi4 = rf->measure.total_angle + LIMIT_LINK_RAD;
+    right_side.phi1_w = rb->measure.speed_rads;
+    right_side.phi4_w = rf->measure.speed_rads;
+    right_side.wheel_dist = r_driven->measure.total_angle / 360 * WHEEL_RADIUS * PI;
+    right_side.wheel_w = r_driven->measure.speed_rads;
+
+    if(chassis_cmd_recv.wz != 0) // 若有转向指令,则使用IMU积分得到的位置,否则左右轮位置相反会产生阻碍转向的力矩
     {
+        // left_side.wheel_dist = 0.5*imu_data->Accel[3];
+        // right_side.wheel_dist = 0.5*imu_data->Accel[3];
+    }
 }
 
 /**
  * @brief 根据关节角度和角速度,计算单杆长度和角度以及变化率
+ * @todo 测试两种的效果,留下其中一种;
+ *       若差分的效果好则不需要VELOCITY_DIFF_VMC内的代码
  *
  * @param p 5连杆和腿的参数
  */
 static void Link2Pod(LinkNPodParam *p)
 { // 拟将功能封装到vmc_project.h中
-    float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2;
+    float xD, yD, xB, yB, BD, A0, B0, xC, yC, phi2t, phi5t;
     xD = JOINT_DISTANCE + THIGH_LEN * arm_cos_f32(p->phi4);
     yD = THIGH_LEN * arm_sin_f32(p->phi4);
     xB = 0 + THIGH_LEN * arm_cos_f32(p->phi1);
@@ -68,20 +88,16 @@ static void Link2Pod(LinkNPodParam *p)
     BD = powf(xD - xB, 2) + powf(yD - yB, 2);
     A0 = 2 * CALF_LEN * (xD - xB);
     B0 = 2 * CALF_LEN * (yD - yB);
-    p->phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
+    phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD);
-    xC = xB + CALF_LEN * arm_cos_f32(p->phi2);
+    xC = xB + CALF_LEN * arm_cos_f32(phi2t);
-    yC = yB + CALF_LEN * arm_sin_f32(p->phi2);
+    yC = yB + CALF_LEN * arm_sin_f32(phi2t);
-#ifdef ANGLE_DIFF_VMC
+
-    float p5t = atan2f(yC, xC - JOINT_DISTANCE / 2); // 避免重复计算
+    p->phi2 = phi2t;
-    float plt = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
-    p->phi5_w = (p5t - p->phi5) / balance_dt;
-    p->pod_len_w = (plt - p->pod_len) / balance_dt;
-    p->phi5 = p5t;
-    p->pod_len = plt;
-#endif
     p->phi5 = atan2f(yC, xC - JOINT_DISTANCE / 2);
     p->pod_len = Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2));
     p->phi3 = atan2(yC - yD, xC - xD); // 稍后用于计算VMC
+    p->theta = p->phi5 - 0.5 * PI - imu_data->Pitch;
+    p->height = p->pod_len * arm_cos_f32(p->theta);
 
 #ifdef VELOCITY_DIFF_VMC
     float phi1_pred = p->phi1 + p->phi1_w * balance_dt; // 预测下一时刻的关节角度(利用关节角速度)
@@ -94,24 +110,46 @@ static void Link2Pod(LinkNPodParam *p)
     BD = powf(xD - xB, 2) + powf(yD - yB, 2);
     A0 = 2 * CALF_LEN * (xD - xB);
     B0 = 2 * CALF_LEN * (yD - yB);
-    phi2 = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
+    phi2t = 2 * atan2f(B0 + Sqrt(powf(A0, 2) + powf(B0, 2) - powf(BD, 2)), A0 + BD); // 不要用link->phi2,因为这里是预测的
-    xC = xB + CALF_LEN * arm_cos_f32(phi2);
+    xC = xB + CALF_LEN * arm_cos_f32(phi2t);
-    yC = yB + CALF_LEN * arm_sin_f32(phi2);
+    yC = yB + CALF_LEN * arm_sin_f32(phi2t);
+    phi5t = atan2f(yC, xC - JOINT_DISTANCE / 2);
     // 差分计算腿长变化率和腿角速度
-    p->pod_w = (atan2f(yC, xC - JOINT_DISTANCE / 2) - p->phi5) / balance_dt;
+    p->phi2_w = (phi2t - p->phi2) / balance_dt; // 稍后用于修正轮速
+    p->pod_w = (phi5t - p->phi5) / balance_dt;
     p->pod_v = (Sqrt(powf(xC - JOINT_DISTANCE / 2, 2) + powf(yC, 2)) - p->pod_len) / balance_dt;
-#endif // VELOCITY_DIFF_VMC
+    p->theta_w = (phi5t - 0.5 * PI - imu_data->Pitch - p->theta) / balance_dt;
+    p->height_v = p->pod_v * arm_cos_f32(p->theta) - p->pod_len * arm_sin_f32(p->theta) * p->theta_w; // 这很酷!PDE!
+#endif
+
+    p->wheel_w = (p->wheel_w - p->phi2_w + imu_data->Gyro[3]) * WHEEL_RADIUS; // 修正轮速
 }
 
 /**
  * @brief 根据状态反馈计算当前腿长,查表获得LQR的反馈增益,并列式计算LQR的输出
  *        由于反馈矩阵和控制矩阵都比较稀疏,故不使用矩阵库,避免非零项计算
+ *
  * @note 计算得到的腿部力矩输出还要再综合运动控制系统补偿后映射为两个关节电机
  *       而轮子的输出则只经过转向PID的反馈增益计算
  *
+ * @todo 确定使用查表还是多项式拟合
+ *
  */
-static void CalcLQR(LinkNPodParam *p)
+static void CalcLQR(LinkNPodParam *p, float target_x)
 {
+    float *gain_list = LookUpKgain(p->pod_len); // K11,K12... K21,K22... K26
+    float T[2];                                 // 0 T_wheel, 1 T_pod;
+    for (uint8_t i = 0; i < 2; i++)
+    {
+        T[i] = gain_list[i * 6 + 0] * -p->theta +
+               gain_list[i * 6 + 1] * -p->theta_w +
+               gain_list[i * 6 + 2] * (target_x - p->wheel_dist) +
+               gain_list[i * 6 + 3] * -p->wheel_w +
+               gain_list[i * 6 + 4] * -imu_data->Pitch +
+               gain_list[i * 6 + 5] * -imu_data->Gyro[3];
+    }
+    p->T_wheel = T[0];
+    p->T_pod = T[1];
 }
 
 /**
@@ -124,7 +162,7 @@ static void SynthesizeMotion()
     left_side.T_pod += anti_crash_pid.Output;
     right_side.T_pod -= anti_crash_pid.Output;
 
-    // PIDCalculate(&swerving_pid, imu_data->Yaw, 0); // 对速度闭环还是使用角度增量闭环?
+    PIDCalculate(&swerving_pid, imu_data->Yaw, chassis_cmd_recv.wz); // 对速度闭环还是使用角度增量闭环?
     left_side.T_wheel -= swerving_pid.Output;
     right_side.T_wheel += swerving_pid.Output;
 }
@@ -135,7 +173,7 @@ static void SynthesizeMotion()
  */
 static void LegControl(LinkNPodParam *p, float target_length)
 {
-    p->F_pod += PIDCalculate(&leg_length_pid, p->pod_len, target_length);
+    p->F_pod += PIDCalculate(&leg_length_pid, p->height, target_length);
 }
 
 /**
@@ -177,8 +215,14 @@ static void FlyDetect()
  * @brief 功率限制,一般不需要
  *
  */
-static void WattLimit(LinkNPodParam *p)
+static void WattLimit()
 {
+    HTMotorSetRef(&lf, left_side.T_front);
+    HTMotorSetRef(&lb, left_side.T_back);
+    HTMotorSetRef(&rf, right_side.T_front);
+    HTMotorSetRef(&rb, right_side.T_back);
+    LKMotorSetRef(&l_driven, left_side.T_wheel);
+    LKMotorSetRef(&r_driven, right_side.T_wheel);
 }
 
 void BalanceInit()
@@ -320,13 +364,32 @@ void BalanceTask()
 {
     chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chassis_comm);
 
+    if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
+    {
+        HTMotorStop(lf);
+        HTMotorStop(rf);
+        HTMotorStop(lb);
+        HTMotorStop(rb);
+        LKMotorStop(l_driven);
+        LKMotorStop(r_driven);
+    }
+    else
+    {
+        HTMotorEnable(lf);
+        HTMotorEnable(rf);
+        HTMotorEnable(lb);
+        HTMotorEnable(rb);
+        LKMotorEnable(l_driven);
+        LKMotorEnable(r_driven);
+    }
+
     ParamAssemble(); // 参数组装,将电机和IMU的参数组装到一起
     // 将五连杆映射成单杆
     Link2Pod(&left_side);
     Link2Pod(&right_side);
     // 根据单杆计算处的角度和杆长,计算反馈增益
-    CalcLQR(&left_side);
+    CalcLQR(&left_side, chassis_cmd_recv.vx); // @todo,需要确定速度or位置闭环
-    CalcLQR(&right_side);
+    CalcLQR(&right_side, chassis_cmd_recv.vx);
     // 腿长控制
     LegControl(&left_side, 0);
     LegControl(&right_side, 0);
@@ -339,9 +402,8 @@ void BalanceTask()
     VMCProject(&right_side);
 
     FlyDetect(); // 滞空检测
-    // 电机输出限幅
+
-    WattLimit(&left_side);
+    WattLimit(); // 电机输出限幅
-    WattLimit(&right_side);
 
     // code to go here... 裁判系统,UI,多机通信
 

application/balance_chassis/balance.h

@@ -4,31 +4,30 @@
 #define CALF_LEN 0.25f        // 小腿
 #define THIGH_LEN 0.15f       // 大腿
 #define JOINT_DISTANCE 0.108f // 关节间距
+#define WHEEL_RADIUS 0.032f   // 轮子半径
+#define LIMIT_LINK_RAD 0.358  //
 
+// 计算速度的方式(五连杆到单杆的映射)
 #define VELOCITY_DIFF_VMC // 通过速度计算增量,然后通过差分计算腿长变化率和腿角速度
 // #define ANGLE_DIFF_VMC //直接保存上一次的值,通过差分计算腿长变化率和腿角速度
 
-
 typedef struct
 {
     // joint
-    float phi1_w, phi4_w;
+    float phi1_w, phi4_w, phi2_w; // phi2_w used for calc real wheel speed
     float T_back, T_front;
-
     // link angle,phi1-ph5, phi5 is pod angle
     float phi1, phi2, phi3, phi4, phi5;
-
     // wheel
-    float wheel_angle;
+    float wheel_dist;
     float wheel_w;
     float T_wheel;
-
     // pod
+    float theta, theta_w; // 杆和垂直方向的夹角,为控制状态之一
     float pod_len;
-    float pod_w;
+    float height,height_v;
-    float pod_v;
+    float pod_v, pod_w;
-    float F_pod;
+    float F_pod, T_pod;
-    float T_pod;
 } LinkNPodParam;
 
 /**

application/balance_chassis/gain_table.h

@@ -19,7 +19,7 @@ void EnalbeInterpolation(void)
 }
 
 /* 默认关闭插值,向下取整 */
-float LookUpKgain(float leg_length)
+float* LookUpKgain(float leg_length)
 {
     
 }

application/chassis/chassis.c

@@ -47,14 +47,10 @@ static Subscriber_t *chassis_sub;                   // 用于订阅底盘的控
 static Chassis_Ctrl_Cmd_s chassis_cmd_recv;         // 底盘接收到的控制命令
 static Chassis_Upload_Data_s chassis_feedback_data; // 底盘回传的反馈数据
 
-// static referee_info_t *referee_data; // 裁判系统相关数据
 static SuperCapInstance *cap;                                       // 超级电容
-static DJIMotorInstance *motor_lf;   // left right forward back
+static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
-static DJIMotorInstance *motor_rf;
-static DJIMotorInstance *motor_lb;
-static DJIMotorInstance *motor_rb;
 
-/* 用于自旋变速策略的时间变量,后续考虑查表加速 */
+/* 用于自旋变速策略的时间变量 */
 // static float t;
 
 /* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
@@ -114,7 +110,6 @@ void ChassisInit()
     // while (referee_data->GameRobotState.robot_id ==0);
     // Referee_Interactive_init(referee_data);
 
-
     SuperCap_Init_Config_s cap_conf = {
         .can_config = {
             .can_handle = &hcan2,
@@ -145,10 +140,10 @@ void ChassisInit()
 #endif // ONE_BOARD
 }
 
-#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
+#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
+#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
+#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
-#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * ANGLE_2_RAD)
+#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
 /**
  * @brief 计算每个轮毂电机的输出,正运动学解算
  *        用宏进行预替换减小开销,运动解算具体过程参考教程
@@ -236,8 +231,8 @@ void ChassisTask()
     // 根据云台和底盘的角度offset将控制量映射到底盘坐标系上
     // 底盘逆时针旋转为角度正方向;云台命令的方向以云台指向的方向为x,采用右手系(x指向正北时y在正东)
     static float sin_theta, cos_theta;
-    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
+    cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
-    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * ANGLE_2_RAD);
+    sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
     chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
     chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
 

application/gimbal/gimbal.c

@@ -8,8 +8,7 @@
 #include "bmi088.h"
 
 static attitude_t *gimba_IMU_data; // 云台IMU数据
-static DJIMotorInstance *yaw_motor;   // yaw电机
+static DJIMotorInstance *yaw_motor, *pitch_motor;
-static DJIMotorInstance *pitch_motor; // pitch电机
 
 static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
 static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
@@ -185,7 +184,7 @@ void GimbalTask()
 
     // 设置反馈数据,主要是imu和yaw的ecd
     gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->motor_measure.angle_single_round;
+    gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
 
     // 推送消息
     PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);

application/shoot/shoot.c

@@ -7,9 +7,7 @@
 #include "general_def.h"
 
 /* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
-static DJIMotorInstance *friction_l; // 左摩擦轮
+static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
-static DJIMotorInstance *friction_r; // 右摩擦轮
-static DJIMotorInstance *loader;     // 拨盘电机
 // static servo_instance *lid; 需要增加弹舱盖
 
 static Publisher_t *shoot_pub;
@@ -142,14 +140,14 @@ void ShootTask()
     // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
     case LOAD_1_BULLET:                                                                     // 激活能量机关/干扰对方用,英雄用.
         DJIMotorOuterLoop(loader, ANGLE_LOOP);                                              // 切换到角度环
-        DJIMotorSetRef(loader, loader->motor_measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
+        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->motor_measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
+        DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
         hibernate_time = DWT_GetTimeline_ms();                                                  // 记录触发指令的时间
         dead_time = 300;                                                                        // 完成3发弹丸发射的时间
         break;
@@ -170,7 +168,6 @@ void ShootTask()
             ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
     }
 
-    
     // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
     if (shoot_cmd_recv.friction_mode == FRICTION_ON)
     {

modules/general_def.h

@@ -3,15 +3,15 @@
 
 // 一些module的通用数值型定义,注意条件macro兼容,一些宏可能在math.h中已经定义过了
 
-
 #ifndef PI
 #define PI 3.1415926535f
-#endif                  // !PI
+#endif
 #define PI2 (PI * 2.0f) // 2 pi
 
-#define RAD_2_ANGLE 57.2957795f    // 180/pi
+#define RAD_2_DEGREE 57.2957795f    // 180/pi
-#define ANGLE_2_RAD 0.01745329252f // pi/180
+#define DEGREE_2_RAD 0.01745329252f // pi/180
 
 #define RPM_2_ANGLE_PER_SEC 6.0f       // ×360°/60sec
+#define RPM_2_RAD_PER_SEC 0.104719755f // ×2pi/60sec
 
 #endif // !GENERAL_DEF_H

modules/motor/DJImotor/dji_motor.c

@@ -124,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
     // 这里对can instance的id进行了强制转换,从而获得电机的instance实例地址
     // _instance指针指向的id是对应电机instance的地址,通过强制转换为电机instance的指针,再通过->运算符访问电机的成员motor_measure,最后取地址获得指针
     uint8_t *rxbuff = _instance->rx_buff;
-    DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->motor_measure); // measure要多次使用,保存指针减小访存开销
+    DJI_Motor_Measure_s *measure = &(((DJIMotorInstance *)_instance->id)->measure); // measure要多次使用,保存指针减小访存开销
 
     // 解析数据并对电流和速度进行滤波,电机的反馈报文具体格式见电机说明手册
     measure->last_ecd = measure->ecd;
@@ -223,7 +223,7 @@ void DJIMotorControl()
     DJIMotorInstance *motor;
     Motor_Control_Setting_s *motor_setting; // 电机控制参数
     Motor_Controller_s *motor_controller;   // 电机控制器
-    DJI_Motor_Measure_s *motor_measure;     // 电机测量值
+    DJI_Motor_Measure_s *measure;           // 电机测量值
     float pid_measure, pid_ref;             // 电机PID测量值和设定值
 
     // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
@@ -232,7 +232,7 @@ void DJIMotorControl()
         motor = dji_motor_instance[i];
         motor_setting = &motor->motor_settings;
         motor_controller = &motor->motor_controller;
-        motor_measure = &motor->motor_measure;
+        measure = &motor->measure;
         pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改
         if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
             pid_ref *= -1; // 设置反转
@@ -243,7 +243,7 @@ void DJIMotorControl()
             if (motor_setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_angle_feedback_ptr;
             else
-                pid_measure = motor_measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
+                pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃
             // 更新pid_ref进入下一个环
             pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref);
         }
@@ -254,7 +254,7 @@ void DJIMotorControl()
             if (motor_setting->speed_feedback_source == OTHER_FEED)
                 pid_measure = *motor_controller->other_speed_feedback_ptr;
             else // MOTOR_FEED
-                pid_measure = motor_measure->speed_aps;
+                pid_measure = measure->speed_aps;
             // 更新pid_ref进入下一个环
             pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
         }
@@ -262,13 +262,12 @@ void DJIMotorControl()
         // 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
         if (motor_setting->close_loop_type & CURRENT_LOOP)
         {
-            pid_ref = PIDCalculate(&motor_controller->current_PID, motor_measure->real_current, pid_ref);
+            pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
         }
 
         // 获取最终输出
         set = (int16_t)pid_ref;
 
-
         // 分组填入发送数据
         group = motor->sender_group;
         num = motor->message_num;

modules/motor/DJImotor/dji_motor.h

@@ -49,7 +49,7 @@ typedef struct
 typedef struct
 {
 
-    DJI_Motor_Measure_s motor_measure;      // 电机测量值
+    DJI_Motor_Measure_s measure;      // 电机测量值
     Motor_Control_Setting_s motor_settings; // 电机设置
     Motor_Controller_s motor_controller;    // 电机控制器
 

modules/motor/HTmotor/HT04.c

@@ -42,16 +42,16 @@ static void HTMotorDecode(CANInstance *motor_can)
 {
     uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
     uint8_t *rxbuff = motor_can->rx_buff;
-    HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->motor_measure; // 将can实例中保存的id转换成电机实例的指针
+    HTMotor_Measure_t *measure = &((HTMotorInstance *)motor_can->id)->measure; // 将can实例中保存的id转换成电机实例的指针
 
     measure->last_angle = measure->total_angle;
 
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
-    measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MIN, P_MAX, 16);
+    measure->total_angle =  uint_to_float(tmp, P_MIN, P_MAX, 16);
 
     tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
-    measure->speed_aps = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
+    measure->speed_rads = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
-                         (1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
+                          (1 - SPEED_SMOOTH_COEF) * measure->speed_rads;
 
     tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
     measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MIN, T_MAX, 12) +
@@ -97,7 +97,7 @@ void HTMotorControl()
     for (size_t i = 0; i < idx; i++)
     { // 先获取地址避免反复寻址
         motor = ht_motor_instance[i];
-        measure = &motor->motor_measure;
+        measure = &motor->measure;
         setting = &motor->motor_settings;
         motor_can = motor->motor_can_instace;
         pid_ref = motor->pid_ref;
@@ -109,7 +109,7 @@ void HTMotorControl()
             else
                 pid_measure = measure->real_current;
             // measure单位是rad,ref是角度,统一到angle下计算,方便建模
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_ANGLE, pid_ref);
+            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
 
         if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
@@ -120,9 +120,9 @@ void HTMotorControl()
             if (setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_speed_feedback_ptr;
             else
-                pid_measure = measure->speed_aps;
+                pid_measure = measure->speed_rads;
             // measure单位是rad / s ,ref是angle per sec,统一到angle下计算
-            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_ANGLE, pid_ref);
+            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure * RAD_2_DEGREE, pid_ref);
         }
 
         if (setting->close_loop_type & CURRENT_LOOP)

modules/motor/HTmotor/HT04.h

@@ -21,14 +21,14 @@ typedef struct // HT04
 {
     float last_angle;
     float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
-    float speed_aps;
+    float speed_rads;
     float real_current;
 } HTMotor_Measure_t;
 
 /* HT电机类型定义*/
 typedef struct
 {
-    HTMotor_Measure_t motor_measure;
+    HTMotor_Measure_t measure;
 
     Motor_Control_Setting_s motor_settings;
 

modules/motor/LKmotor/LK9025.c

@@ -1,5 +1,6 @@
 #include "LK9025.h"
 #include "stdlib.h"
+#include "general_def.h"
 
 static uint8_t idx;
 static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
@@ -23,8 +24,8 @@ static void LKMotorDecode(CANInstance *_instance)
 
     measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
 
-    measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps +
+    measure->speed_rads = (1 - SPEED_SMOOTH_COEF) * measure->speed_rads +
-                         SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
+                          DEGREE_2_RAD * SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4]));
 
     measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current +
                             CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2]));
@@ -101,7 +102,7 @@ void LKMotorControl()
             if (setting->angle_feedback_source == OTHER_FEED)
                 pid_measure = *motor->other_speed_feedback_ptr;
             else
-                pid_measure = measure->speed_aps;
+                pid_measure = measure->speed_rads;
             pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
             if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
                 pid_ref += *motor->current_feedforward_ptr;

modules/motor/LKmotor/LK9025.h

@@ -14,13 +14,14 @@
 #define SPEED_SMOOTH_COEF 0.85f
 #define REDUCTION_RATIO_DRIVEN 1
 #define ECD_ANGLE_COEF_LK (360.0f / 65536.0f)
+#define CURRENT_TORQUE_COEF_LK 0.003645f // 电流设定值转换成扭矩的系数,算出来的设定值除以这个系数就是扭矩值
 
 typedef struct // 9025
 {
     uint16_t last_ecd;        // 上一次读取的编码器值
     uint16_t ecd;             // 当前编码器值
     float angle_single_round; // 单圈角度
-    float speed_aps;          // speed angle per sec(degree:°)
+    float speed_rads;         // speed rad/s
     int16_t real_current;     // 实际电流
     uint8_t temperate;        // 温度,C°