Arduino智能车PID参数调校

15113753731

您好！请问有大神，能解决这个问题吗？
long L1,L2,L3,Sensor;                 //三颗传感器读取值

int PWM_L,PWM_R,PWM_PD,Bias,Bias_Last;    //左右轮驱动电机的PWM值，PD计算后PWM的增量，偏差值，上一次偏差值

static int PWM_ini = 180;             //驱动电机的PWM初始值

float Kp = （）, Kd = （）;               //Kp影响过弯后抖动幅度，Kd太大小车直线抖动

#define ENA_R 10         //右边电机转速控制

#define IN1 9         //右边电机正反转控制

#define IN2 8       //右边电机正反转控制

#define ENA_L 5         //左边电机转速控制

#define IN3 6         //左边电机正反转控制

#define IN4 7       //左边电机正反转控制

#define stopPin 2      //红外避障传感器信号连接端子



void setup() {

  pinMode(IN1,OUTPUT);

  pinMode(IN2,OUTPUT);

  pinMode(IN3,OUTPUT);

  pinMode(IN4,OUTPUT);

  pinMode(ENA_L,OUTPUT);

  pinMode(ENA_R,OUTPUT);

  pinMode(stopPin,INPUT_PULLUP);    //外部中断输入上拉

  attachInterrupt(0, carStop, LOW); //触发外部中断，进入停车子程序

  Serial.begin(9600);

  delay(1000);

}



void loop() {

  readSensor();

  PDcontrol();

}



void readSensor(){                 //传感器信号读取处理子程序

  L1 = analogRead(A1);

  L2 = analogRead(A2);

  L3 = analogRead(A3);

  if ((L1 + L2 + L3)> 100){              //合理设置该值可以让小车跑偏时停车

    Sensor = (L1 * 1 + L2 * 50 + L3 * 99) / (L1 + L2 + L3);   //归一化处理

  }

  else{                                  //当传感器值低于预期时，进入停车子程序，防止小车冲出跑道

    carStop();

  }

}



void motorDrive(int leftVel,int rightVel){   //电机驱动子程序

  digitalWrite(IN1,HIGH);

  digitalWrite(IN2,LOW);

  digitalWrite(IN3,LOW);

  digitalWrite(IN4,HIGH);

  analogWrite(ENA_L,leftVel);

  analogWrite(ENA_R,rightVel);

}



void PDcontrol(){                   //PD算法处理子程序

  Bias = Sensor - 50;

  PWM_PD = Kp*Bias + Kd*(Bias - Bias_Last);

  Bias_Last = Bias;

  PWM_L = PWM_ini - PWM_PD;

  PWM_R = PWM_ini + PWM_PD;

  if(PWM_L > 255){

    PWM_L = 255;

  }

  if(PWM_L < 0){

    PWM_L = 0;

  }

  if(PWM_R > 255){

    PWM_R = 255;

  }

  if(PWM_R < 0){

    PWM_R = 0;

  }

  motorDrive(PWM_L,PWM_R);

}



void carStop(){              //终点停车子程序

  digitalWrite(IN1,HIGH);

  digitalWrite(IN2,HIGH);

  digitalWrite(IN3,HIGH);

  digitalWrite(IN4,HIGH);

  digitalWrite(ENA_L,HIGH);

  digitalWrite(ENA_R,HIGH);

}