1. 首页
  2. IT资讯

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; }

谢谢观看11 记得点赞 · 加油学习 7 · 有问题私信我 1万+

原文始发于:

主题测试文章,只做测试使用。发布者:夏枳★,转转请注明出处:http://www.cxybcw.com/118279.html

联系我们

13687733322

在线咨询:点击这里给我发消息

邮件:1877088071@qq.com

工作时间:周一至周五,9:30-18:30,节假日休息

QR code