ROS小车地盘4 PID算法

PID算法：

不需要对PID的源码做改动就可以直接用。需要指出的是，它的左右轮的PID用的是同一套Kp,Kd,Ki,Ko， 如果你的两个电机特性差异比较大，就要对这块做下优化。

代码如下：

typedef struct {  double TargetTicksPerFrame;    // target speed in ticks per frame：每帧的目标速度  long Encoder;                  // encoder count：编码器计数  long PrevEnc;                  // last encoder count：最后一组计数  int PrevInput;                // last input：最后的输入  int ITerm;                    //integrated term：差分项  long output;                    // last motor setting：最后一个电机设置}//定义一个结构体SetPointInfo;SetPointInfo leftPID, rightPID;int Kp = 20;int Kd = 12;int Ki = 0;int Ko = 50;unsigned char moving = 0; // is the base in motion?电机运动的基准?void resetPID(){   leftPID.TargetTicksPerFrame = 0.0;   leftPID.Encoder = readEncoder(LEFT);   leftPID.PrevEnc = leftPID.Encoder;   leftPID.output = 0;   leftPID.PrevInput = 0;   leftPID.ITerm = 0;   rightPID.TargetTicksPerFrame = 0.0;   rightPID.Encoder = readEncoder(RIGHT);   rightPID.PrevEnc = rightPID.Encoder;   rightPID.output = 0;   rightPID.PrevInput = 0;   rightPID.ITerm = 0;}void doPID(SetPointInfo * p) {  long Perror;  long output;  int input;  input = p->Encoder - p->PrevEnc;  Perror = p->TargetTicksPerFrame - input;  output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;  p->PrevEnc = p->Encoder;  output += p->output;  if (output >= MAX_PWM)    output = MAX_PWM;  else if (output <= -MAX_PWM)    output = -MAX_PWM;  else    p->ITerm += Ki * Perror;  p->output = output;  p->PrevInput = input;}void updatePID() {  leftPID.Encoder = readEncoder(LEFT);  rightPID.Encoder = readEncoder(RIGHT);  if (!moving){    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();    return;  }  doPID(&rightPID);  doPID(&leftPID);  setMotorSpeeds(leftPID.output, rightPID.output);}long readPidIn(int i) {  long pidin=0;    if (i == LEFT){    pidin = leftPID.PrevInput;  }else {    pidin = rightPID.PrevInput;  }  return pidin;}long readPidOut(int i) {  long pidout=0;    if (i == LEFT){    pidout = leftPID.output;  }else {    pidout = rightPID.output;  }  return pidout;}