PID 循跡自走車範例

1. #include <Arduino.h>
   8 {) P6 w3 N* K8 I* {
2. #include <Wire.h>
   4 N3 p% b& L, T; Y; S
3. #include <Servo.h>
   . v' l! c2 S" a  h- b% B1 Z* @
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   ' }0 I# g9 K, s5 P8 G: H9 |
10. float MaxSpeed = 255;
    * u" a% Z) u; d+ I& h) @4 @2 [4 U! L
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    6 z* U" P7 ^3 E4 }
14. 
    
15. float Kp=0.14;
    & P' {/ d! q& D( W
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    0 J" C& d9 r; i3 U+ f" y4 l) @
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ( @9 t: d: f- T' h, w
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
      Y0 f$ i; k' J. h6 b! w; u/ k8 V0 F
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    ' b, ?% [! b: p+ F
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    - e$ v! a' H2 Z+ t, e
33. 
    
34. void loop(){
    9 X  K' ]; B% {3 ?
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ! Y( y; i( H; w
38.     ErrorAcc +=  nError*Kd ;
    / U) L3 a( V( x) u
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    # |" c7 E' d& k+ u4 v
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    & D- N6 l; L* Q% q1 k' D
43.         nPower += 3;
    ! Z2 t+ h4 A) i
44.       }
    7 x2 E. Y$ I% D1 C! p6 L- l8 c
45.     } else{
    # N2 v. G' ~: p5 f
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    - X+ P) b* C! c% C
52.   }else{
    6 m* d/ ~% K: X3 t
53.     motor_9.run(0);
    + s, V- u" d: H* ^, k1 l/ ^
54.     motor_10.run(0);
    
55.   }
    3 \2 V" a- e0 P% a( z6 M
56.   do{}while(millis() - StartTime < interval);
    
57. }
    3 q9 p! ~& K" m6 m& d! c
58. 
    
59. void MotoL(int Power){
    ) }0 \, ?8 R9 `( l$ w3 q
60.   if (Power > MaxSpeed){
    5 _" B) x" e# w  {2 G( f, @0 M
61.      Power = MaxSpeed;
    " S  i1 ^) Z4 q( l& E" v4 }' v
62.   }
    % {& W. P3 E/ e- |. W
63.   if (Power < -MaxSpeed){
    9 X1 w  \, {" S' n; M, q( p
64.      Power = -MaxSpeed;
    / {$ ^1 x5 V( u5 d9 j
65.   }
    
66.   motor_9.run(Power);
    5 Q, z5 X) T) p5 ?# d8 k
67. }  
    2 B5 P; i+ {6 j+ g/ x; V
68. 
    7 ]; o5 w$ j& g7 [$ h' L
69. void MotoR(int Power){
    , J/ X' h% r; K' W# z
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    2 o9 U9 A2 J; t' |
72.   }
    # M# c+ T! g2 O1 u* @$ C
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    ' w6 }$ i0 {* k6 x
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

& H- e. Y7 s0 y6 P
1 W3 z( k2 E& @! J/ e

% m' S4 a; G6 t3 ]您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
0 A4 r# F$ _- O! @+ j雖然已使用較繁雜的方式處理了跑出黑線外的狀況

& `7 d' g8 p2 [7 n但基於想追求更精簡的程式所以還是想請問一下
2 s' e1 Q1 y; \2 u+ n2 K就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
1 \$ _* v( u3 `$ x" R) M: X
1 r/ I( T0 s+ x# D' m% @1 U9 J
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   / A  g0 v# a  a7 f; x; Y' D
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   5 p4 |5 M& g7 G) k4 D: C
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
     j' L# X+ [# v( j  b: C
10.   valuesRR = analogRead(RR)
    8 b2 l: K; D/ c; d
11.   valuesMR = analogRead(MR);
    - F& Z1 n; g$ e/ [& w
12.   valuesMM = analogRead(MM);
    0 G# Z2 J5 }9 n0 [' `7 c( `
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    4 a8 F7 _& `  D4 |" ^0 B0 E
15. 
    
16.   if (valuesRR > CNY70Val)
    - K) I' N: o, e4 E/ c- E
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
      f# Q4 S' O- b8 s! q4 g
20.   if (valuesMM > CNY70Val)
    ; U% K: j0 F; [! W
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    ( F8 G! Q( M* h2 T
23.     valuesML = CNY70Val;
    0 [  j' b3 }2 i
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    ; `3 O. c) s$ d3 i
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    9 y2 \7 `- _* r
28. }
    
29. 
    1 X( H# q' K* e- G# f
30. void Car()
    1 I' X# A# l- W" e: S; _
31. {
    
32.   while (1) {
    / }* ?8 U) u6 [, U" N; P* j/ m, ~
33.     CNY70();
    % f# n# m! I2 c. h" d& U4 ~
34. 
    ( n9 O' q2 g1 h+ k- z
35.     int error = ((int)values);
    4 \& r5 i  ^9 W% P
36.     interror += error;
      Q; ]; {7 ~4 ~
37.     int lasterror = error - olderror;
    & K8 F) h, G* o2 u) [
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    5 [. |0 w; k  c% S4 {
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    + r. f4 N: }; \% g* z
44.       power = -MotoSpeed;
    4 S5 F: S3 j2 d; q! u$ }& \8 N
45. 
    
46.     if (power > 0)
    1 ]$ H3 v  R$ N
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    + r, Y2 q5 b) R2 ^4 ^8 r
50.   }
    4 {& |! t* t) h% ~
51. }

複製代碼

( Q0 g0 S9 J4 l6 |
5 A% s: v- B  ~. o  S- z! L

感謝分享

感謝分享