PID 循跡自走車範例

1. #include <Arduino.h>
   - ~: i. L# }/ R! n. u/ g2 Y
2. #include <Wire.h>
   
3. #include <Servo.h>
   2 X3 H4 f* w, M# T8 I
4. 
   
5. #include "MePort.h"
   6 d$ r1 Q. J9 Q8 B& w0 D
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
     _" l% ~$ v  P$ B$ g8 i
8. 
   5 `4 H4 m! \2 a
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    $ D) J& q, Q$ t2 g
12. float MinPower = 120;
    # ?4 Q, }9 v; P6 ~7 w* p2 S# s
13. float Error,ErrorAcc,ErrorDec;
    " [! ?, T8 N* d# g5 E3 S' c- t5 S
14. 
    ; ]4 R+ @9 d" Y6 i% P, S
15. float Kp=0.14;
    - n6 r* Q  N8 Z: s- U9 _  u9 T
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    % u0 v$ `4 o6 ^9 h" G8 _
20. MePort lightsensor_6(6);
      \8 x7 N8 u$ J$ z' G
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    9 P# w, Q, o+ n, C* ?; C  U9 p+ c& y
34. void loop(){
    + T$ B3 u( B, W3 b3 }; O
35.   unsigned long StartTime = millis();
    6 z; Q! I1 `6 \& z
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    6 B3 X4 s3 ^/ F7 A& a8 X) Y/ G
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    0 c8 B0 d8 I$ C$ a2 e: G
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    , n8 w$ Z# |0 i% o& j  P
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    $ ]* y1 W0 D, N6 W7 O
43.         nPower += 3;
    
44.       }
    
45.     } else{
    & S5 X/ V6 S; W# e1 Q3 g
46.       if(nPower > MinPower){
    , o4 E1 G7 H! `9 c  z
47.         nPower -= 2;
    
48.       }
    
49.     }
    2 _6 S( q% `4 S* d, K
50.     MotoL(nPower-Error);
    0 }. u! ?/ R7 `7 h
51.     MotoR(nPower+Error);   
    , x! X! E# R2 N9 Z! ?6 f
52.   }else{
    2 Z$ U: c0 J" A. A$ [' y
53.     motor_9.run(0);
    9 }2 P, R  T( V
54.     motor_10.run(0);
    
55.   }
    * F3 [( ?# ]: U2 x# ~
56.   do{}while(millis() - StartTime < interval);
    . W" s! T, X" ]1 d+ u) _
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    2 O, ]: R0 V0 q6 d9 P7 y  A
64.      Power = -MaxSpeed;
    
65.   }
    0 G4 m& V8 C, @6 v* X1 k
66.   motor_9.run(Power);
    ) J. M2 m& p1 f' G' [/ T) [. `- J7 |
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    3 E9 @9 g3 u# _* {" K
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    8 Y3 L7 e' R5 ^- v- m
75.   }
    
76.   motor_10.run(Power);
    8 ^/ E4 W3 j% ^" y  V; K# b4 }% q
77. }  

複製代碼

9 A' c7 [% L% |7 a5 X8 T$ @% t

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
- A; O# y; y' a; s  C就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
+ `0 [9 s9 O6 [8 S% p, p  v不知道您是否願意教我可以如何處理


2 F* u0 M6 G: ~' v- w7 k以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ' L0 c: B/ b; z
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   2 P$ T5 d& w/ \6 r
5. double values;
   ' B) j" |' N) e- f+ R& w
6. 
   , m) y9 f* l$ \9 |/ I, ^1 T1 o- ^" ^
7. 
   
8. void CNY70()
   7 v' f# j$ \# p2 f( r! T% h
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    9 f* q9 d8 n' k6 I1 I) i
12.   valuesMM = analogRead(MM);
    * h5 v3 R. p) w3 J# Z3 y! N
13.   valuesML = analogRead(ML);
    7 g! N' @4 _! o8 Z! \5 N  m
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    . b- U& x* Y% m1 L2 _6 E
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    % N( \7 e- j9 o  w9 Q# c; t
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    4 u, ~) R- H  ^" M+ o
26. 
    7 W' N: o" h7 ]7 h2 d* C% T: z
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    " x/ i# k7 I3 e. m1 V7 Y' ]
30. void Car()
    1 ?* w- R1 N" o1 k. N+ @- b
31. {
    5 m; ~2 \) x2 O+ M7 O  h
32.   while (1) {
    ! |1 P) R1 B; T( l
33.     CNY70();
    1 Z1 e3 Q! K0 @% R8 X8 D, I  N' S
34. 
    
35.     int error = ((int)values);
    3 S, q- T' d$ D# r) B1 [
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    $ Q, G; P# K9 Q; `! b% K; u' a
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    2 D1 s" Y. n  r9 i# _
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    7 Q: q4 X- N: n: T: r* s. s: w
50.   }
    6 }2 U" Z0 k; W
51. }

複製代碼

感謝分享

感謝分享