PID 循跡自走車範例

1. #include <Arduino.h>
   ; `2 U9 G4 O; N2 [
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   : y$ ~( |2 M3 M/ D* v6 J7 e+ Z
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   % Y  a5 y5 _- @# ~+ \- ]
7. #include "MeDCMotor.h"
   
8. 
   * Z8 S9 v, L- P' c1 I( `2 k
9. //double Input, Output ;
     {/ |- ~) }. `! W" `5 F
10. float MaxSpeed = 255;
    : X+ B/ w5 z5 l1 N
11. float MaxPower = 180;
    / e0 \1 C% h; g  z* H
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    " B( I7 s+ W6 Y  L& J+ ]
15. float Kp=0.14;
    ) J$ Y- q9 M! Z* Z  k1 S/ c
16. float Kd=0.00020;//23;
    " _9 ?7 e& e4 V: W* m7 Z
17. float Ki=0.000201;
    8 X5 |' L( y8 F, h& q: V
18. 
    $ m; R5 B' c- c5 j( H
19. float nPower;
    ' D3 e3 L# h& _. g* Y, ~
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    / G# m8 J& h" Y3 y' F; W; Y
23. MeDCMotor motor_10(10);
    2 k/ `2 g; f* Q# j0 j! n6 }- S
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    6 j1 w, V( Y; I/ v0 O
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    # R& R8 R, _6 i1 k7 N/ d* R
32. }
    ; s$ i3 J" r' [' ~; L
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    9 h  [+ G9 S, @& o" c
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    , I4 n4 O( z7 [; z8 @& F
43.         nPower += 3;
    ( R# i! _. L$ z0 \' S% Z3 e( B
44.       }
    0 x1 R3 z5 n1 \4 j( j7 P
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    2 D+ k7 B1 x" A2 Z( c) P6 I
50.     MotoL(nPower-Error);
    & ?" d, C! M7 K5 ?$ R; S/ W/ Y
51.     MotoR(nPower+Error);   
    7 R) q  `; t- ?, Q6 Y" [. v3 ]
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
      C6 g. b" U5 x
56.   do{}while(millis() - StartTime < interval);
    7 Q9 g/ F7 v/ A; I; i" L% M4 t: {
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    4 q+ s( m- s+ h
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    $ R4 ]: q7 E0 C! J* {5 d
66.   motor_9.run(Power);
    
67. }  
    
68. 
    0 H0 |% N" O# C& v$ I+ N5 k
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    2 Y& `) B9 {  D5 h9 o& A1 v
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    & W# y! A. f/ @# x" p4 \
75.   }
    ! i9 H! }+ W- @- M+ ]# |  ]( F5 N; ?( g
76.   motor_10.run(Power);
    1 o" H6 y: W0 \8 m; j! D1 k
77. }  

複製代碼

  {! I' k  E! w) f& t

4 y2 S, b; a, [* e
2 h4 k, r# ^  \7 f+ z/ W5 D4 t您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

) q& I5 V9 O' @/ J1 Q! w% p但基於想追求更精簡的程式所以還是想請問一下
! y6 P7 F: S+ h% F/ G5 n3 v就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

* H+ L+ E9 e8 k
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   & B- r3 O' s( p; R" Q# b2 u
3. int interror = 0;
   
4. int olderror = 0;
   % ^, Q( o' f$ C
5. double values;
   
6. 
   
7. 
   6 A8 ~3 f4 i0 r2 F) B+ `" C
8. void CNY70()
   
9. {
   6 e) J2 \9 ~: t- d- R
10.   valuesRR = analogRead(RR)
    / w7 _. I2 U8 c
11.   valuesMR = analogRead(MR);
    % N4 T1 P+ ~: d& C
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    & ?/ t; I8 T5 U( t* Z6 ^+ |
14.   valuesLL = analogRead(LL);
    0 s- V5 M( @* g, T" I, R
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ; u+ O$ N5 r$ \8 W! \3 @' l
20.   if (valuesMM > CNY70Val)
    ; _. O4 `0 X1 h/ Z! U+ T$ i
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ' U5 f6 P' g) \' x
25.     valuesLL = CNY70Val;
    1 S$ x  T- n% d1 A9 j# r2 @: v
26. 
    , Y8 V0 ?1 g/ S6 j) q
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    . U# G' o( L% ^8 U  s. N
28. }
    
29. 
    3 {8 i: Z' @! v5 ]
30. void Car()
    
31. {
    ; G# F: i9 c! Q
32.   while (1) {
    * L- U0 D! Y) P$ B) p6 S
33.     CNY70();
      R* B9 Y. G# E, p( `% }! X
34. 
    6 s1 w$ H( E- {- Y) e# J! E
35.     int error = ((int)values);
    # F  `" B% w5 X3 Y, ]; x
36.     interror += error;
    ' g8 ]% W& ~$ P' B! k# C6 M& x( Q
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    - n4 E8 d6 M, j# @( U% A
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    6 y* @+ q2 {- V# Z4 J+ p0 Q
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    , E4 O7 x+ a+ x5 D5 j" T
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    1 c( z& r- `# H1 e4 U- k6 O' e: A3 u; F
50.   }
    
51. }

複製代碼

" C0 g4 R" j  L. n( Z  E" j& n
+ Q: g- u. m3 I+ G( n

感謝分享

感謝分享