修复了HT电机PID计算的bug

modules/motor/DJImotor/dji_motor.h

@@ -6,7 +6,8 @@
  * @date 2022-11-01
  *
  * @todo  1. 给不同的电机设置不同的低通滤波器惯性系数而不是统一使用宏
-          2. 为M2006和M3508增加开环的零位校准函数
+          2. 为M2006和M3508增加开环的零位校准函数,并在初始化时调用(根据用户配置决定是否调用)
+          3. 完成前馈功能（已经添加了前馈数据指针）
 
  * @copyright Copyright (c) 2022 HNU YueLu EC all rights reserved
  *

modules/motor/DJImotor/dji_motor.md

@@ -177,7 +177,7 @@ void DJIMotorChangeFeed(dji_motor_instance *motor,
                         Feedback_Source_e type);
 ```
 
-调用第一个并传入设定值，它会自动根据你设定的PID参数进行动作。
+调用第一个并传入设定值，它会自动根据你设定的PID参数进行动作。 如果对不同闭环都有参考输入,则设置最外层的闭环(通过此函数)并将剩下的参考输入通过前馈数据指针进行设定
 
 调用第二个并设定要修改的反馈环节和反馈类型，它会将反馈数据指针切换到你设定好的变量（需要在初始化的时候设置反馈指针）。
 

modules/motor/HTmotor/HT04.c

@@ -6,17 +6,17 @@ static uint8_t idx;
 HTMotorInstance *ht_motor_instance[HT_MOTOR_CNT];
 
 /**
- * @brief 
+ * @brief 设置电机模式,报文内容[0xff,0xff,0xff,0xff,0xff,0xff,0xff,cmd]
  *
  * @param cmd
  * @param motor
  */
 static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor)
-{
-    static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};
-    buf[7] = (uint8_t)cmd;
-    memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf));
-    CANTransmit(motor->motor_can_instace,1);
+{                                                        
+    memset(motor->motor_can_instace->tx_buff, 0xff, 7);  // 发送电机指令的时候前面7bytes都是0xff
+    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
+    CANTransmit(motor->motor_can_instace, 1);
+    memset(motor->motor_can_instace->tx_buff, 0, 6); // 发送控制指令的时候前面6bytes都是0
 }
 
 /* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
@@ -53,7 +53,7 @@ static void HTMotorDecode(CANInstance *motor_can)
     measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MIN, P_MAX, 16);
 
     tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
-    measure->speed_aps = RAD_2_ANGLE * SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
+    measure->speed_aps = SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MIN, V_MAX, 12) +
                          (1 - SPEED_SMOOTH_COEF) * measure->speed_aps;
 
     tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
@@ -102,7 +102,7 @@ void HTMotorControl()
         motor = ht_motor_instance[i];
         measure = &motor->motor_measure;
         setting = &motor->motor_settings;
-        motor_can = motor_can;
+        motor_can = motor->motor_can_instace;
         pid_ref = motor->pid_ref;
 
         if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
@@ -111,8 +111,8 @@ void HTMotorControl()
                 pid_measure = *motor->other_angle_feedback_ptr;
             else
                 pid_measure = measure->real_current;
-
-            pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
+            // measure单位是rad,ref是角度,统一到angle下计算,方便建模
+            pid_ref = PID_Calculate(&motor->angle_PID, pid_measure*RAD_2_ANGLE, pid_ref);
         }
 
         if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
@@ -124,8 +124,8 @@ void HTMotorControl()
                 pid_measure = *motor->other_speed_feedback_ptr;
             else
                 pid_measure = measure->speed_aps;
-                
-            pid_ref = PID_Calculate(&motor->angle_PID, pid_measure, pid_ref);
+            // measure单位是rad / s ,ref是angle per sec,统一到angle下计算
+            pid_ref = PID_Calculate(&motor->speed_PID, pid_measure*RAD_2_ANGLE, pid_ref);
         }
 
         if (setting->close_loop_type & CURRENT_LOOP)
@@ -140,26 +140,29 @@ void HTMotorControl()
         if (setting->reverse_flag == MOTOR_DIRECTION_REVERSE)
             set *= -1;
 
-        tmp = float_to_uint(set, T_MIN, T_MAX, 12);
-        motor_can->tx_buff[6] = tmp >> 8;
+        LIMIT_MIN_MAX(set, T_MIN, T_MAX); // 限幅,实际上这似乎和pid输出限幅重复了
+        tmp = float_to_uint(set, T_MIN, T_MAX, 12);  // 数值最后在 -12~+12之间
+        motor_can->tx_buff[6] = (tmp >> 8);
         motor_can->tx_buff[7] = tmp & 0xff;
 
         if (motor->stop_flag == MOTOR_STOP)
         { // 若该电机处于停止状态,直接将发送buff置零
             memset(motor_can->tx_buff + 6, 0, sizeof(uint16_t));
         }
-        CANTransmit(motor_can,1);
+        CANTransmit(motor_can, 1);
     }
 }
 
 void HTMotorStop(HTMotorInstance *motor)
 {
     HTMotorSetMode(CMD_RESET_MODE, motor);
+    motor->stop_flag = MOTOR_STOP;
 }
 
 void HTMotorEnable(HTMotorInstance *motor)
 {
     HTMotorSetMode(CMD_MOTOR_MODE, motor);
+    motor->stop_flag = MOTOR_ENALBED;
 }
 
 void HTMotorCalibEncoder(HTMotorInstance *motor)

modules/motor/HTmotor/HT04.h

@@ -18,9 +18,9 @@
 #define T_MAX 18.0f
 
 typedef struct // HT04
-{              // 角度为多圈角度,范围是-95.5~95.5,单位为rad
-    float last_angle;
-    float total_angle;
+{              
+    float last_angle; 
+    float total_angle; // 角度为多圈角度,范围是-95.5~95.5,单位为rad
     float speed_aps;
     float real_current;
 } HTMotor_Measure_t;

modules/motor/motor_task.c

@@ -11,14 +11,14 @@ void MotorControlTask()
     // static uint8_t cnt = 0; 设定任务频率
     // if(cnt%5==0) //200hz
     // if(cnt%10==0) //100hz
-    
     DJIMotorControl();
 
-    //LKMotorControl();
+    /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
+    LKMotorControl();
 
-    //HTMotorControl();
+    HTMotorControl();
 
-    //ServeoMotorControl();
+    ServeoMotorControl();
 
     //StepMotorControl();
 }

modules/motor/motor_task.h

@@ -16,6 +16,8 @@
  * @brief 电机控制闭环任务,在RTOS中应该设定为1Khz运行
  *        舵机控制任务的频率设定为20Hz或更低
  * 
+ * @note 好无语,就一个函数罢了,干脆全部放到头文件里好了.
+ * 
  */
 void MotorControlTask();
 

modules/motor/servo_motor/servo_motor.c

@@ -80,7 +80,7 @@ void Servo_Motor_Type_Select(ServoInstance *Servo_Motor, int16_t mode)
  * @brief 舵机输出控制
  *
  */
-void Servo_Motor_Control()
+void ServeoMotorControl()
 {
     static ServoInstance *Servo_Motor;
 

modules/motor/servo_motor/servo_motor.h

@@ -86,5 +86,5 @@ typedef struct
 ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config);
 void Servo_Motor_FreeAngle_Set(ServoInstance *Servo_Motor, int16_t S_angle);
 void Servo_Motor_Type_Select(ServoInstance *Servo_Motor,int16_t mode);
-void Servo_Motor_Control();
+void ServeoMotorControl();
 #endif // SERVO_MOTOR_H

modules/motor/servo_motor/servo_motor.md

@@ -1,6 +1,10 @@
 ##舵机的使用
+
 <p align='left' >panrui@hnu.edu.cn</p>
+
+> todo: 由于新增了bsp_pwm的支持,舵机模块需要部分重构
 ### 舵机基础知识
+
 已最常见的SG90舵机为例，SG90舵机要求工作在频率为50HZ——周期为20ms的PWM波，且对应信号的高低电平在0.5ms - 2.5ms之间，对应的舵机转动角度如下表所示（当然也可以按照这个线性的对应关系去达到转动自己想要的角度，如想要转动60°，则高电平脉宽为大概为1.2ms，具体能不能转到特定的角度还和舵机的精度有关）
 
 >0.5ms-------------0度； 2.5%