From e42c77611b1a345f924b8c7b40a52a57847543fd Mon Sep 17 00:00:00 2001
From: chenfu <2412777093@qq.com>
Date: Sat, 2 Dec 2023 11:29:18 +0800
Subject: [PATCH] rm vofa/led,add bluetooth,change servo_motor

---
 .Doc/TODO.md                             |   6 +-
 .vscode/settings.json                    |   3 +-
 Makefile                                 |   6 +-
 application/gimbal/gimbal.c              |   9 +-
 modules/bluetooth/HC05.c                 |  58 ++++++++++
 modules/bluetooth/HC05.h                 |  23 ++++
 modules/imu/BMI088driver.c               |   2 +-
 modules/led/led.c                        |  46 --------
 modules/led/led.h                        |  33 ------
 modules/led/led.md                       |   3 -
 modules/motor/DMmotor/dmmotor.c          |  10 +-
 modules/motor/DMmotor/dmmotor.h          |   6 +-
 modules/motor/motor_task.c               |   2 -
 modules/motor/servo_motor/servo_motor.c  | 139 ++++++++---------------
 modules/motor/servo_motor/servo_motor.h  |  97 +++++-----------
 modules/motor/servo_motor/servo_motor.md |  49 +-------
 modules/vofa/vofa.c                      |  33 ------
 modules/vofa/vofa.h                      |  21 ----
 modules/vofa/vofa.md                     |   3 -
 19 files changed, 178 insertions(+), 371 deletions(-)
 create mode 100644 modules/bluetooth/HC05.c
 create mode 100644 modules/bluetooth/HC05.h
 delete mode 100644 modules/led/led.c
 delete mode 100644 modules/led/led.h
 delete mode 100644 modules/led/led.md
 delete mode 100644 modules/vofa/vofa.c
 delete mode 100644 modules/vofa/vofa.h
 delete mode 100644 modules/vofa/vofa.md

diff --git a/.Doc/TODO.md b/.Doc/TODO.md
index d7a9a79..323ce7b 100644
--- a/.Doc/TODO.md
+++ b/.Doc/TODO.md
@@ -26,7 +26,7 @@
 
 #### bsp_pwm
 
-- [ ] 是否允许修改预分频计数器？
+- [x] 是否允许修改预分频计数器？
 
 ### 待添加
 
@@ -36,7 +36,7 @@
 
 #### bsp_blueteetch
 
-- [ ] 增加蓝牙功能，方便调试和测试
+- [x] 增加蓝牙功能，方便调试和测试
 
 #### bsp_wifi
 
@@ -59,7 +59,7 @@ Unicom
 舵机模块，需要预先定义90/180/360连续旋转的电机类型，并且能够设定max和min位置。
 
 - [x] 编写舵机模块（待测试和优化）
-- [ ] 可能需要串口舵机的支持？
+- [x] 可能需要串口舵机的支持？
 
 #### imu
 
diff --git a/.vscode/settings.json b/.vscode/settings.json
index fa10b77..c84d887 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -55,7 +55,8 @@
         "stm32f4xx_hal_conf.h": "c",
         "master_process.h": "c",
         "bsp_can.h": "c",
-        "can.h": "c"
+        "can.h": "c",
+        "servo_motor.h": "c"
     },
     // "clangd.arguments": [
     //     "-query-driver=C:/msys64/mingw64/bin/arm-none-eabi-*.exe",
diff --git a/Makefile b/Makefile
index 1f4f42f..02814aa 100644
--- a/Makefile
+++ b/Makefile
@@ -124,12 +124,12 @@ modules/algorithm/QuaternionEKF.c \
 modules/algorithm/crc8.c \
 modules/algorithm/crc16.c \
 modules/algorithm/user_lib.c \
+modules/bluetooth/HC05.c \
 modules/BMI088/bmi088.c \
 modules/imu/BMI088driver.c \
 modules/imu/BMI088Middleware.c \
 modules/imu/ins_task.c \
 modules/ist8310/ist8310.c \
-modules/led/led.c \
 modules/master_machine/master_process.c \
 modules/master_machine/seasky_protocol.c \
 modules/motor/DJImotor/dji_motor.c \
@@ -149,7 +149,6 @@ modules/super_cap/super_cap.c \
 modules/can_comm/can_comm.c \
 modules/message_center/message_center.c \
 modules/daemon/daemon.c \
-modules/vofa/vofa.c \
 modules/alarm/buzzer.c \
 application/gimbal/gimbal.c \
 application/chassis/chassis.c \
@@ -246,10 +245,10 @@ C_INCLUDES =  \
 -Ibsp/pwm \
 -Ibsp \
 -Imodules/algorithm \
+-Imodules/bluetooth \
 -Imodules/BMI088 \
 -Imodules/imu \
 -Imodules/ist8310 \
--Imodules/led \
 -Imodules/master_machine \
 -Imodules/motor/DJImotor \
 -Imodules/motor/LKmotor \
@@ -265,7 +264,6 @@ C_INCLUDES =  \
 -Imodules/can_comm \
 -Imodules/message_center \
 -Imodules/daemon \
--Imodules/vofa \
 -Imodules/alarm \
 -Imodules  \
 -IMiddlewares/ST/ARM/DSP/Inc
diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c
index e1a070f..9968cee 100644
--- a/application/gimbal/gimbal.c
+++ b/application/gimbal/gimbal.c
@@ -4,7 +4,7 @@
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
-
+#include "bmi088.h"
 #include "bmi088.h"
 
 static attitude_t *gimba_IMU_data; // 云台IMU数据
@@ -15,9 +15,12 @@ static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
 
+//static BMI088Instance *bmi088; // 云台IMU
 void GimbalInit()
-{
+{   
     gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
+
+   // bmi088=BMI088Register(&imu_config);
     // YAW
     Motor_Init_Config_s yaw_config = {
         .can_init_config = {
@@ -146,7 +149,7 @@ void GimbalTask()
     // ...
 
     // 设置反馈数据,主要是imu和yaw的ecd
-    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
+    //gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
     gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
 
     // 推送消息
diff --git a/modules/bluetooth/HC05.c b/modules/bluetooth/HC05.c
new file mode 100644
index 0000000..2d170d6
--- /dev/null
+++ b/modules/bluetooth/HC05.c
@@ -0,0 +1,58 @@
+#include "HC05.h"
+#include "bsp_usart.h"
+
+#define HC05_BUFFERSIZE  HC05_DATASIZE+2  // HC05发送和接收数据buffer大小，不得大于256
+
+#define FRAME_HEAD  0XAA    // 帧头
+#define FRAME_END   0X55    // 帧尾
+
+static HC05 hc05_msg;   // HC05通信数据
+static USARTInstance *hc05_usart_instance;    // HC05串口通信实例
+
+static uint8_t hc05_init_flag = 0;  // HC05初始化标志位
+
+// *hc05_usart_instance串口回调函数
+static void HC05RxCallback()
+{
+    uint8_t *rxbuff;
+    rxbuff = hc05_usart_instance->recv_buff;
+
+    // 帧头帧尾判断
+    if(rxbuff[0] == FRAME_HEAD && rxbuff[HC05_BUFFERSIZE - 1] == FRAME_END)
+    {
+        for(int i = 0; i < HC05_DATASIZE; i++)
+            hc05_msg.recv_data[i] = (uint8_t)rxbuff[i+1];
+    }
+    
+    return;
+}
+
+// HC05串口接收初始化
+HC05 *HC05Init(UART_HandleTypeDef *hc05_usart_handle)
+{
+    USART_Init_Config_s conf;
+    conf.module_callback = HC05RxCallback;
+    conf.usart_handle = hc05_usart_handle;
+    conf.recv_buff_size = HC05_BUFFERSIZE;
+    hc05_usart_instance = USARTRegister(&conf);
+
+    hc05_init_flag = 1;
+    return (HC05*)&hc05_msg;
+}
+
+// HC05串口发送函数，一次最多发送HC05_DATASIZE个数据
+void HC05_SendData(uint8_t *data, uint8_t data_num)
+{
+
+    // 发送数据中加入帧头和帧尾
+    hc05_msg.send_data[0] = FRAME_HEAD;
+
+    for (int i = 0; i < data_num; i++)
+        hc05_msg.send_data[i+1] = data[i];
+    
+    hc05_msg.send_data[HC05_BUFFERSIZE - 1] = FRAME_END;
+
+    // 发送数据
+    USARTSend(hc05_usart_instance, hc05_msg.send_data, data_num+2, USART_TRANSFER_IT); 
+
+}
\ No newline at end of file
diff --git a/modules/bluetooth/HC05.h b/modules/bluetooth/HC05.h
new file mode 100644
index 0000000..eb54bfe
--- /dev/null
+++ b/modules/bluetooth/HC05.h
@@ -0,0 +1,23 @@
+#ifndef HC05_H
+#define HC05_H
+
+#include <stdint.h>
+#include "main.h"
+#include "usart.h"
+
+#define  HC05_DATASIZE  4   // HC05接收和发送数据大小，根据需要修改
+
+// HC05通信数据结构体，后续根据需要添加和修改
+typedef struct 
+{
+    uint8_t  send_data[HC05_DATASIZE + 2];  // 发送数据，加上帧头和帧尾
+    uint8_t  recv_data[HC05_DATASIZE];      // 接收数据
+}HC05;
+
+// HC05串口接收初始化
+HC05 *HC05Init(UART_HandleTypeDef *hc05_usart_handle);
+
+// HC05串口发送函数，一次最多发送HC05_DATASIZE个数据
+void HC05_SendData( uint8_t *data, uint8_t data_num);
+
+#endif
diff --git a/modules/imu/BMI088driver.c b/modules/imu/BMI088driver.c
index e681428..4b0be85 100644
--- a/modules/imu/BMI088driver.c
+++ b/modules/imu/BMI088driver.c
@@ -123,7 +123,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
     uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
     int16_t bmi088_raw_temp;
     float gyroMax[3], gyroMin[3];
-    float gNormTemp, gNormMax, gNormMin;
+    float gNormTemp = 0.0f, gNormMax = 0.0f, gNormMin = 0.0f;
 
     startTime = DWT_GetTimeline_s();
     do
diff --git a/modules/led/led.c b/modules/led/led.c
deleted file mode 100644
index 276d86f..0000000
--- a/modules/led/led.c
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "led.h"
-#include "stdlib.h"
-#include "memory.h"
-#include "user_lib.h"
-
-static uint8_t idx;
-static LEDInstance *bsp_led_ins[LED_MAX_NUM] = {NULL};
-
-LEDInstance *LEDRegister(LED_Init_Config_s *led_config)
-{
-    LEDInstance *led_ins = (LEDInstance *)zmalloc(sizeof(LEDInstance));
-    // 剩下的值暂时都被置零
-    led_ins->led_pwm = PWMRegister(&led_config->pwm_config);
-    led_ins->led_switch = led_config->init_swtich;
-
-    bsp_led_ins[idx++] = led_ins;
-    return led_ins;
-}
-
-void LEDSet(LEDInstance *_led, uint8_t alpha, uint8_t color_value, uint8_t brightness)
-{
-}
-
-void LEDSwitch(LEDInstance *_led, uint8_t led_switch)
-{
-    if (led_switch == 1)
-    {
-        _led->led_switch = 1;
-    }
-    else
-    {
-        _led->led_switch = 0;
-        // PWMSetPeriod(_led,0);
-    }
-}
-
-void LEDShow(uint32_t aRGB)
-{
-    // static uint8_t alpha;
-    // static uint16_t red, green, blue;
-
-    // alpha = (aRGB & 0xFF000000) >> 24;
-    // red = ((aRGB & 0x00FF0000) >> 16) * alpha;
-    // green = ((aRGB & 0x0000FF00) >> 8) * alpha;
-    // blue = ((aRGB & 0x000000FF) >> 0) * alpha;
-}
diff --git a/modules/led/led.h b/modules/led/led.h
deleted file mode 100644
index 0741315..0000000
--- a/modules/led/led.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef _LED_H_
-#define _LED_H_
-
-#include "stdint.h"
-#include "bsp_pwm.h"
-
-#define LED_MAX_NUM 3
-
-typedef struct
-{
-    PWMInstance* led_pwm;
-    uint8_t led_alpha; // 透明度,通过pwm频率改变
-    uint8_t led_brightness; // 亮度,通过电压改变(如果可以,使用dac)
-    uint8_t led_color; // rgb value,0-255
-    uint8_t led_switch; // 开关,on1 off0
-    // void (*action_callback)(void); // led动作回调函数
-} LEDInstance;
-
-typedef struct
-{
-    PWM_Init_Config_s pwm_config;
-    uint8_t init_swtich; // 初始化开关
-} LED_Init_Config_s;
-
-LEDInstance* LEDRegister(LED_Init_Config_s* led_config);
-
-void LEDSet(LEDInstance *_led,uint8_t alpha,uint8_t color_value,uint8_t brightness);
-
-void LEDSwitch(LEDInstance *_led,uint8_t led_switch);
-
-void LEDShow();
-
-#endif // !_LED_H_
diff --git a/modules/led/led.md b/modules/led/led.md
deleted file mode 100644
index e5b1e17..0000000
--- a/modules/led/led.md
+++ /dev/null
@@ -1,3 +0,0 @@
-TO BE DONE
-
-请勿使用
\ No newline at end of file
diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c
index 5fa2054..fad8221 100644
--- a/modules/motor/DMmotor/dmmotor.c
+++ b/modules/motor/DMmotor/dmmotor.c
@@ -115,14 +115,16 @@ void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
     motor->motor_settings.outer_loop_type = type;
 }
 
+
+//@Todo: 目前只实现了力控，更多位控PID等请自行添加
 void DMMotorTask(void const *argument)
 {
-    float pid_measure, pid_ref, set;
+    float  pid_ref, set;
     DMMotorInstance *motor = (DMMotorInstance *)argument;
-    DM_Motor_Measure_s *measure = &motor->measure;
+   //DM_Motor_Measure_s *measure = &motor->measure;
     Motor_Control_Setting_s *setting = &motor->motor_settings;
-    CANInstance *motor_can = motor->motor_can_instace;
-    uint16_t tmp;
+    //CANInstance *motor_can = motor->motor_can_instace;
+    //uint16_t tmp;
     DMMotor_Send_s motor_send_mailbox;
     while (1)
     {
diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h
index f653998..8f03e28 100644
--- a/modules/motor/DMmotor/dmmotor.h
+++ b/modules/motor/DMmotor/dmmotor.h
@@ -8,11 +8,11 @@
 
 #define DM_MOTOR_CNT 4
 
-#define DM_P_MIN  -12.5f
+#define DM_P_MIN  (-12.5f)
 #define DM_P_MAX  12.5f
-#define DM_V_MIN  -45.0f
+#define DM_V_MIN  (-45.0f)
 #define DM_V_MAX  45.0f
-#define DM_T_MIN  -18.0f
+#define DM_T_MIN  (-18.0f)
 #define DM_T_MAX   18.0f
 
 typedef struct 
diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c
index 47ff232..a52980b 100644
--- a/modules/motor/motor_task.c
+++ b/modules/motor/motor_task.c
@@ -21,7 +21,5 @@ void MotorControlTask()
     // HTMotorControl();
     // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
 
-    ServeoMotorControl();
-
     // StepMotorControl();
 }
diff --git a/modules/motor/servo_motor/servo_motor.c b/modules/motor/servo_motor/servo_motor.c
index e5c4564..0c0007a 100644
--- a/modules/motor/servo_motor/servo_motor.c
+++ b/modules/motor/servo_motor/servo_motor.c
@@ -1,122 +1,77 @@
 #include "servo_motor.h"
 #include "stdlib.h"
 #include "memory.h"
+#include "bsp_log.h"
+uint8_t servo_angle_read[6]={0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
+uint8_t servo_angle_write[16]={0x55 ,0x55, 0x08, 0x03, 0x01 ,0xF4 ,0x01 ,0x01 ,0x20 ,0x03,0x55 ,0x55 ,0x04, 0x15 ,0x01 ,0x01 ,};
+uint8_t servo_unload[6]={0x55,0x55,0x04,0x14,0x01,0x01};
 
-extern TIM_HandleTypeDef htim1;
 /*第二版*/
-static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT] = {NULL};
-static int16_t compare_value[SERVO_MOTOR_CNT] = {0};
+static ServoInstance *servo_motor_instance[SERVO_MOTOR_CNT];
 static uint8_t servo_idx = 0; // register servo_idx,是该文件的全局舵机索引,在注册时使用
-
+static void DecodeServo();
 // 通过此函数注册一个舵机
 ServoInstance *ServoInit(Servo_Init_Config_s *Servo_Init_Config)
 {
     ServoInstance *servo = (ServoInstance *)malloc(sizeof(ServoInstance));
     memset(servo, 0, sizeof(ServoInstance));
+    USART_Init_Config_s config;
+    servo->servo_type = Servo_Init_Config->servo_type;
+    switch (Servo_Init_Config->servo_type)
+    {
+    case Bus_Servo:
+        config.module_callback = DecodeServo;
+        config.recv_buff_size = Servo_MAX_BUFF;
+        config.usart_handle = Servo_Init_Config->_handle;
+        servo->usart_instance = USARTRegister(&config);
+        break;
+    case PWM_Servo:
+        servo->pwm_instance = PWMRegister(&Servo_Init_Config->pwm_init_config);
+        break;
+    default:
+        LOGERROR("Servo type error");
+        break;
+    }
+    servo->servo_id = Servo_Init_Config->servo_id;
+    servo_idx++;
+    servo_motor_instance[servo->servo_id] = servo;
 
-    servo->Servo_type = Servo_Init_Config->Servo_type;
-    servo->htim = Servo_Init_Config->htim;
-    servo->Channel = Servo_Init_Config->Channel;
-
-    HAL_TIM_PWM_Start(Servo_Init_Config->htim, Servo_Init_Config->Channel);
-    servo_motor_instance[servo_idx++] = servo;
     return servo;
 }
-
-/**
- * @brief 写入自由角度数值
- *
- * @param Servo_Motor 注册的舵机实例
- * @param S_angle  改变自由模式设定的角度
- */
-void Servo_Motor_FreeAngle_Set(ServoInstance *Servo_Motor, int16_t S_angle)
+//@todo PWM舵机的角度设置需要根据相应定时器PWM等参数进行计算(是否需要规范定时器PWM的初始化参数，以便于计算)
+void ServoSetAngle(ServoInstance *servo, float angle)
 {
-    switch (Servo_Motor->Servo_type)
+
+    switch (servo->servo_type)
     {
-    case Servo180:
-        if (S_angle > 180)
-            S_angle = 180;
+    case Bus_Servo:
+        servo_angle_write[8] = (uint16_t)angle&0xff;
+        servo_angle_write[9] = (uint16_t)angle>>8;
+        USARTSend(servo->usart_instance, servo_angle_write, 16, USART_TRANSFER_DMA);
+       // USARTSend(servo->usart_instance, servo_angle_read, 6, USART_TRANSFER_DMA);
         break;
-    case Servo270:
-        if (S_angle > 270)
-            S_angle = 270;
-        break;
-    case Servo360:
-        if (S_angle > 100)
-            S_angle = 100;
+    case PWM_Servo:
+        servo->angle = angle;
+        PWMSetDutyRatio(servo->pwm_instance, angle);
         break;
     default:
         break;
     }
-    if (S_angle < 0)
-        S_angle = 0;
-    Servo_Motor->Servo_Angle.free_angle = S_angle;
 }
 
-/**
- * @brief 写入起始，终止角度数值
- *
- * @param Servo_Motor 注册的舵机实例
- * @param Start_angle 起始角度
- * @param Final_angle 终止角度
- */
-void Servo_Motor_StartSTOP_Angle_Set(ServoInstance *Servo_Motor, int16_t Start_angle, int16_t Final_angle)
+//@todo 只读取了角度 还有电压，动作是否完成等 且只支持一个串口
+static void DecodeServo()
 {
-    Servo_Motor->Servo_Angle.Init_angle = Start_angle;
-    Servo_Motor->Servo_Angle.Final_angle = Final_angle;
-}
-/**
- * @brief 舵机模式选择
- *
- * @param Servo_Motor 注册的舵机实例
- * @param mode 需要选择的模式
- */
-void Servo_Motor_Type_Select(ServoInstance *Servo_Motor, int16_t mode)
-{
-    Servo_Motor->Servo_Angle_Type = mode;
-}
-
-/**
- * @brief 舵机输出控制
- *
- */
-void ServeoMotorControl()
-{
-    ServoInstance *Servo_Motor;
-
-    for (size_t i = 0; i < servo_idx; i++)
+    for (uint8_t i = 0; i < servo_idx; i++)
     {
-        if (servo_motor_instance[i])
+        if (servo_motor_instance[i]->servo_type == Bus_Servo)
         {
-            Servo_Motor = servo_motor_instance[i];
-
-            switch (Servo_Motor->Servo_type)
+            if (servo_motor_instance[i]->usart_instance->recv_buff[0] == Servo_Frame_First && servo_motor_instance[i]->usart_instance->recv_buff[1] == Servo_Frame_Second)
             {
-            case Servo180:
-                if (Servo_Motor->Servo_Angle_Type == Start_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 90;
-                if (Servo_Motor->Servo_Angle_Type == Final_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 90;
-                if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 90;
-                __HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
-                break;
-            case Servo270:
-                if (Servo_Motor->Servo_Angle_Type == Start_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Init_angle * 20000 / 20 / 135;
-                if (Servo_Motor->Servo_Angle_Type == Final_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.Final_angle * 20000 / 20 / 135;
-                if (Servo_Motor->Servo_Angle_Type == Free_Angle_mode)
-                    compare_value[i] = 0.5 * 20000 / 20 + Servo_Motor->Servo_Angle.free_angle * 20000 / 20 / 135;
-                __HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
-                break;
-            case Servo360:
-                /*500-2500的占空比 500-1500对应正向转速 1500-2500对于反向转速*/
-                compare_value[i] = 500 + 20 * Servo_Motor->Servo_Angle.servo360speed;
-                __HAL_TIM_SET_COMPARE(Servo_Motor->htim, Servo_Motor->Channel, compare_value[i]);
-                break;
-            default:
-                break;
+                if (servo_motor_instance[i]->usart_instance->recv_buff[3] == 21)
+                {
+                    servo_motor_instance[i]->recv_angle = (servo_motor_instance[i]->usart_instance->recv_buff[7] << 8 | servo_motor_instance[i]->usart_instance->recv_buff[6]);
+                }
             }
         }
     }
diff --git a/modules/motor/servo_motor/servo_motor.h b/modules/motor/servo_motor/servo_motor.h
index 319161d..66bcea9 100644
--- a/modules/motor/servo_motor/servo_motor.h
+++ b/modules/motor/servo_motor/servo_motor.h
@@ -1,90 +1,45 @@
-/**
- * @file servo_motor.h
- * @author panrui
- * @brief 舵机控制头文件
- * @version 0.1
- * @date 2022-12-12
- *
- * @copyright Copyright (c) 2022
- *
- */
-
 #ifndef SERVO_MOTOR_H
 #define SERVO_MOTOR_H
 
 #include "main.h"
 #include "tim.h"
 #include <stdint-gcc.h>
+#include "bsp_pwm.h"
+#include "bsp_usart.h"
 
 #define SERVO_MOTOR_CNT 7
-
-/*各种舵机类型*/
+#define Servo_Frame_First 0x55
+#define Servo_Frame_Second 0x55
+#define Servo_MAX_BUFF 10
+#define SERVO_MOVE_CMD 0x03
+#define SERVO_UNLOAD_CMD 0x14
+#define SERVO_POS_READ_CMD 0x15
 typedef enum
 {
-    Servo180 = 0,
-    Servo270 = 1,
-    Servo360 = 2,
-} Servo_Type_e;
+    Servo_None_Type = 0,
+    Bus_Servo = 1,
+    PWM_Servo = 2,
+}ServoType_e;
 
-/*舵机模式选择*/
-typedef enum
-{
-    Free_Angle_mode, // 任意角度模式
-    Start_mode,      // 起始角度模式
-    Final_mode,      // 终止角度模式
-} Servo_Angle_Type_e;
-/*角度设置*/
-typedef struct
-{
-    /*起止角度模式设置值*/
-    int16_t Init_angle;
-    int16_t Final_angle;
-    /*任意角度模式设置值*/
-    int16_t free_angle;
-    /*下述值仅仅适用于360°舵机
-     *设定值为0-100 为速度值百分比
-     *0-50为正转 速度由快到慢
-     *51-100为反转 速度由慢到快
-     */
-    int16_t servo360speed;
-} Servo_Angle_s;
 
 /* 用于初始化不同舵机的结构体,各类舵机通用 */
 typedef struct
 {
-    Servo_Type_e Servo_type;
-    Servo_Angle_Type_e Servo_Angle_Type;
-    // 使用的定时器类型及通道
-    TIM_HandleTypeDef *htim;
-    /*Channel值设定
-     *TIM_CHANNEL_1
-     *TIM_CHANNEL_2
-     *TIM_CHANNEL_3
-     *TIM_CHANNEL_4
-     *TIM_CHANNEL_ALL
-     */
-    uint32_t Channel;
-
-} Servo_Init_Config_s;
+    PWM_Init_Config_s pwm_init_config;
+    ServoType_e servo_type;
+    UART_HandleTypeDef *_handle;
+    uint8_t servo_id;
+}Servo_Init_Config_s;
 typedef struct
-{
-    Servo_Angle_Type_e Servo_Angle_Type;
-    Servo_Angle_s Servo_Angle;
-    Servo_Type_e Servo_type;
-    // 使用的定时器类型及通道
-    TIM_HandleTypeDef *htim;
-    /*Channel值设定
-     *TIM_CHANNEL_1
-     *TIM_CHANNEL_2
-     *TIM_CHANNEL_3
-     *TIM_CHANNEL_4
-     *TIM_CHANNEL_ALL
-     */
-    uint32_t Channel;
-} ServoInstance;
+{   
+    uint8_t servo_id;
+    float angle;
+    uint16_t recv_angle;
+    PWMInstance *pwm_instance;
+    USARTInstance *usart_instance;
+    ServoType_e servo_type;
+}ServoInstance;
 
 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 ServeoMotorControl();
+void ServoSetAngle(ServoInstance *servo, float angle);
 #endif // SERVO_MOTOR_H
\ No newline at end of file
diff --git a/modules/motor/servo_motor/servo_motor.md b/modules/motor/servo_motor/servo_motor.md
index 12d1079..d7cecd0 100644
--- a/modules/motor/servo_motor/servo_motor.md
+++ b/modules/motor/servo_motor/servo_motor.md
@@ -2,7 +2,7 @@
 
 <p align='left' >panrui@hnu.edu.cn</p>
 
-> todo: 由于新增了bsp_pwm的支持,舵机模块需要部分重构
+> todo: 角度设置等需要规范化
 ### 舵机基础知识
 
 已最常见的SG90舵机为例，SG90舵机要求工作在频率为50HZ——周期为20ms的PWM波，且对应信号的高低电平在0.5ms - 2.5ms之间，对应的舵机转动角度如下表所示（当然也可以按照这个线性的对应关系去达到转动自己想要的角度，如想要转动60°，则高电平脉宽为大概为1.2ms，具体能不能转到特定的角度还和舵机的精度有关）
@@ -21,50 +21,3 @@ eg：当初始占空比为1200/20000则为6%，根据20*6%=1.2ms （1.2-0.5）/(
 为了方便通过上述eg我们将所需要的角度与PWM计数值对应关系封装成函数。需要在初始化的時候输入我们所需要的角度和相关定时器参数即可。这样我们就可以设置SG90为参数范围内(0~180°)任意度数。
 
 ---
-
-## 如何注册一个舵机实例
-!!!
-
-**注意！由于舵机为开环控制，无论选择舵机为何种类型，舵机都能够正常运行，但是运行的角度可能会与设定不同，请务必正确选择舵机型号！且最多添加7个舵机！**
-我们可以像这样注册一个舵机实例
-```c
-
-static ServoInstance *leftservomoto;
-//初始化参数
-Servo_Init_Config_s config={
-    //舵机安装选择的定时器及通道
-    //C板有常用的7路PWM输出:TIM1-1,2,3,4 TIM8-1,2,3
-    .htim=&htim1,
-    .Channel=TIM_CHANNEL_1,
-    //舵机的初始化模式和类型
-    .Servo_Angle_Type=Start_mode,
-    .Servo_type=Servo180,
-};
-// 设置好参数后进行初始化并保留返回的指针
-leftservomoto = ServoInit(&config);
-```
->要控制一个舵机 我们提供了以下三个接口
-```c
-//自由模式下，写入自由角度数值
-void Servo_Motor_FreeAngle_Set(ServoInstance *Servo_Motor, int16_t S_angle);
-//起止模式下，写入起始，终止角度数值(防止反复写入起始和终止角度)
-void Servo_Motor_StartSTOP_Angle_Set(ServoInstance *Servo_Motor, int16_t Start_angle, int16_t Final_angle);
-/*
-    Free_Angle_mode, // 任意角度模式
-    Start_mode,      // 起始角度模式
-    Final_mode,      // 终止角度模式
-*/
-void Servo_Motor_Type_Select(ServoInstance *Servo_Motor,int16_t mode);
-//比如我们要使用舵机，并更改一个舵机的模式
-void ServoTask()
-{
-    //更改leftservomoto为Free_Angle_mode模式
-    Servo_Motor_Type_Select(leftservomoto,Free_Angle_mode);
-    //设置转到0角度
-    Servo_Motor_FreeAngle_Set(leftservomoto, 0);
-    //调用函数，控制电机
-    Servo_Motor_Control();
-}
-
-
-```
\ No newline at end of file
diff --git a/modules/vofa/vofa.c b/modules/vofa/vofa.c
deleted file mode 100644
index 298bbcf..0000000
--- a/modules/vofa/vofa.c
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * @Descripttion: 
- * @version: 
- * @Author: Chenfu
- * @Date: 2022-12-05 12:39:07
- * @LastEditTime: 2022-12-05 14:15:53
- */
-#include "vofa.h"
-
-/*VOFA浮点协议*/
-void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart )
-{
-    static uint8_t i = 0;
-    send_float temp[num];			//定义缓冲区数组
-    uint8_t send_data[4 * num + 4]; //定义通过串口传出去的数组，数量是所传数据的字节数加上4个字节的尾巴
-    for (i = 0; i < num; i++)
-    {
-        temp[i].float_t = data[i]; //将所传数据移到缓冲区数组
-    }
-    for (i = 0; i < num; i++)
-    {
-        send_data[4 * i] = temp[i].uint8_t[0];
-        send_data[4 * i + 1] = temp[i].uint8_t[1];
-        send_data[4 * i + 2] = temp[i].uint8_t[2];
-        send_data[4 * i + 3] = temp[i].uint8_t[3]; //将缓冲区数组内的浮点型数据转成4个字节的无符号整型，之后传到要通过串口传出的数组里
-    }
-    send_data[4 * num] = 0x00;
-    send_data[4 * num + 1] = 0x00;
-    send_data[4 * num + 2] = 0x80;
-    send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴
-
-    HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
-}
diff --git a/modules/vofa/vofa.h b/modules/vofa/vofa.h
deleted file mode 100644
index a6c9dd3..0000000
--- a/modules/vofa/vofa.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * @Descripttion: 
- * @version: 
- * @Author: Chenfu
- * @Date: 2022-12-05 12:39:18
- * @LastEditTime: 2022-12-05 13:37:36
- */
-#ifndef VOFA_H
-#define VOFA_H
-#include <stdint.h>
-#include "bsp_usart.h"
-#include "usart.h"
-
-typedef union
-{
-    float float_t;
-    uint8_t uint8_t[4];
-} send_float;
-void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart);
-
-#endif // !1#define 
\ No newline at end of file
diff --git a/modules/vofa/vofa.md b/modules/vofa/vofa.md
deleted file mode 100644
index 8782609..0000000
--- a/modules/vofa/vofa.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# vofa
-
-**除非迫不得已，否则强烈不推荐使用vofa进行调试。应通过bsp_log输出日志，或使用ozone可视化。**
\ No newline at end of file