---

---

前言

在STM32F103RCT6上使用ToF（Time-of-Flight）模塊（如VL53L0X、VL53L1X或TFmini）結合PID算法實現穩定距離控制，適用于高精度場景（如自動跟隨、避障或工業定位）。以下是簡單實現步驟：

---

1. 硬件準備

主控芯片

主控芯片：STM32F103RCT6（Cortex-M3，72MHz，足夠處理ToF數據與PID運算）。

ToF模塊

1.VL53L0X

VL53L0X：測距范圍30cm~2m，精度±3mm，I2C接口。

2.TFmini

TFmini：串口通信，測距0.3m~12m，精度1%。

執行機構：

電機

電機（直流電機+編碼器/PWM調速）。

舵機

舵機（用于方向調整，可選）。

其他

其他：電源、電機驅動（如TB6612）、OLED（顯示距離，可選）。

2. 硬件連接

(1) VL53L0X（I2C接口）

VL53L0X引腳 STM32引腳 說明
VCC 3.3V 模塊供電
GND GND 共地
SDA PB7 I2C數據線
SCL PB6 I2C時鐘線
XSHUT PA8 復位引腳（可選）

(2) TFmini（串口通信）

TFmini引腳 STM32引腳 說明
VCC 5V 模塊供電
GND GND 共地
TX PA10 接STM32的RX
RX PA9 接STM32的TX

3. ToF模塊初始化與數據讀取

(1) VL53L0X（基于HAL庫）

#include "vl53l0x.h"
VL53L0X_Dev_t dev = {.i2c_handle = &hi2c1}; // I2C初始化略void ToF_Init() {VL53L0X_Error status;status = VL53L0X_Init(&dev);if (status != VL53L0X_ERROR_NONE) {printf("ToF init failed!\n");}VL53L0X_StartMeasurement(&dev);
}float Get_Distance() {VL53L0X_RangingMeasurementData_t data;VL53L0X_GetRangingMeasurementData(&dev, &data);return data.RangeMilliMeter / 10.0f; // 轉換為cm
}

(2) TFmini（串口接收）

uint8_t tfmini_buffer[9];
float Get_Distance() {HAL_UART_Receive(&huart1, tfmini_buffer, 9, 100); // 接收9字節數據幀if (tfmini_buffer[0] == 0x59 && tfmini_buffer[1] == 0x59) { // 幀頭校驗uint16_t distance = tfmini_buffer[2] + (tfmini_buffer[3] << 8);return distance / 100.0f; // 轉換為米}return -1; // 無效數據
}

4. PID算法實現

PID控制器通過調節輸出使當前距離（反饋值）趨近目標距離（設定值）。

(1) PID結構體定義

typedef struct {float Kp, Ki, Kd;    // PID參數float target;        // 目標距離（單位與ToF一致）float error, last_error, integral;float output_max, output_min; // 輸出限幅
} PID_Controller;PID_Controller pid = {.Kp = 0.8, .Ki = 0.05, .Kd = 0.2,.target = 50.0,         // 目標距離50cm.output_max = 100, .output_min = -100
};

(2) PID計算函數（帶抗積分飽和）

float PID_Update(PID_Controller *pid, float current) {pid->error = pid->target - current;// 積分項（抗飽和）pid->integral += pid->error;if (pid->integral > pid->output_max) pid->integral = pid->output_max;else if (pid->integral < pid->output_min) pid->integral = pid->output_min;// 微分項（抑制突變）float derivative = pid->error - pid->last_error;pid->last_error = pid->error;// PID輸出（限幅）float output = pid->Kp * pid->error + pid->Ki * pid->integral + pid->Kd * derivative;if (output > pid->output_max) output = pid->output_max;else if (output < pid->output_min) output = pid->output_min;return output;
}

5. 控制執行機構

(1) 電機控制（PWM調速）

// 初始化PWM（TIM4通道1，PB6）
void PWM_Init() {TIM_OC_InitTypeDef sConfigOC = {0};htim4.Instance = TIM4;htim4.Init.Prescaler = 71;       // 1MHz頻率htim4.Init.Period = 999;         // 1kHz PWMHAL_TIM_PWM_Init(&htim4);sConfigOC.Pulse = 0;             // 初始占空比0sConfigOC.OCMode = TIM_OCMODE_PWM1;HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
}// 設置電機速度和方向
void Set_Motor(float pid_output) {uint16_t pwm = (uint16_t)fabs(pid_output);__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, pwm);// 方向控制（假設PB0為方向引腳）HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, pid_output > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

(2) 舵機控制（角度調整）

// 設置舵機角度（PID輸出映射到0~180°）
void Set_Servo(float pid_output) {uint16_t angle = 90 + (int16_t)pid_output; // 示例：PID輸出±30對應60°~120°uint16_t pwm = 500 + angle * 2000 / 180;   // 0.5ms~2.5ms__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwm);
}

6. 主循環邏輯

int main(void) {HAL_Init();SystemClock_Config();MX_GPIO_Init();MX_I2C1_Init();    // VL53L0X使用I2C// MX_USART1_UART_Init(); // TFmini使用串口PWM_Init();ToF_Init();float current_dist, pid_output;while (1) {current_dist = Get_Distance();          // 獲取當前距離pid_output = PID_Update(&pid, current_dist);Set_Motor(pid_output);                // 控制電機// Set_Servo(pid_output);             // 或控制舵機HAL_Delay(20); // 控制周期20ms（50Hz）}
}

7. 關鍵優化與問題處理

(1) ToF數據濾波

移動平均濾波：
#define FILTER_SIZE 5
float filter_buffer[FILTER_SIZE];
float Filter_Distance(float new_value) {static uint8_t index = 0;filter_buffer[index++] = new_value;if (index >= FILTER_SIZE) index = 0;float sum = 0;for (uint8_t i = 0; i < FILTER_SIZE; i++) sum += filter_buffer[i];return sum / FILTER_SIZE;
}

(2) PID參數整定

階躍響應法

設Ki=0, Kd=0，逐漸增大Kp直到系統振蕩，然后取50%的值。
加入Kd抑制超調，最后加Ki消除穩態誤差。

典型參數范圍：

Kp: 0.5~2.0（比例增益）
Ki: 0.01~0.1（積分時間）
Kd: 0.1~0.5（微分時間）

(3) 動態目標適應

若目標距離變化頻繁，可加入動態參數調整：
if (fabs(pid.error) > 20) pid.Kp = 1.5; // 大誤差時提高響應
else pid.Kp = 0.8;

8. 實際應用注意事項

ToF模塊限制

ToF模塊限制：VL53L0X在強光下性能下降，需避免直射陽光。

實時性

實時性：控制周期建議20~50ms，過短可能導致PID震蕩。

機械延遲

機械延遲：電機響應滯后時，需增加Kd或降低Ki。
通過上述步驟，STM32F103RCT6可精確控制物體與ToF模塊間的距離。實際調試時需結合硬件特性（如電機慣性、ToF精度）優化參數。

---