PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   4 Z7 N4 T" g3 R4 ^! }1 x5 r
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
      |: t8 H! ^2 Z/ J8 \$ e9 Y
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ! b0 R  R8 W8 O
14. 
    : p7 m# \' [) r- G( D2 ^
15. float Kp=0.14;
    $ J6 E$ n/ |& o/ Z/ S2 A
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    5 Z" I1 X6 I9 F; V
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    0 w2 ~, n' [1 k/ n) O
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    " g: \0 h( g4 b7 v, ?3 E
25. const long interval = 1;
    5 l: f5 F5 a$ @( T: y9 P9 c: Q
26. 
    # ~1 i, {! d1 |+ O- i
27. void setup(){
    / z, G% V$ @5 U( f8 O3 X
28.     lightsensor_6.dWrite1(1);
    & R+ x4 n8 ~$ N- Q6 H- d, ]( V
29.     nPower = 160;
    * [1 G0 I3 F# ^
30.     Error=0;
    
31.     ErrorAcc=0;
    , E0 |; i6 h# C/ l& [5 E* S
32. }
    
33. 
    - w, {) G2 w" R2 W+ N
34. void loop(){
    
35.   unsigned long StartTime = millis();
    # G" m+ K7 ^% N. v  {3 y; u0 S
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    8 ]5 r( E1 f6 g& ]/ A$ J
38.     ErrorAcc +=  nError*Kd ;
    & P! Z# U( X* X  g
39.     ErrorDec -=  nError*Ki ;
    / i. Z' y$ f0 l8 @  s2 Y/ C
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    - h* x1 h- r2 M+ p% W) ^
42.       if(nPower < MaxPower){
    + w" G2 e" R7 O- g5 f
43.         nPower += 3;
    
44.       }
    
45.     } else{
    ; J( W3 K# K+ h- ~
46.       if(nPower > MinPower){
    6 U6 G, r: K+ p, P! D9 V
47.         nPower -= 2;
    
48.       }
    . o1 l3 c5 Y, q: R8 |- [
49.     }
    , v  o+ U, w9 h1 e
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    % E) a( V0 x  g1 L/ T4 j
52.   }else{
    5 `6 N- {; |' s: e+ e" [2 t
53.     motor_9.run(0);
    / w, @# }9 h' w0 I% R
54.     motor_10.run(0);
    $ i3 E3 G( t: K' I2 S" O' f# v& k& e
55.   }
      Z8 ?8 P9 X2 R% b+ c; I
56.   do{}while(millis() - StartTime < interval);
    4 B% x( E" Z- A
57. }
    / Y& e0 {0 D. Z) O3 L; b
58. 
    
59. void MotoL(int Power){
    ( y4 [, W; f- x( O
60.   if (Power > MaxSpeed){
    + z; E3 A8 H1 p) Q
61.      Power = MaxSpeed;
      g0 N- t: {  E5 Z, l; t1 a% m
62.   }
    
63.   if (Power < -MaxSpeed){
    . A' d, c; z/ u  R
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    8 m8 t6 j  e, [" ?8 q" F' S4 t. D
67. }  
    
68. 
    
69. void MotoR(int Power){
    0 R/ N) U, h# L# ?' ]: `1 s  `
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    6 u& H) t1 C& y7 U
75.   }
    " r( f' V0 p1 P
76.   motor_10.run(Power);
    ( A1 T9 Q/ G  l" u9 l% v( h
77. }  

複製代碼

6 u  g/ u) u& t. n" w6 W5 A

- X1 h3 ?1 u8 L# I. D您好，不知是否能向您請教。
7 {, P6 V, w: b: B目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
% b& x* B$ U+ Z: n( V就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

+ ~/ e6 z5 D2 `
+ W" D7 _% e6 H( a; P4 r以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   # a; F1 N% N2 t8 B) ?7 X
4. int olderror = 0;
   
5. double values;
   * K( F$ ~% ], V% F
6. 
   
7. 
   5 v. O$ Z( _  H* C1 e0 A
8. void CNY70()
   
9. {
   & N4 f# L; n9 O3 t+ \5 @
10.   valuesRR = analogRead(RR)
    9 o# {% Z( R/ q% n
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    " t8 w  Q" ]( |/ Z6 ~) q5 B
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    3 o7 V3 F% K: z$ g/ Y
15. 
    
16.   if (valuesRR > CNY70Val)
    1 v  W( w4 m: k% v
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    7 u5 c& t0 \. |
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    . S9 ?, m& Q3 |
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    . d! l; s  u6 U. l% ]8 }
26. 
    ( A) d' e3 x' H8 H+ j
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    / z  ?4 d( \2 [' w. F; U
28. }
    ) P/ b* j4 Y5 A1 C# S
29. 
    , L! @% D0 @1 j5 l% Q
30. void Car()
    5 B- b+ W' @1 t& |0 y
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    ) z+ v, G# ^3 E! {$ I, o. {' r
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    4 q% K9 a" f; H9 m4 ^
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    2 r9 R7 h0 {: `' G
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    # j- M7 g; {8 o- b: X# a8 f/ M
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    ( U6 s. G- v9 y0 M
51. }

複製代碼

/ d1 `) ^8 I( ~. y" J
  L1 A4 D" a! A! \6 ?' M

感謝分享

感謝分享