极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 13879|回复: 0

求助PID输出恒为0

[复制链接]
发表于 2018-5-9 17:27:29 | 显示全部楼层 |阅读模式
求助,我刚开始用PID,借鉴论坛里面的程序写了个用角度传感器来控制小车走直线和斜线的PID,但发现输出恒为0,求指点一二。
  1. /*
  2. Name:                Sketch_work.ino
  3. Created:        2018/5/5 13:48:51
  4. Author:        Qi
  5. */
  6. #include <FlexiTimer2.h>
  7. #include <Servo.h>
  8. #include <PID_v1.h>

  9. Servo Vacum_c;//定义真空控制单位
  10. #define Red1 26  //定义红外传感器数字接口
  11. #define Red2 28
  12. #define Red3 30
  13. #define Red4 32
  14. #define L1 2   //定义L298N驱动板数字接口
  15. #define L2 3
  16. #define R1 4
  17. #define R2 5
  18. #define Vacum 6  //真空泵数字接口
  19. #define Vo A0   //角度传感器模拟接口
  20. double setpointF=420, setpointL=480, setpointR=340;//设置直线行走角度设定值,左侧行走角度设定值,右侧行走角度设定值
  21. double outptF, outputL, outputR;//设置直线行走输出值,左侧行走输出值
  22. double Kp = 4, Ki = 0, Kd = 0;//设置PID常量
  23. double inputF, inputL, inputR;//设置直线行走时候角度角度输入值,左侧行走角度输入值,右侧行走角度输入值
  24. int pwmR = 220;//设置右侧电机固定PWM值
  25. int angle; //角度读取模拟值

  26. PID PIDF(&inputF, &outptF, &setpointF, Kp, Ki,Kd, DIRECT);//设置直线行驶PID
  27. PID PIDL(&inputL, &outputL, &setpointL, Kp, Ki, Kd,DIRECT);//设置左侧行驶PID
  28. PID PIDR(&inputR, &outputR, &setpointR, Kp, Ki, Kd, DIRECT);//设置右侧行驶PID
  29. void setup() {
  30.         Serial.begin(9600);
  31.         Vacum_c.attach(Vacum, 500, 2500);
  32.         Vacum_c.writeMicroseconds(800);
  33.         delay(1000);
  34.         Vacum_c.writeMicroseconds(2000);
  35.         foword();
  36.         pinMode(Red1, INPUT);
  37.         pinMode(Red2, INPUT);
  38.         pinMode(Red3, INPUT);
  39.         pinMode(Red4, INPUT);
  40.         pinMode(Vo, INPUT);
  41.         //FlexiTimer2::set(200, Rangle);
  42.         //FlexiTimer2::start();
  43.         PIDF.SetSampleTime(200);
  44.         PIDF.SetOutputLimits(0, 255);
  45.         PIDF.SetMode(AUTOMATIC);
  46.         PIDL.SetSampleTime(200);
  47.         PIDL.SetOutputLimits(0, 255);
  48.         PIDL.SetMode(AUTOMATIC);
  49.         PIDR.SetSampleTime(200);
  50.         PIDR.SetOutputLimits(0, 255);
  51.         PIDR.SetMode(AUTOMATIC);
  52. }
  53. void Rangle()
  54. {
  55.         angle = analogRead(Vo);
  56.         inputF = inputL = inputR = angle;
  57. }
  58. void foword() //直线前进
  59. {
  60.         setpointF = 420;
  61.         PIDF.Compute();
  62.         digitalWrite(L1, outptF);
  63.         digitalWrite(L2, LOW);
  64.         digitalWrite(R1, pwmR);
  65.         digitalWrite(R2, LOW);
  66. }
  67. void reverse()  //直线后退
  68. {
  69.         setpointF = 420;
  70.         PIDF.Compute();
  71.         digitalWrite(L1, LOW);
  72.         digitalWrite(L2, outptF);
  73.         digitalWrite(R1, LOW);
  74.         digitalWrite(R2, pwmR);
  75. }
  76. void brake()  //停止
  77. {
  78.         digitalWrite(L1, LOW);
  79.         digitalWrite(L2, LOW);
  80.         digitalWrite(R1, LOW);
  81.         digitalWrite(R2, LOW);
  82. }
  83. void upturnR()  //上方右转
  84. {
  85.         do{
  86.                 digitalWrite(L1, HIGH);
  87.                 digitalWrite(L2, LOW);
  88.                 digitalWrite(R1, LOW);
  89.                 digitalWrite(R2, HIGH);
  90.         } while (angle > 340);
  91. }
  92. void upturnL()  //上方左转
  93. {
  94.         do{
  95.                 digitalWrite(L1, LOW);
  96.                 digitalWrite(L2, HIGH);
  97.                 digitalWrite(R1, HIGH);
  98.                 digitalWrite(R2, LOW);
  99.         } while (angle<480);
  100. }
  101. void dow_turnL()  //下方右转
  102. {
  103.         do{
  104.                 digitalWrite(L1, LOW);
  105.                 digitalWrite(L2, HIGH);
  106.                 digitalWrite(R1, HIGH);
  107.                 digitalWrite(R2, LOW);
  108.         } while (angle<480);
  109. }
  110. void dow_turnR() //下方左转
  111. {
  112.         do{
  113.                 digitalWrite(L1, LOW);
  114.                 digitalWrite(L2, HIGH);
  115.                 digitalWrite(R1, HIGH);
  116.                 digitalWrite(R2, LOW);
  117.         } while (angle > 340);
  118. }
  119. void line_L()  //左侧直线
  120. {
  121.         setpointL = 345;
  122.         PIDL.Compute();
  123.         digitalWrite(L1, LOW);
  124.         digitalWrite(L2, outputL);
  125.         digitalWrite(R1, LOW);
  126.         digitalWrite(R2, pwmR);
  127. }
  128. void line_R()  //右侧直线
  129. {
  130.         setpointR = 475;
  131.         PIDR.Compute();
  132.         digitalWrite(L1, LOW);
  133.         digitalWrite(L2, outputR);
  134.         digitalWrite(R1, LOW);
  135.         digitalWrite(R2, pwmR);
  136. }
  137. void loop() {
  138.         Rangle();
  139.         boolean I1, I2, I3, I4;
  140.         I1 = digitalRead(Red1);
  141.         I2 = digitalRead(Red2);
  142.         I3 = digitalRead(Red3);
  143.         I4 = digitalRead(Red4);
  144.         if (I1 == 1)
  145.         {
  146.                 brake();
  147.                 delay(30);
  148.                 reverse();
  149.                 delay(500);
  150.                 upturnR();
  151.                 line_L();
  152.         }
  153.         if (I2 == 1)
  154.         {
  155.                 brake();
  156.                 delay(50);
  157.                 reverse();
  158.                 delay(500);
  159.                 upturnR();
  160.                 line_L();
  161.         }
  162.         if (I3 == 1)
  163.         {
  164.                 brake();
  165.                 delay(50);
  166.                 foword();
  167.                 delay(500);
  168.                 //dow_turnL();
  169.                 foword();
  170.         }
  171.         if (I4 == 1)
  172.         {
  173.                 brake();
  174.                 delay(50);
  175.                 foword();
  176.                 delay(500);
  177.                 //dow_turnR();
  178.                 foword();
  179.         }
  180.         if (I1 = 1 && I2 == 1)
  181.         {
  182.                 brake();
  183.                 delay(50);
  184.                 reverse();
  185.                 delay(500);
  186.                 upturnR();
  187.                 line_L();
  188.         }
  189.         if (I3 == 1 && I4 == 1)
  190.         {
  191.                 brake();
  192.                 delay(50);
  193.                 foword();
  194.         }
  195.         Serial.print("inputF=");
  196.         Serial.println(inputF);
  197.         Serial.print("angle=");
  198.         Serial.println(angle);
  199.         Serial.print("outputF=");
  200.         Serial.println(outptF);
  201. }
复制代码
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-28 21:23 , Processed in 0.040403 second(s), 17 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表