电子节气门控制系统

/* 电子节气门控制系统
 * 驱动: L298N/BTS7960
 * 反馈: 电位计 (ADC)
 */

// 引脚定义
const int PIN_PWM_FWD = 9;   // 正向驱动
const int PIN_PWM_REV = 10;  // 反向驱动
const int PIN_TPS_FB  = A0;  // 节气门位置传感器

// PID 常数 (可调)
double Kp = 2.5; 
double Ki = 0.1;
double Kd = 1.0;

// 变量
double setpoint = 0;    // 目标角度 (0-90度)
double input = 0;       // 当前角度
double output = 0;      // PWM 输出
double error, last_error, integral;

// 定时
unsigned long last_time;
const int SAMPLE_TIME = 10; // 10ms 循环 (100Hz)

void setup() {
  pinMode(PIN_PWM_FWD, OUTPUT);
  pinMode(PIN_PWM_REV, OUTPUT);
  Serial.begin(115200);
  
  // 安全初始化：关闭状态
  analogWrite(PIN_PWM_FWD, 0);
  analogWrite(PIN_PWM_REV, 0);
}

void loop() {
  unsigned long now = millis();
  
  if (now - last_time >= SAMPLE_TIME) {
    // 1. 读取反馈
    int raw_adc = analogRead(PIN_TPS_FB);
    input = map(raw_adc, 0, 1023, 0, 90); // 校准到角度
    
    // 2. 读取目标 (例如从串口或踏板)
    // setpoint = ...; 

    // 3. 计算 PID
    error = setpoint - input;
    integral += (error * SAMPLE_TIME);
    double derivative = (error - last_error) / SAMPLE_TIME;
    
    // 积分抗饱和
    if (integral > 1000) integral = 1000;
    if (integral < -1000) integral = -1000;

    output = (Kp * error) + (Ki * integral) + (Kd * derivative);

    // 4. 驱动电机
    driveMotor(output);

    // 5. 更新状态
    last_error = error;
    last_time = now;
  }
}

void driveMotor(double pwm_val) {
  // 限制 PWM 到 8 位范围
  int drive = (int) constrain(abs(pwm_val), 0, 255);
  
  // 死区补偿 (可选，取决于电机)
  if (drive < 20) drive = 0; 

  if (pwm_val > 0) {
    // 打开节气门
    analogWrite(PIN_PWM_FWD, drive);
    analogWrite(PIN_PWM_REV, 0);
  } else {
    // 关闭节气门
    analogWrite(PIN_PWM_FWD, 0);
    analogWrite(PIN_PWM_REV, drive);
  }
}