PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   . l# Q+ t6 ^0 m& g) \
4. 
   4 U* A- y4 L" ~" M. a, n3 i3 N! a
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   3 w$ R/ s) W& V5 D" U! [7 v
7. #include "MeDCMotor.h"
   
8. 
   ; {' r2 W% G/ ?- K2 u
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    5 s/ g* ?9 a- i. ?9 R
11. float MaxPower = 180;
    * M& S1 ~" z# N; F
12. float MinPower = 120;
    ( E0 u8 u, k2 Q& e
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    " X5 W) }" I9 _
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    ) g8 ]0 b# Y0 C2 g" G3 u
17. float Ki=0.000201;
    % k+ f, Z; N: G
18. 
    0 l+ C& M% u" L5 N" o, {
19. float nPower;
    
20. MePort lightsensor_6(6);
    9 }: {. L) B: a6 `/ t
21. MePort lightsensor_8(8);
    : z2 r' \- X: k0 @6 v7 z' |
22. MeDCMotor motor_9(9);
    " [+ X2 b/ v  M+ e
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    $ X& I5 H& c6 H* V" `% K
26. 
    ( F9 l! m4 N* U! n+ ?
27. void setup(){
    5 J8 _& G# _( N5 V# r5 k8 K
28.     lightsensor_6.dWrite1(1);
    4 q" |! I3 ^3 l
29.     nPower = 160;
    1 g" E+ Z" A$ T* y) E
30.     Error=0;
    ! ^0 o6 i9 N3 _
31.     ErrorAcc=0;
    
32. }
    
33. 
    ! S5 \+ E  R( [. U+ V* H' g$ Y
34. void loop(){
    # k' q+ s. }* ~/ u  {) @
35.   unsigned long StartTime = millis();
    8 E" ~0 f  w) Q* t0 x5 c7 [4 q
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    , X3 L# e( b$ K; E4 {
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){
    / w" m  H. c/ h9 e+ K) u
43.         nPower += 3;
    
44.       }
    ' [5 N% q  m. K2 D9 D6 K
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    # }5 |2 q. d5 b4 Y
49.     }
    . l. V' i  E, o2 J1 g
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    " }: |0 O: y! g! s  ]2 `4 x; s
53.     motor_9.run(0);
    % i3 X9 z; ^, r$ j2 M
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    . ^5 S+ g/ ]  q( M3 O8 N
59. void MotoL(int Power){
    " \. @; G" x* F4 j( T) g8 }
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    9 e6 \( J: E8 d- D) e  n4 k' A/ Z* R: h
63.   if (Power < -MaxSpeed){
    ; ^* u: M- W. @# B
64.      Power = -MaxSpeed;
    ' I  a9 f1 ]% K5 v/ U- M$ d
65.   }
    
66.   motor_9.run(Power);
    6 U. N" u, E6 b( E3 E
67. }  
    - a- u" f! ^! \* |2 K5 K7 @2 `
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ! Q4 k) Q9 c2 i" y5 i5 o& ~
71.      Power = MaxSpeed;
    
72.   }
    + l  O. s* w2 `+ N
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    : Z$ V$ D9 x# w, _% ]0 y& A
75.   }
    
76.   motor_10.run(Power);
    4 u4 K, E' c1 X/ a
77. }  

複製代碼

5 v& W3 Z4 W' b) }
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
# d" Q) k1 X& y! Y6 F雖然已使用較繁雜的方式處理了跑出黑線外的狀況
  r( V3 y7 v9 {+ P% w
! Q4 n9 r( t' j但基於想追求更精簡的程式所以還是想請問一下
+ [5 i4 f  Z; D" W( o) T就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
& [8 L0 \4 K( o" m! e$ |不知道您是否願意教我可以如何處理

3 Q) ?6 `) z1 ^  F8 V. }4 j
! u1 n4 s, r3 G% f/ T/ ]7 c以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   6 y3 `) Z$ Y( N/ M" Y
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   ' ~" x* q# W$ I# x
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    1 X+ F' T3 S+ B" C
13.   valuesML = analogRead(ML);
    0 k8 Y, ]$ a2 T+ G2 h
14.   valuesLL = analogRead(LL);
    / Y* F: ?* O; J+ C7 W- z# D# g: i
15. 
    
16.   if (valuesRR > CNY70Val)
    : o1 G. I1 i% a/ ]! d+ T: o
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    # h/ |. ^9 Q/ K9 s, p% A
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    8 G& Q7 C' ?: h4 B$ k
24.   if (valuesLL > CNY70Val)
    - t( W) z# q0 x- b
25.     valuesLL = CNY70Val;
    $ {3 X+ c8 t* w- u. p; |7 R
26. 
    9 O4 e: G# a  H5 c$ K! o4 O$ `6 e
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    . ~- j) m* }# P$ U5 p, C3 ~
29. 
    
30. void Car()
    ) G: V# A- M$ @% a0 H: j$ R
31. {
    ; c5 d9 _% P0 A# L3 H! t  B1 @. r
32.   while (1) {
    1 c+ l$ W3 o* y* \& A: p: a7 v
33.     CNY70();
    
34. 
    ' o; a! u0 k' F
35.     int error = ((int)values);
    . n& ~8 c1 ]1 m! b. t8 j
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
      o9 r( H: s/ E
39.     int power = error / 5 + interror / 10000 + lasterror;
    $ a* ~2 [: q- C8 h
40. 
    ) I, `, W3 I# m5 b2 U9 _( N9 y4 Q
41.     if (power > MotoSpeed)
    % t( f3 ~! k# o$ C$ Z' @' y
42.       power = MotoSpeed;
    ( d) i0 q0 I  f* W
43.     if (power < -MotoSpeed)
    " b$ W* x% \" u6 c
44.       power = -MotoSpeed;
    % A' j: q7 ?+ C, F( n4 {
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    8 M* L% O$ T  a" x8 X2 x- h1 g
48.     else
    8 z# @2 ~) e4 s2 i  O- ]% |
49.       Speed(MotoSpeed + power, MotoSpeed);
    5 g  Z% S% ~5 B8 K% y
50.   }
    3 S. |4 r$ W; A5 c
51. }

複製代碼

- C  r( n8 p$ G  Q1 Y5 ?, A: b
, M# N- r  `0 B0 o3 ]4 w( E8 x* a

感謝分享

感謝分享