PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   , Y6 s" \8 M: N7 Z9 v
3. #include <Servo.h>
   6 j. b' R2 a" X  e0 s3 |2 u+ g
4. 
   & A7 B7 \, q" e7 W+ o( H- V5 ?
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   6 ]8 l8 p6 ^+ a0 |5 n7 l
7. #include "MeDCMotor.h"
   ' W3 Q% H5 ?# ~0 N; ]5 d" U
8. 
   
9. //double Input, Output ;
   ' e5 _: H9 ~! J' `. e$ R8 a
10. float MaxSpeed = 255;
    ; y$ y) _2 U; m" Z1 ~2 O* U
11. float MaxPower = 180;
    
12. float MinPower = 120;
    ( y, y9 J: d/ }: L) ?- n( _
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    : Q- ?0 L1 ~" q( x
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    - F1 h6 Y3 L: q1 Q& Z3 @
17. float Ki=0.000201;
    
18. 
    ! M+ e# F3 s  W. l% O: v
19. float nPower;
    9 O7 r9 g. J8 s- L1 E
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    5 U, j: k3 C- I! O
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    + g% ~- v6 L& L; [& o; d. ^0 x. F
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    % L: H9 L* w, \
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    0 w0 T5 K3 v* x8 x" E# m& z
33. 
    
34. void loop(){
    ( s: E" A& p" p% z4 U. n8 g% f: h
35.   unsigned long StartTime = millis();
    9 I6 p+ y3 B/ O9 q6 ]$ `  G" N3 z
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    1 G9 w$ o! P2 K9 ^/ j6 X8 R) v  n8 m
39.     ErrorDec -=  nError*Ki ;
    ; S  \, |2 N! L4 o/ |* |
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ; B1 z: M% q2 t% j# \7 I) J
41.     if(nError < 80 && nError > -80){
    0 f9 n- e( j" n& [
42.       if(nPower < MaxPower){
      h2 @- s" c) s8 G, d* |
43.         nPower += 3;
    
44.       }
    : |( s5 v( ]* Z% U, n) w$ B- k& ?
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    6 ^# L( L6 N: u$ a0 x6 u
48.       }
    + E+ `( T7 K1 i' Y6 e' r  H5 P' i5 v$ B
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    & b9 V7 A  j/ I- `  o0 P
56.   do{}while(millis() - StartTime < interval);
    5 P5 E/ I1 R5 ^; a
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    , y2 p/ i9 e) J  U$ d0 T! g
61.      Power = MaxSpeed;
    
62.   }
    4 t2 q, v! v0 q7 Y. [; j6 `
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
      q; |  \" k( X) @9 j5 i1 d
65.   }
    ! @# G& `" ?6 G  [- K" E
66.   motor_9.run(Power);
    
67. }  
    ) R0 ^9 V7 N  ]1 U/ J' c& m
68. 
    . u: D  G" f+ X: x5 n0 [
69. void MotoR(int Power){
    + v9 `$ d8 f$ G- l
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    & F" x3 _) [. e' G. {0 H5 b0 v
72.   }
    ' t' ]$ E: F. F( b" k
73.   if (Power < -MaxSpeed){
    . H) u8 X8 d- O. h0 I/ r. O- w
74.      Power = -MaxSpeed;
    ( p+ a, v7 Z: R0 h( d
75.   }
    ! }6 N5 U6 D# G5 P
76.   motor_10.run(Power);
    
77. }  

複製代碼

! Q+ u# H7 L. N1 s8 I+ w3 Q
! ~  _* n" B" x- f

. a, \. I2 e7 [. Q' R
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
" l  S; e, b4 J4 N
3 O% p& C/ I% ]  a但基於想追求更精簡的程式所以還是想請問一下
9 E3 X4 @# c+ Y" z% x就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
# `' W. P& g$ T$ K+ T/ \& F
' o+ {5 [8 i  s0 ^* I
% G4 h7 e+ C; r以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ' S$ B/ b( \8 v% K5 r: Y8 {
2. double CNY70Val = 1000;
   ! ^$ e$ r. q8 V: }1 j
3. int interror = 0;
   
4. int olderror = 0;
   / `" e+ l6 Z9 P  _6 Z
5. double values;
   1 `9 ^# M# a) |8 ]7 p( _( L8 U
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    9 \- l  J4 O- o( X8 R
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    7 l# Q% j+ C: [3 O& f. [0 B
18.   if (valuesMR > CNY70Val)
    " F9 X' T8 P: w: u
19.     valuesMR = CNY70Val;
    5 z1 h% l" o$ Z) S( P, n3 M
20.   if (valuesMM > CNY70Val)
    5 D( l  y4 [3 h* r9 ?0 D
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    ) O+ ?7 p  `7 P! a0 Y. X3 v4 f0 t
24.   if (valuesLL > CNY70Val)
    6 ]  {; a% Z# K" v/ w5 t, _0 q3 I
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    # w  n: |2 C! w4 s$ G0 p/ t
28. }
    0 @1 u' \: a! }5 ?0 x
29. 
    
30. void Car()
    
31. {
    ' Y! }$ N4 |0 U8 A
32.   while (1) {
    # d1 d' w1 U9 t; p5 R
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    1 C2 t' p. R- j- I9 l. S+ N
36.     interror += error;
    . l- F0 w% |+ K$ Q; E! k9 j
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
      j0 f; ?; {: \5 G/ _. c$ i# U
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    , X! C/ v) b  |7 h! B
46.     if (power > 0)
    0 O4 D0 S; E9 z( g/ \, |: T( [) ?5 N& b
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

9 j3 x! l) q/ s+ \' ]

感謝分享

感謝分享