PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   3 f9 N; Z% ?) Q9 ]) P9 Z8 v, m* I2 A& D
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   0 t9 f8 w% O5 P9 z
7. #include "MeDCMotor.h"
   5 j& a3 H: e9 v- v! ~5 B
8. 
     a* v  w7 N) N9 G
9. //double Input, Output ;
   8 t4 t( H( L7 }6 w$ D1 O
10. float MaxSpeed = 255;
    & [# M+ A) b( r5 h- p
11. float MaxPower = 180;
    ' W5 P$ {# x* D# L& W8 o
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    $ e6 V) W( C% N6 Q; G$ }
14. 
    
15. float Kp=0.14;
    % {$ c- @+ v. O- U& \8 r
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ; h- B) b1 s3 G/ k+ M( t% K0 Z+ x
18. 
    , |! c7 Q* n. ^3 F! T
19. float nPower;
    0 n1 I% l' W( [! p
20. MePort lightsensor_6(6);
    ( x/ j5 M: @: C1 c; \0 h
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
      _0 ~, S( {; L9 Q3 U8 I8 {& K1 ?
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    & l6 I7 ]% Z. ~5 X& @: N& {6 ~
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    ' [$ Q% B- v8 J* A
29.     nPower = 160;
    
30.     Error=0;
    2 f: `& n$ ]' s9 m7 X$ E. A
31.     ErrorAcc=0;
    ; L# l0 d" \# b4 \& P
32. }
    
33. 
    ( Z5 W6 T8 ^) B+ f0 `& o% t/ ?
34. void loop(){
    
35.   unsigned long StartTime = millis();
    4 v5 m+ K3 V0 ?8 C: M! O- z; I6 |% M
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
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){
    3 x! T2 S8 g* }; z& N
42.       if(nPower < MaxPower){
    2 Z! [" y1 m' L0 S+ _& t
43.         nPower += 3;
    
44.       }
    4 w) b* f) q; n: K- v
45.     } else{
    3 |# f1 C/ N9 u9 ^7 Q; v# a
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    ' n" e) g3 f0 c' A) ?
52.   }else{
    
53.     motor_9.run(0);
    * K" D5 D7 Q) P4 X; G; b6 }: I
54.     motor_10.run(0);
    # T/ a" f0 K" H5 t
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    & y2 m3 w/ t# S0 j$ d
59. void MotoL(int Power){
    . A2 v% v; w) R
60.   if (Power > MaxSpeed){
    ! m) w8 g$ @( f& c9 o  ^1 h- r
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    8 {6 L8 x' V# J6 u$ N: ?- `) d
65.   }
    * g" ^2 H) m7 T0 X3 i
66.   motor_9.run(Power);
    + ~* f; j7 ]6 O: z! M  N  U
67. }  
    8 z# k' S/ W, B9 v' F
68. 
    - ~1 j! e5 W3 w+ J3 n
69. void MotoR(int Power){
    " C3 t4 f0 F# M5 ^0 @" ^8 a
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    ( [# ^9 Q) n1 x- c! }  i( ^
73.   if (Power < -MaxSpeed){
    * _$ y  K  D& i3 K8 h0 X! K( m
74.      Power = -MaxSpeed;
    
75.   }
    4 {" D  G0 P2 }
76.   motor_10.run(Power);
    0 ]0 G( Y; A  Q% h  u6 k
77. }  

複製代碼

1 ~4 q1 n& w, K0 z( @

9 T% H5 u3 z/ \) k
6 B! P( B' `; F# g您好，不知是否能向您請教。
; H' K! C+ I* V' Z' v) j) }' B目前和宋修賢老師在處理Ardui Car
2 B2 H5 D# Q3 {: i+ }7 m2 ~雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 d6 m8 s  p6 _
# j4 R: l0 G! s- z' R但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
5 a% @# h" Y( k不知道您是否願意教我可以如何處理
* _+ l1 z( F" N9 r' w
# s* b( C0 i4 ?1 [& v
4 }* b% c$ H1 q$ C* q: q( x6 g以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   3 v4 K& N$ }# \% N& _+ R
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   3 ^, {9 q, y/ y& a- ^4 g
6. 
   / W6 k: P" S( ?/ M4 i; Z2 n
7. 
     C4 u4 x$ j; v
8. void CNY70()
   $ G) @7 U5 t4 T, n- g. E8 K
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    4 |0 x& Y6 b! G3 @- V/ T. c
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    4 i1 I% B9 L; |- z9 o6 g, G! f
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    * t* H$ j4 F; a( p1 s1 N  o6 G4 k
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    4 L7 k( E3 l3 d6 f3 v% B/ _& M8 ~
21.     valuesMM = CNY70Val;
    " P  |8 H% [' c, y0 b6 h
22.   if (valuesML > CNY70Val)
    / V2 K' j- A. D2 g6 V1 j9 Q
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ; K+ v% o4 t2 c, E& n/ E
25.     valuesLL = CNY70Val;
    
26. 
    # c: o1 N: C6 d( U1 p' ~+ g. p
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    - A' j9 _) T2 Y/ ]1 U
28. }
    8 P( c) }' k( {3 y' Q
29. 
    - @4 M4 a' H( p0 ?  n
30. void Car()
    % |4 p. G6 ~( y3 g3 }
31. {
    
32.   while (1) {
    
33.     CNY70();
    " L1 y( \9 z% }9 I- }/ I% l
34. 
    
35.     int error = ((int)values);
      T9 ^. u* z1 d3 q! V* ]! w6 ~. E
36.     interror += error;
    4 C! l. d" ~( O0 p: `; L+ q
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    - o. `! Q+ i9 V% L' x
39.     int power = error / 5 + interror / 10000 + lasterror;
    % K6 n$ m/ D  \, d
40. 
    
41.     if (power > MotoSpeed)
    ( M7 v& I7 T$ s7 A/ T/ M! R. t
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ! u/ F2 ^0 X, V; S  b- o! z2 b, P
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    ' B- g$ Z" `, }5 b; H( O) |
51. }

複製代碼

2 Y; \5 _! p+ c

感謝分享

感謝分享