PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   * q4 d$ J& o/ s, T3 k( W
3. #include <Servo.h>
     h+ {0 l+ a! a2 T
4. 
   " `9 o  Y6 s; h2 c3 ^
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   ; _+ ?' [& S+ _" _" t  T
9. //double Input, Output ;
     v4 D) o5 N- y, l% c; S5 [( l
10. float MaxSpeed = 255;
    . m9 s# Q2 D+ Y
11. float MaxPower = 180;
    * Z1 x) [( c, f2 u1 v
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    " }" [6 ^3 u3 }, R: n5 j
14. 
    2 j1 q( O6 m% e
15. float Kp=0.14;
    & W6 T' J5 g  B$ k3 K
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    0 v( ?" H/ M5 \" b% w- Q* t8 {; b
18. 
    
19. float nPower;
    7 a' j/ L* E$ b* ~
20. MePort lightsensor_6(6);
    * h3 p; ]: f, t/ p/ a. u4 s
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    : J4 _# o+ G) z0 T$ ?" V
24. unsigned long previousMillis = 0;
    9 }2 O2 P3 m. J% I4 h
25. const long interval = 1;
    
26. 
    ) `: Y% B5 d3 l$ g
27. void setup(){
    5 o( n( D# e9 c+ W0 v
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    + h# r; R1 g# t+ V( A6 G% e
32. }
    
33. 
    8 j- ~" U- k+ X
34. void loop(){
    9 E1 D  O2 Y/ F% k
35.   unsigned long StartTime = millis();
    . \/ o1 B! ~, z# W( t' l
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    . B4 h3 H! u& \" L( ^
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    * W  @& Z# b& ]# P5 p
41.     if(nError < 80 && nError > -80){
    0 q6 \" U) O) G4 j- Z5 z: Q: ]
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    8 q9 H, b4 r% m( T/ ]
44.       }
    # B8 z6 f9 ]- V# V9 C# T
45.     } else{
    
46.       if(nPower > MinPower){
    / T8 f+ t5 o0 }  |8 e  b# K1 K
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    7 V* J" Y& v2 L, j' Z  `( R  f
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    3 i- o% b# V: }- V7 c
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    ! x8 }' T7 V/ g$ x2 A# ~) X
58. 
    $ P  t0 K: }, ?2 D1 `/ X
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    . a" L% o4 V! W1 }" ?3 \
61.      Power = MaxSpeed;
    
62.   }
    6 S/ A5 A" k* ~2 V7 A
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    % _( T0 \- U) E& Y
66.   motor_9.run(Power);
    0 z* d/ r7 d+ I6 ~
67. }  
    
68. 
    
69. void MotoR(int Power){
    ( E1 p$ w2 S0 T7 s9 m- {- o2 m* o8 i
70.   if (Power > MaxSpeed){
    4 A: s" {3 G* c7 h% L4 h
71.      Power = MaxSpeed;
    
72.   }
    , {/ ~0 V2 L: |- N, }: E5 a9 i
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

1 C3 s9 {/ R7 _1 x" _- N
: I- n& J, w. [

& l* I$ y% o& r. ^( c" A+ D
7 E5 G! g3 C* t0 g/ v" p, |您好，不知是否能向您請教。
( K! Z- ~5 |9 B. V$ v目前和宋修賢老師在處理Ardui Car
- m( ?7 M) o. R雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
  o) v6 q- N; z; }) V, P8 P不知道您是否願意教我可以如何處理
" ~. ]+ [: ]5 i7 P
: a, Y* S& B' z+ I1 l0 j: V
1 Q( n. {  `! w: g& r( [! l以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   3 K. G; k" I' x1 d! G) }1 o5 X
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   5 G7 G/ H: w0 F  }+ w/ L: {
5. double values;
   
6. 
   ) L5 W& W6 Q2 N' n
7. 
   
8. void CNY70()
   + [: y1 n, f: d9 e
9. {
   
10.   valuesRR = analogRead(RR)
    3 f; O- d& i; S2 O) Z
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    " I! `8 ^/ u5 g! _$ A& c3 D
14.   valuesLL = analogRead(LL);
    
15. 
    ( t$ D; H$ U  }: ^& G' g* l
16.   if (valuesRR > CNY70Val)
    9 r% J9 N' |7 {: W2 x2 @
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    + `) B* ^& C" R2 A1 c( K3 M) }
21.     valuesMM = CNY70Val;
    7 \; p! s: V0 z* \' b8 k; W2 Q* y
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    - O2 {5 d1 x% B7 F8 ?+ h9 u" ~
24.   if (valuesLL > CNY70Val)
    9 N7 H6 O2 r+ n) F) M. P
25.     valuesLL = CNY70Val;
    ' `! v# P) c4 c  V7 J; _9 i
26. 
    # I- f0 M. |- m: W  ^
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 ]9 N, X5 h# w
28. }
    
29. 
    " c$ Q  D' M# K9 p, p9 F# x- P) C
30. void Car()
    , N2 i  Y- ~5 v
31. {
    
32.   while (1) {
    . v0 ~9 T% l9 j1 ^0 q
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    8 k6 o* D; ?. r3 n6 z& x: ~2 Z
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    , x, Q( b9 |& H1 w4 C; P
40. 
    0 q6 A# {2 u& {; ]7 T6 n
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    0 V2 E  M/ L& u; b
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    5 a( i, V8 d, X9 S9 B" w
45. 
    . b9 Y; E+ }- u+ q$ r2 F. g+ G
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ) h8 D6 u) t( {* S9 h+ R+ x7 B9 d
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

6 _9 X/ e# D  B( j7 Y

感謝分享

感謝分享