PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   7 h+ A' r; H5 Y7 j6 L( v9 }6 P
3. #include <Servo.h>
   
4. 
   - y: L2 u$ v4 O* G4 o& k4 q
5. #include "MePort.h"
   ! e, ^' _" d: q9 H
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   ( H0 L7 j" A  q2 A2 D6 q# ]7 l
8. 
   
9. //double Input, Output ;
   : \& N6 ?. ~" K: \; z* k% j3 k, s- a
10. float MaxSpeed = 255;
    , R' \+ Y' _2 a
11. float MaxPower = 180;
    $ A7 ~& @+ N- N: O
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    1 h; f- L* R4 a  X0 R5 @6 m: N6 s+ O
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    9 i/ f, Z6 @/ R: o7 ~
18. 
    0 \2 _7 i; M7 q) D4 c" X
19. float nPower;
    
20. MePort lightsensor_6(6);
    9 f" h+ y4 [4 y1 F
21. MePort lightsensor_8(8);
    . I6 I6 A) l. Z2 V
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    . w9 E# T1 W' h' Z0 d( U- f! v# u: I
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    2 c4 p$ B& t1 Q. B
29.     nPower = 160;
    
30.     Error=0;
    0 F" q1 `2 Y' G+ B$ l' D
31.     ErrorAcc=0;
    7 G/ X9 g4 ^6 `! ~
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    7 e* W  V7 ?. G  {9 a5 P' n
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
      a9 n  R5 k0 H( L, S. L
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    : Q7 `8 t5 {4 @7 }5 Q
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    & Z! T! B. p1 k
44.       }
    - G+ k2 ]4 |, z7 j  J4 H$ X
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    . n2 j/ ^, B9 l) ?
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    2 H2 Y  {/ q% u. ~
53.     motor_9.run(0);
    ) d2 u- h. }2 C/ G9 `
54.     motor_10.run(0);
    - A8 d! O* N$ A$ [* _2 Y( {
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
      r, i& I- ]: k( A& d, H' i* O4 ]
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    - q$ ?* }( h( C! D" \6 s$ K5 T
61.      Power = MaxSpeed;
    # Z! O  |( b. J6 n( N- f
62.   }
    1 N7 N7 p9 P7 X' v; T9 J
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    : k$ ]$ j: c& [/ K  c
65.   }
      H4 t5 ~+ D) i
66.   motor_9.run(Power);
    
67. }  
    0 u& L; c# x  p; Q" U7 w
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    0 v  Z% w! ^  G2 ^4 \- F8 K* W' V
71.      Power = MaxSpeed;
    
72.   }
    4 u9 I5 N/ m; S& G; l7 N/ J! u
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    9 n. n2 \0 m8 ]" W
77. }  

複製代碼

) U( j7 u4 K$ ^3 L. ^, @

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
0 S6 B$ ]3 y$ P" b! }) I) y雖然已使用較繁雜的方式處理了跑出黑線外的狀況
: W+ F4 B$ r# H/ g# f5 O- r( m
4 Z3 t0 [$ Z" u0 \* N4 L但基於想追求更精簡的程式所以還是想請問一下
' F7 l5 K( y; j! N( u就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
" G/ A0 B1 m0 f0 s& t5 E; l不知道您是否願意教我可以如何處理
: S4 E5 _% Y% h7 k& _: r8 Z4 F0 c
3 n6 V. c2 V% g9 j" X4 r; ~/ f( \
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   + Z, U, l( F# h4 N* B5 |
3. int interror = 0;
   
4. int olderror = 0;
   4 s# R+ p- [6 q( M$ O4 w
5. double values;
   * V/ @" v: ^6 e0 H
6. 
   
7. 
   3 R5 Q, I1 a2 U) t& z
8. void CNY70()
   5 D$ I* i' v& S' [, y. d
9. {
   5 l) A% W7 H5 `( T: z9 a7 f' G
10.   valuesRR = analogRead(RR)
    : H- X6 T! H3 e5 E2 g
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    9 u1 d1 l8 D3 l9 P4 v- k
14.   valuesLL = analogRead(LL);
    
15. 
    ) A' r7 n# w; s/ p$ h4 _& Q4 v
16.   if (valuesRR > CNY70Val)
    ' z0 ^+ n! p3 M
17.     valuesRR = CNY70Val;
    ; ]4 o  |2 X/ W/ _: r7 B" O
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    + q6 I" k/ A5 N$ i4 O# l  U) k8 z
24.   if (valuesLL > CNY70Val)
    , u! h) @5 Y' Q9 {
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    2 ]0 [9 E! _) U
30. void Car()
    
31. {
    * a1 I# b! F: `
32.   while (1) {
    5 [( V/ F/ o/ }9 v/ e* Y3 D
33.     CNY70();
    
34. 
    2 l0 T1 i3 x" b5 l
35.     int error = ((int)values);
    ' |1 J9 v5 H0 d* {: a/ @
36.     interror += error;
    3 _& A0 R  t' b
37.     int lasterror = error - olderror;
    ! u0 ?, Q9 v' Y+ b# u9 {
38.     olderror = error;
    5 o, g, e! w: A" r) T' S
39.     int power = error / 5 + interror / 10000 + lasterror;
    ) n' R9 C0 j( |9 u
40. 
    4 m2 m6 j2 _, d' z
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    $ X- C! r) L; a, v; B* d
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    8 H7 B1 Y; }( S# [. q; u. A1 E
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    4 b0 t: ^. A$ P! o
48.     else
      O( c; M. @. H' Y# p2 {
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    . q1 [7 ^& ^9 h3 e& w
51. }

複製代碼

感謝分享

感謝分享