PID 循跡自走車範例

1. #include <Arduino.h>
   + U: u. b0 R& l$ H. H* o$ }: Z
2. #include <Wire.h>
   3 O7 K: p: i$ }2 m6 ^% u  q
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   . {5 _9 W) D: x
6. #include "MeUltrasonic.h"
     `5 l+ C9 b4 k) b( H% [  d2 ^
7. #include "MeDCMotor.h"
   7 x) k' Z0 h6 L% l
8. 
   . e7 C/ }& \9 i% y/ e
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ; _3 u/ E: R: I3 l2 c
12. float MinPower = 120;
    7 s, Y: r& O2 W
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    1 C% G& y5 p# i4 D# r
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    5 l3 o4 f9 b' Y4 Z6 o0 e
18. 
    - T3 M; }) N" B4 ?4 k) `$ G
19. float nPower;
    , O7 I* C/ E; g$ h; x- ~: R
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ' ^+ R; P- Y" v# [4 \( C
22. MeDCMotor motor_9(9);
    3 K# j/ T* L9 m" O7 Z! {- {# X1 @
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    * d3 K9 k2 t0 B/ o; }5 {
26. 
    
27. void setup(){
    & l+ ?& i- Z( `/ x3 `" b
28.     lightsensor_6.dWrite1(1);
    % S; i" e: I2 x9 Z  _
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    ( M& Q2 E( v) [6 K( n
34. void loop(){
    : M# q+ b, t( ?2 h
35.   unsigned long StartTime = millis();
    ; S% F7 t' p' ^7 u. ^, m5 x2 o; S
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & J0 V# v2 W0 x6 m1 M
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    % ~. \& C: h2 j1 ^. B* y( h7 k8 z
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ( m( n* Z. y+ Q
41.     if(nError < 80 && nError > -80){
    1 ^3 m5 O7 N5 g/ M. P3 v7 {
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    2 v, \5 ]% H$ S  S. Q+ |+ Q
44.       }
    " Y* l- u( ^% ?4 K1 a
45.     } else{
    ) {/ C3 U4 [# U# g* v5 V, P
46.       if(nPower > MinPower){
      \9 g  Q) j+ |9 t$ O# S. ^! a5 q
47.         nPower -= 2;
    ! j5 M7 o; J. j
48.       }
    
49.     }
    3 [% H! Z$ `2 \" [! o) D) {) C
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    : @1 S7 V# I9 [9 c& Y( Q
52.   }else{
    
53.     motor_9.run(0);
    3 Z* `+ f/ G) f
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    $ g2 m" ~( D- D  H; ^) W8 l
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    , \1 F2 a8 {% I$ r! ?, D0 e0 c: E
66.   motor_9.run(Power);
    
67. }  
    $ r; T) `; g& N3 n
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    7 A  L0 l$ x* f6 m
71.      Power = MaxSpeed;
    + p# I+ l- w- c9 ~( A8 F3 h( n
72.   }
    / M' z$ g4 u4 P# j8 P
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    ) j; m2 T7 x  g, {7 S
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

% _3 C9 H) e6 Z: U' C
- W2 ~! p" E' l- z5 \您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
% Z' _: b' g+ A3 {/ m雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
2 o) L$ `+ F4 I7 E# h* C就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
5 e8 l! {( I2 }不知道您是否願意教我可以如何處理


" s+ d1 @& X* \/ @3 Y6 a) {以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   2 g) b( W0 X. r5 S6 d
2. double CNY70Val = 1000;
   
3. int interror = 0;
   8 j( z2 I( ?& p
4. int olderror = 0;
   
5. double values;
   
6. 
   / F; ]+ G3 V' V, ]+ Z' x
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    % P3 y2 T! d  G; |: Q/ r
11.   valuesMR = analogRead(MR);
    4 {; B) o; G" D  ]9 s
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    / V/ }. X  E( |. Q4 }
16.   if (valuesRR > CNY70Val)
    ; J! ?" ]$ Q* O' X
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    8 j  z( H- K4 Y7 N0 S
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    + N+ k/ z' F" t' Z% U4 X# ]
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    4 U7 t5 [& n* P  z
28. }
    7 ?' I% O1 s: A4 }
29. 
    , J+ `  Y3 M2 p% @% d3 ~; B( W
30. void Car()
    
31. {
    9 @  J, _2 h$ y1 F0 V
32.   while (1) {
    ' e* h0 `0 n  t: b% N
33.     CNY70();
    . @6 [3 I; _# q9 T/ r! P( D5 s
34. 
    
35.     int error = ((int)values);
    + I: t5 X6 p0 u3 O
36.     interror += error;
    * \( h4 V5 ^7 f5 P+ r/ K
37.     int lasterror = error - olderror;
    + \# C$ k. [- T; c2 b, a
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    , [! D' U. i8 I4 u5 w
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    5 w* F0 f) z5 Q" \3 {6 t
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

8 K! l0 `3 \  C) `% x

感謝分享

感謝分享