PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   , H# J% q7 {6 Z: W5 Z$ t' f. A
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    5 F# U) A& V; D
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    6 T. I& K6 m+ W6 L
14. 
    6 S( q, `& G9 j  z4 P
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    * X! w( h% g, `7 t
18. 
    
19. float nPower;
      Z, ?' E5 X0 t
20. MePort lightsensor_6(6);
    0 ]0 g* D# G; ]- s' L& @9 f
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    5 A/ ~0 X( o# g
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    % x& {; `- X+ }6 t3 L' r
25. const long interval = 1;
    ( E; U2 e* R  h
26. 
      I+ ]6 l# ]) ^$ s# R
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    # d/ o4 ?& v% f  `: F% o% R7 v$ L
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    ! n$ }; R! ?9 V0 y' g( D" f- n
33. 
    
34. void loop(){
    7 I' Q% J' U% d8 m' b
35.   unsigned long StartTime = millis();
    4 l4 O) y  _" F+ [. j. }
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    % x) k$ {& b1 w+ s* D
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
      X* W4 g. q2 K( A$ P+ F) M
42.       if(nPower < MaxPower){
    1 e! l6 Q" R- U! F; k
43.         nPower += 3;
    
44.       }
    8 @9 H5 P; N" |) W0 V
45.     } else{
    
46.       if(nPower > MinPower){
    + p: `2 c, W, f5 s
47.         nPower -= 2;
    
48.       }
    % a; D; u8 F" P( F" G( ^' d; n/ E
49.     }
    - ]) B8 N# T% K/ ~
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    $ N) d! \3 L; F1 I! T6 E) x
52.   }else{
      s4 S& r+ w6 I$ Q
53.     motor_9.run(0);
    7 T- c" e( ?' m. m
54.     motor_10.run(0);
    
55.   }
    0 M; I! x- `" g/ u' u5 Z5 h' ^
56.   do{}while(millis() - StartTime < interval);
    $ Q  ]0 _* E  C$ d# |6 `) a
57. }
    
58. 
    9 k0 o1 W  V" u7 a, u
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    ! s* U7 K& W/ p
61.      Power = MaxSpeed;
    , s( Z/ L% u$ }, _, C
62.   }
    
63.   if (Power < -MaxSpeed){
    3 p( c  D# J% V2 ?: F$ C) e& \  T& U# V
64.      Power = -MaxSpeed;
    . N( |6 `/ p2 A' G
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    " {0 p% I" a! d& O- c9 `
68. 
    9 {6 G% y0 q! }/ v- c& s
69. void MotoR(int Power){
    * x  P; x3 p9 W) h
70.   if (Power > MaxSpeed){
    . Z7 L8 O# U. z
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    9 m! v+ i( V! ?
74.      Power = -MaxSpeed;
    " m; a3 w; U7 l1 ^* N' _
75.   }
    - A. V( o8 J. ]& w8 O9 |
76.   motor_10.run(Power);
    
77. }  

複製代碼

, i3 X) {! o9 T9 t& W5 i& d; \
) ~; K* }2 ]! V" V

* O) B3 z9 N% d* p: F: i您好，不知是否能向您請教。
- l1 I$ n, [2 `/ P& k6 }/ h3 P& ^目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

0 ]1 v2 @/ l/ y2 z但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
8 S$ x: Y! I1 J不知道您是否願意教我可以如何處理

! o# [. @3 c+ Z+ ^$ o) u3 q% m, h* z
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   * p% ~9 @+ u( Q1 V, k+ H
2. double CNY70Val = 1000;
   
3. int interror = 0;
   9 M) l( H9 M0 K# V0 W' x2 @
4. int olderror = 0;
   ( s0 d: H9 }- `3 i
5. double values;
   . u7 D$ E5 T$ u
6. 
   
7. 
   
8. void CNY70()
   : Z, j3 J' j9 e- F5 O, c" A
9. {
   $ {+ p7 ~5 r( ^# X5 {/ Z+ @, k6 T( `4 S
10.   valuesRR = analogRead(RR)
    / i" t9 N9 D  d. @* ?8 b& }
11.   valuesMR = analogRead(MR);
    ! P% J& D: @7 D/ i, {- O
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    ! n8 Q+ Z4 M+ T1 K3 L" |# t
15. 
    6 S% q$ k. A/ [% l' N
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ) {$ N7 X6 i2 Z  L+ G' t9 L
20.   if (valuesMM > CNY70Val)
    0 k4 L3 F$ a: U# c0 J
21.     valuesMM = CNY70Val;
    9 d) A. R$ h- a( p8 U7 O% a
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    3 \$ v/ ]3 {" ^
25.     valuesLL = CNY70Val;
      @1 M' ^! H4 ?1 M* v
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    . i/ }6 N: x  E0 u
28. }
    : n* v# g0 J0 y/ j" h
29. 
    & e/ e) X& x) B1 R# z4 a/ a
30. void Car()
    
31. {
    
32.   while (1) {
    + _/ d/ A0 c' _% R% L, N5 N
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    + J. E4 E* }, k& L
36.     interror += error;
    
37.     int lasterror = error - olderror;
    / S- z+ u' A% l
38.     olderror = error;
    5 G- j; l1 c- }+ h( N
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    " w+ z2 I0 ]. d# k- X& E4 v2 Z
42.       power = MotoSpeed;
    1 J3 _1 ^) x2 }9 B) N8 n2 S9 V. y
43.     if (power < -MotoSpeed)
    # L% l6 p3 l, m  b: j* _4 c
44.       power = -MotoSpeed;
    0 y% F+ L# R. e2 j
45. 
    
46.     if (power > 0)
    $ g+ {1 X- t1 b, X
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

+ z; U3 o8 G7 z6 e; c' e; [

感謝分享

感謝分享