PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   % b* T6 b1 ?& C& |; P+ B
3. #include <Servo.h>
   ! F- w+ D2 w+ @- a6 m
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   ! C) V2 y2 m& w9 A$ I
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    0 t  P5 W5 y6 W! \3 w
13. float Error,ErrorAcc,ErrorDec;
    / J- g! q' t$ W: [" v) L9 U) M
14. 
    
15. float Kp=0.14;
    ' |. J8 I* E7 D2 \
16. float Kd=0.00020;//23;
      c  L( L7 b0 H5 V: u+ T
17. float Ki=0.000201;
    , N; A8 l6 W2 Q* b. Y" q" P* @
18. 
    - a$ O, @; H' n  J" o2 K4 p
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    % O- ]0 f" s0 X' H" I
22. MeDCMotor motor_9(9);
    - O5 Z8 f8 M9 i3 D; |# r
23. MeDCMotor motor_10(10);
    8 y/ U; X' B8 F3 X6 q) F3 J
24. unsigned long previousMillis = 0;
    1 W3 v$ e8 b/ j8 ]/ T, ]$ o
25. const long interval = 1;
    
26. 
    
27. void setup(){
    * y* T' z; i, Y# ]) ]
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    . \+ ?6 q! G' S; A; {& K' P
30.     Error=0;
    
31.     ErrorAcc=0;
    , Q/ v5 h% N& S1 G
32. }
    + H. C+ G9 ]- [( x+ r
33. 
    
34. void loop(){
    & R9 _) F- R0 C/ Y
35.   unsigned long StartTime = millis();
    - ?" A( {/ D: O4 u" H  R
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    / b' p1 f! m/ @. \& G' R& ~3 x
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    # D, B$ i7 ~: P$ j
44.       }
    5 z  x* d4 t! b/ S* |, k. f+ s6 Z; M
45.     } else{
    * ^1 E7 A" R; K- T6 x
46.       if(nPower > MinPower){
    & B/ ^" M' @9 A
47.         nPower -= 2;
    8 x/ A$ |  |5 V3 m2 _, X. F. u
48.       }
    
49.     }
    0 i7 L) p1 u2 I6 m6 O( ?/ u5 X* Y
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    3 N8 H5 k/ g5 b9 x& R% W
52.   }else{
    
53.     motor_9.run(0);
    9 t* G( {8 P( h2 O
54.     motor_10.run(0);
    
55.   }
      B* |$ M) d  Y* X& o+ j# w  V
56.   do{}while(millis() - StartTime < interval);
    : a+ ^: v! T- s+ H2 k( M! M
57. }
    
58. 
    4 l+ M: V. b9 U/ j4 ^6 T
59. void MotoL(int Power){
    9 K* g5 g' A: A! P
60.   if (Power > MaxSpeed){
    9 U3 z  Q; W5 ~* m
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    0 x1 v6 E& K8 R0 V, i" G+ {: y, p
67. }  
    
68. 
    
69. void MotoR(int Power){
    4 Q  q  B" p9 v; r+ S
70.   if (Power > MaxSpeed){
    % }. j1 J7 h$ }" ^# ^/ h
71.      Power = MaxSpeed;
    $ l: s* n# L$ f6 ^* v, j2 j; _: r
72.   }
    
73.   if (Power < -MaxSpeed){
    7 ^; t( M* Z* @# l4 v
74.      Power = -MaxSpeed;
    * M, c. `1 u0 B( k
75.   }
    
76.   motor_10.run(Power);
    ) l; A8 Z1 x& y6 p* ]8 p
77. }  

複製代碼

7 q% V1 o: m6 u+ ]( U
; ~$ T! A5 A+ h, B: X, @) ~3 q

8 k9 A9 \  W5 }  k7 B7 v9 G4 J
  |9 q+ c  X8 m' {- K* D' K您好，不知是否能向您請教。
3 Y* N" T4 o# F! m* C目前和宋修賢老師在處理Ardui Car
# L1 x$ l: l! f, s% c+ J7 Z4 h雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
% r, t1 |2 R: Y就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
9 Y! N* Z1 l' A5 `! k7 U; d
; t9 G  v, C. }$ R- e) r* {
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   0 A7 d- n- {$ W0 }
2. double CNY70Val = 1000;
   
3. int interror = 0;
   9 w' e7 Y0 n9 R( z& L% Y  l; u- c
4. int olderror = 0;
   8 |6 v0 q2 L6 p1 m( q6 F# Y) E$ @
5. double values;
   
6. 
   2 [6 x% Z, ~& f  |& x  n+ I
7. 
   
8. void CNY70()
   
9. {
   2 n! o7 L5 V$ W( H" t" u: d
10.   valuesRR = analogRead(RR)
    1 V% ^- ]0 ?' @/ I
11.   valuesMR = analogRead(MR);
    9 Y; O0 V+ t' \1 n' E
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    / G; c  Z/ }' c. Y5 v* T
16.   if (valuesRR > CNY70Val)
    . J8 F; K0 f* `
17.     valuesRR = CNY70Val;
    1 _  x2 [* L; T! A0 m6 e& q
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    1 M% k) p) X' b: c& f& x
22.   if (valuesML > CNY70Val)
    8 C* ~9 S1 D- a' Y4 D- G# S
23.     valuesML = CNY70Val;
    ' }: q5 F& @+ x# _+ G* w% L8 q% _5 p
24.   if (valuesLL > CNY70Val)
    # d3 q+ ~0 }# j: g6 S, x
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    " e8 g1 z- g$ B8 c0 d/ f) l, I
29. 
    9 {" t3 l. y% n& l) x
30. void Car()
    , I7 U! ?( V$ H. c
31. {
    
32.   while (1) {
    7 Z4 u( N* g5 v4 h9 h8 _  {
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    * t9 W: O$ ^0 f# {) [& P
36.     interror += error;
    
37.     int lasterror = error - olderror;
    & J- ^- b" E% \  p: ^; k# r
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    . M9 |* d; A# V
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    0 {' b+ k! C( K# Z- I
44.       power = -MotoSpeed;
    
45. 
    . _9 N8 a% K" ~) P2 v- \6 l
46.     if (power > 0)
    6 I- @3 N' j+ |# U; q. e
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    4 n% e% Y8 }3 |' @* K, L+ T
48.     else
    4 W. U1 E9 F$ l& A
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

6 ^+ `) o% x6 S+ [1 D. }' ^- M

感謝分享

感謝分享