PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   . v; y8 t! ]3 o" [# A# x' ]8 H- |" E
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   / J1 R( m0 x" w+ ^5 i6 _# V% B; o
7. #include "MeDCMotor.h"
   
8. 
   $ w3 x6 Y& m2 R5 a. f) v: E
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    & ^0 _- W, ^! f! k  R& b
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    + Y! \* Q. w* k& c
14. 
    
15. float Kp=0.14;
    " l, t) t2 ]0 ^+ ?# p
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    . v2 v% h  v' ?! B) H/ i; n! A
18. 
    7 E9 X% S5 P# d! M* Z- I
19. float nPower;
    % i8 ~* J8 O* U2 R' p0 b
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ' |$ l9 h* l( k0 ^+ ^0 _: `
22. MeDCMotor motor_9(9);
    6 y; i, Y, g( A8 \, d  u
23. MeDCMotor motor_10(10);
    & z) V- E- N# B* s9 p7 x
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    ! |/ s; O7 h) F9 e) S- j
26. 
    4 P: \; N3 f* t
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    1 V, S3 H, y5 ~5 E4 P
32. }
    
33. 
    / u, H* [, q) {3 u! Y1 E3 E3 ^
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    6 F, l6 ^) \  P
38.     ErrorAcc +=  nError*Kd ;
      \) @$ }% p! `0 M$ q# d
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    " c; v: |% P1 p7 H5 O, G1 S1 Q
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    " B' O' j2 p4 [
44.       }
    
45.     } else{
    : d+ U7 u" R4 ]
46.       if(nPower > MinPower){
    3 q$ T- c. o; I  k, K! L7 i
47.         nPower -= 2;
    
48.       }
    * _4 _6 l( I* y. A4 `0 a7 L
49.     }
    
50.     MotoL(nPower-Error);
    3 z0 b/ ~7 o0 e
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    . o/ A) p2 C3 F0 ^7 d
58. 
    % g. s: \4 v. E7 F2 d. b0 L
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    & L! b2 G( }7 }& u* b
61.      Power = MaxSpeed;
    ; ?* R& M+ U! d# k! e+ v& @
62.   }
    - q" E4 s: R+ d8 V, b$ [
63.   if (Power < -MaxSpeed){
    0 K  j, T( C6 h6 S8 z2 b. O) ^) q
64.      Power = -MaxSpeed;
    
65.   }
    0 G/ X4 `" u# V
66.   motor_9.run(Power);
    + [$ x7 f& n7 O6 L. @! M
67. }  
    9 c0 Q4 a9 y- C; U# D
68. 
    8 q% K# z/ o4 ?( E7 u+ w9 r
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    , g4 k, P" R7 f6 F0 H: v0 [
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    : C) n# j) M/ ]5 d' G) Y
76.   motor_10.run(Power);
    
77. }  

複製代碼

# a3 o  B. \5 q6 O" R

8 x' y+ p* \" n! h9 R您好，不知是否能向您請教。
. Z. Z- I( N/ x4 [' c4 R, ]目前和宋修賢老師在處理Ardui Car
' t/ P- p1 c$ e; N* K' ^1 P7 g雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
( R$ M  H: L" i不知道您是否願意教我可以如何處理

" p+ c# g' c9 R4 n# S
6 a( A# [* a! Q0 [; r, z- v以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ( |2 j/ R$ c& A' ]* L- Q
3. int interror = 0;
   . u7 J, o2 v& M  }& a9 p+ m
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   + g2 ]3 m9 A& ?9 R7 L1 j* f
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    & d9 q* I2 k' T1 }- @. F  R4 t5 F, N
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    4 t5 ~8 l0 M$ i
16.   if (valuesRR > CNY70Val)
    9 g: X1 F& h7 E& a$ o8 Q
17.     valuesRR = CNY70Val;
    % U  T7 W/ O& q" B$ p" B& l6 J
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ! W, f% D4 B) |$ y& d# F/ d7 [
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    7 ^- a: {" o6 x7 ^2 a9 R( s
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    . j/ m: Y- n- i5 @0 E  I6 e7 Q
31. {
    
32.   while (1) {
    
33.     CNY70();
    / U. f+ G+ I3 g3 c( Z
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    8 r7 j8 R/ Q8 S9 h# f& h+ }
37.     int lasterror = error - olderror;
    - ~" z, h4 w0 V: D& N
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    + f! u2 q1 ]4 u) B) N
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    " G5 z' ]* q% V
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ' ?( U# G- @1 |
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    / p, c8 l5 m; G' ^2 C  l
50.   }
    . Y$ X# [+ R3 L% Q4 ~
51. }

複製代碼

感謝分享

感謝分享