PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   0 z  V2 I2 _- o# d- M4 d( b& L  N
3. #include <Servo.h>
   
4. 
   0 _6 N1 V( S' o8 _
5. #include "MePort.h"
   6 D5 r' I/ h5 y4 n* \( h
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   : ?# l" r9 D, f! N1 I( t  `  I
9. //double Input, Output ;
   ( @% u; }) K9 S1 K& \
10. float MaxSpeed = 255;
    1 B7 a& T- L" D. @* S# M6 k0 o
11. float MaxPower = 180;
    # T: q' S7 y) J- ^6 k8 c# l
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ) j! X8 b/ z1 j; x0 r  S% R
15. float Kp=0.14;
    7 k5 b% |- u# w7 o
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ; ?) G# s# T5 |
18. 
    1 s  D) }2 W. X( V
19. float nPower;
    1 Z  z, S0 W0 g+ N
20. MePort lightsensor_6(6);
    , }0 |" l$ H; D7 T4 S1 Y
21. MePort lightsensor_8(8);
    2 I- F. z& v/ H
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    8 {1 {9 g, f( S2 J+ X
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    ' f7 C! d5 l0 [! n" k
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    " B$ i* H( z% `8 n2 X
34. void loop(){
    
35.   unsigned long StartTime = millis();
    & x5 n5 x. }6 ~
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    2 {5 x& r! e! E
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    ! F1 C1 e, L7 ?7 l1 e1 c6 K
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      r3 \: E7 |. ?
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    3 \  j8 ~1 V+ L
44.       }
    : M* T4 A1 q( k& L8 S
45.     } else{
    . H7 @4 S2 n4 E# B: [1 W* ^
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    # V1 i9 L6 W& J1 _/ \$ F
52.   }else{
    1 J+ P6 Z* Z8 H' u6 l
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    4 S: I* L: @' d0 z
56.   do{}while(millis() - StartTime < interval);
    ) O( B/ r4 I0 n8 ~
57. }
    $ V% j3 w( r, @
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    ; G8 l6 Y% u- y
61.      Power = MaxSpeed;
    1 H* ?* v8 [. ?
62.   }
    - E' v& Q- l# B$ |
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ' \! r. [% |6 ]8 x4 t; z, n, b
65.   }
    3 Y' n* f- W6 N, r) M+ D
66.   motor_9.run(Power);
    
67. }  
    
68. 
    : b& d) u; E' @+ g" a) @! g) [
69. void MotoR(int Power){
    # `( H7 ?. k& E; g( U# i6 `
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    # `; ^" o! Q/ w" V
74.      Power = -MaxSpeed;
    # Y# W' X& @# y  s
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

5 g2 c8 P/ C* h& r9 H; P
% `2 y* k  b5 e# M) V

  m* F" b+ g+ v0 b* \4 h$ f6 Q/ M2 Z
您好，不知是否能向您請教。
. l. w6 ?$ e7 U* ?目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
- B, E3 z+ F& M1 t* r
8 \! h& ]+ h. y( |, a6 t但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
* K/ u( T6 a0 t& K2 u

, T5 b5 G( O, |& \* e1 p% ^; F: b: u以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   " A9 [9 B" h3 W! b6 S( ]( j
3. int interror = 0;
   ! T6 ]1 o+ S2 I8 t. }  B+ [6 _
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
     I0 }0 y5 P( O$ f: S/ W
9. {
   ! [3 ?2 s/ f2 Z" y. {7 e6 T
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    . Z! w) P0 X& B+ E& g! }: V
14.   valuesLL = analogRead(LL);
    
15. 
    * [- p7 l8 s; d2 @$ H5 `5 P" V
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    / d& {5 _& Q2 \! a
18.   if (valuesMR > CNY70Val)
    ) Q; m7 V8 W6 p7 b) t3 `( m& u, p
19.     valuesMR = CNY70Val;
    ) j9 H8 Q& X1 d0 R! K9 P8 f
20.   if (valuesMM > CNY70Val)
    8 p2 v+ {' g; c( z
21.     valuesMM = CNY70Val;
    ; B3 ^* `- _% f0 |4 a
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    * E% z3 |- U+ ?6 Y* z7 L0 V$ f
25.     valuesLL = CNY70Val;
    ) h$ S: u: B4 B8 ~* q
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    4 H( O4 A5 S) C( S" B. k8 l6 Q1 c$ A
28. }
    
29. 
      G$ J( k: v- C2 C; Y0 d
30. void Car()
    
31. {
    0 [' R: l: j  \0 G; C
32.   while (1) {
    
33.     CNY70();
    . K  E1 E0 n) _. B7 R2 G! o
34. 
    * \. V( i+ o$ l1 [$ k
35.     int error = ((int)values);
    
36.     interror += error;
    / P6 R5 x4 U  V6 F+ C0 x
37.     int lasterror = error - olderror;
    ; K1 X! n) T6 s* r1 t
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    8 n3 `' P) z- Y1 Y
40. 
    + L+ v0 ?  L: E! U3 p$ M& O' }9 j! T
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    8 X+ Z* |/ o* v) ~: N# c5 N
43.     if (power < -MotoSpeed)
    ; _) l  h- b1 |0 e8 O* [' |
44.       power = -MotoSpeed;
      v6 F1 F% t& L
45. 
    5 T2 u" N1 c: y- H; m3 B7 D# k
46.     if (power > 0)
    4 O1 N. k5 N: d) x$ L$ q
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ; [5 y0 W, O; d8 F/ T7 J/ j, E
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    9 _1 a9 @  j* u' h* u, j
50.   }
    1 t% E8 j9 G. L) d4 G- G' `
51. }

複製代碼

# V) n2 V7 G. h2 \; l* Z

感謝分享

感謝分享