PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   ! R+ t9 o# i* c( _- K2 `
4. 
   
5. #include "MePort.h"
   ( |! w5 O/ Z1 ^- W) r, n& n
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   4 o+ o6 Z% w# l0 t: w
8. 
   
9. //double Input, Output ;
   ! M! ~2 u; ^" z
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    $ }. x1 Q& v4 x3 x( a, O" g
12. float MinPower = 120;
    8 R# c' ^$ t3 t3 F
13. float Error,ErrorAcc,ErrorDec;
    8 f( t/ A! l, P! X- ?+ v
14. 
    $ a8 m7 _7 \4 K# a
15. float Kp=0.14;
    ! }3 `4 i( f* F3 K/ \
16. float Kd=0.00020;//23;
    + z! p$ w/ {0 Z: A; `  C
17. float Ki=0.000201;
    
18. 
    $ ?2 t& j5 w( J+ U5 t! K  K/ S" l: Y% a
19. float nPower;
    . I7 M; z4 p' x; v
20. MePort lightsensor_6(6);
    0 b( J/ {. y) l6 R
21. MePort lightsensor_8(8);
    ; l% S* M- A, `) e/ d
22. MeDCMotor motor_9(9);
    ' X5 }- G. i6 e7 x8 Z' k
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    1 ]# c! q: T) L5 G7 v/ Z6 N  h
25. const long interval = 1;
    8 a' \$ A+ A" ^8 s/ Z% _
26. 
      ^/ o. l) I$ t4 q; X' i# N) P
27. void setup(){
    % k( F" Y) s: A
28.     lightsensor_6.dWrite1(1);
    7 V( `% V: }! a- P2 ^3 Z
29.     nPower = 160;
    : g( d: h9 f2 N  \) \, c
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    
34. void loop(){
    * X3 z# f) M! g6 y
35.   unsigned long StartTime = millis();
    - D' e3 S  J8 y8 v9 u8 [2 u6 y
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ! ]0 r1 o: ~3 `2 u
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % u) f8 O8 _' b! Q0 h9 Q
38.     ErrorAcc +=  nError*Kd ;
    9 C7 E& B; e* g% |
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
      D& P+ q3 ]  Z2 w# c# ?6 A$ `7 ]
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    4 `% E, `# p+ U. T" J
44.       }
    , z: \# E" S8 z& k
45.     } else{
    : y8 y% |+ [' b' R8 x" O
46.       if(nPower > MinPower){
    * h1 p: e$ }7 q1 Q) Z" `8 U
47.         nPower -= 2;
    . `& R. R: f; o" j( d
48.       }
    * f) [$ `" o1 c! z2 }4 Q
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    5 U( c9 ?+ {1 S+ m/ P0 J
52.   }else{
    
53.     motor_9.run(0);
    5 z! ^, I3 j4 h4 _+ \' b& \
54.     motor_10.run(0);
    - m' |$ q2 ]/ N3 a; E
55.   }
    9 u' [3 B& R) }  ]$ o6 b7 U2 F
56.   do{}while(millis() - StartTime < interval);
    
57. }
    9 R$ Q: P( R$ f$ \4 c9 e3 l
58. 
    4 O" w2 j; m9 r+ K  s' A- |
59. void MotoL(int Power){
    ) V' M5 \  s0 A0 o: n
60.   if (Power > MaxSpeed){
    ; d, |( t2 s2 m* E! l9 K9 E* [
61.      Power = MaxSpeed;
    
62.   }
    " u, E: V( p- c( E- T5 N
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ( a; m' A! Z) K" T
65.   }
    " z% j: J: A! [) h0 |
66.   motor_9.run(Power);
    , [2 F% @6 j  o+ s
67. }  
    # j4 U' P4 L0 J
68. 
    
69. void MotoR(int Power){
    5 |0 p4 G0 {  L9 a8 T
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    2 R8 N  d% k! J& V1 Z# U; E
74.      Power = -MaxSpeed;
    # R2 F' a/ ?' n# u/ j
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

( J- R2 j( r  W7 V
7 u% J$ {$ C$ l

您好，不知是否能向您請教。
' I8 ~) h0 ^2 {6 y. _8 \$ c7 u目前和宋修賢老師在處理Ardui Car
, v! [! b- X; t雖然已使用較繁雜的方式處理了跑出黑線外的狀況
$ s1 ]: |% b! I# D$ a1 }( O6 ?
# X: X, J5 ^4 S/ n但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
/ Q9 L5 R7 i- t) B4 a! A5 b不知道您是否願意教我可以如何處理


以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   / G+ w4 _8 h$ X. {$ r
5. double values;
   1 P8 V! S5 J& ]+ r4 ]) e( e" r
6. 
   8 p" T8 T6 u1 v4 j
7. 
     w( Q) t, n4 z+ T& v
8. void CNY70()
   + p( A0 T; a7 t; @3 v+ V
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    , N+ [0 b+ Z) l1 i% E% ^7 X
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    4 `# @# f3 r) j% ?" ~* g
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    1 ]( Q9 F) `! c& D; Y
21.     valuesMM = CNY70Val;
    # \" @+ h6 I6 l7 ^
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    " E# v7 z* f) I- U# j% u2 f2 w
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    ; S" R: h$ c6 x+ ~9 E! G, ]* m
33.     CNY70();
    $ y2 I# m' s1 F
34. 
    
35.     int error = ((int)values);
      b& ^! m( X3 z5 v" X; [
36.     interror += error;
    9 j  e/ N" x0 Y0 @* }
37.     int lasterror = error - olderror;
    0 S' q( T9 [' C$ M0 `0 Q) }
38.     olderror = error;
    " ]7 s. X& n. ]& t- H
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    1 f0 ~( W2 I' J4 I( _4 a
41.     if (power > MotoSpeed)
    / ~2 ], u2 n& u% i$ X
42.       power = MotoSpeed;
    1 n( Q+ j( M' \3 y/ Z0 ^/ `
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    " p5 N) j9 L5 j# |7 F
46.     if (power > 0)
    9 f( g$ r1 G6 R) L7 y3 @6 b8 p
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    7 w' Y. d' A, j1 ]- ^1 }0 U6 x
50.   }
    / ?* @6 ^* @# o
51. }

複製代碼

: W% p$ h' Q) P

感謝分享

感謝分享