PID 循跡自走車範例

1. #include <Arduino.h>
   . O6 d. z6 o" T! E" a
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   6 {: @# e* ~) e1 y1 H$ b' O
5. #include "MePort.h"
   & z3 B! |, ]. V3 r# U' s. _$ F
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   ' C* F9 A  u! p7 X( q
9. //double Input, Output ;
   / d# Y+ k1 d; a- j
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    , r" [4 m, V/ ]8 ?2 k
12. float MinPower = 120;
    5 V) x: ~6 U. b7 U
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ; B* M; m, y) y2 V' [- B6 J  C% s
15. float Kp=0.14;
    ; D5 P$ N, l, A
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    8 O" m3 C  }) A! W  l- @
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    9 |2 F6 o0 |) F' L, M( X# C
22. MeDCMotor motor_9(9);
    5 N2 A+ A3 w4 d' ~! m# c
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    ! I/ d0 k/ v* _: s  k1 l9 `
25. const long interval = 1;
    
26. 
    2 }6 v( ~/ X, E, Q
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    , b3 P' n  R7 b* o; J9 n, M9 \5 c
29.     nPower = 160;
    
30.     Error=0;
    : R) t+ k) ]8 P6 \
31.     ErrorAcc=0;
    + m) @" Z2 i9 t5 X
32. }
    
33. 
    4 v0 h  U. l' f. x! u
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 ;
    1 I9 a9 x2 O+ R6 Q- r6 {
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    3 u4 `- W4 z7 I, d2 K& d
41.     if(nError < 80 && nError > -80){
    ! G* U3 ?9 \" o8 X) d
42.       if(nPower < MaxPower){
    . v+ L: b4 P' M; p* M) \7 V
43.         nPower += 3;
    
44.       }
    
45.     } else{
    , J8 b- F& K- Z) a& z3 S
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    9 p, W* T7 u# a' `1 h/ G0 g
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    9 E2 }1 N7 Q1 u% O
54.     motor_10.run(0);
    1 b- [! L0 A" H8 q
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    5 ?$ }6 L& ]7 O) H* D
58. 
    % }3 N: S# n, P, L% X0 R4 ^
59. void MotoL(int Power){
    8 Z; m, c& e0 D2 a
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    " I% v! _0 l- g1 @
62.   }
    , l# ?  {& B5 B$ i7 G4 J# _. P6 `& X
63.   if (Power < -MaxSpeed){
    , N) k1 G7 q( h1 \: g8 n
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    ; e4 }& V+ H! \, k8 Y
72.   }
    
73.   if (Power < -MaxSpeed){
    2 x& T: l" ^6 d& K
74.      Power = -MaxSpeed;
    
75.   }
    7 p* g; C( Q6 t. v
76.   motor_10.run(Power);
    $ A% A7 s" ]4 w6 n) _
77. }  

複製代碼

3 T2 X+ i3 S2 X5 p
, R% J4 E+ @; k

0 w% i" A# P2 ^% s# B
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
1 r$ ^; j0 ~6 V9 \
但基於想追求更精簡的程式所以還是想請問一下
- ~6 ~, ]( D/ A就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
6 H4 f/ H' L, i! z) v6 F# U7 ]
, X2 l6 y$ ?$ S& }# |
  `" j1 c* R4 C( Y; F  X! y以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   ; j+ S4 Z7 j4 M8 r8 l: C! L/ ^
3. int interror = 0;
   
4. int olderror = 0;
   + |9 V+ X. N) o7 H8 S: ?
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    / K; z4 ]9 V) y
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    : [0 D1 f  K8 H# \7 J1 H, M
20.   if (valuesMM > CNY70Val)
    * s3 o2 X6 \3 d  a* Q
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    + b. h# h$ d% x, y6 o1 |, v- G; T
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ; Q  M- K7 ?6 r! }, x2 A: j: Z
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 p* l9 C% G7 T* @6 z; {
28. }
    
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    / r+ y$ s! ~9 `* ~2 y
34. 
    
35.     int error = ((int)values);
    ) @4 D# A7 X( ^7 j1 }
36.     interror += error;
    
37.     int lasterror = error - olderror;
    - W) I, _6 [3 k5 b; j9 k- q
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    4 Z( e* ~& _3 `9 \1 C; u6 ]5 m
40. 
    3 \+ `1 S* m6 J: d
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    2 Q/ k* j& I8 A& l' b* m
43.     if (power < -MotoSpeed)
    5 |# j7 f, w! e2 c; n
44.       power = -MotoSpeed;
    : W: [5 E9 d/ \5 h/ s! @6 D) _
45. 
    : x% B1 b0 t4 G: \( u. J
46.     if (power > 0)
    : ?. Z. h: D+ P7 Q. N$ R3 G6 d
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ; H1 u0 \: G; {( }/ J; K) N* E
48.     else
    / E6 K7 g' c5 `$ \! v- f% J4 U
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 Z( O* F2 z! k# w$ x8 Z
50.   }
    
51. }

複製代碼

- {, C' a; v8 S

感謝分享

感謝分享