PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   " m: }6 A0 N3 ]6 t9 M
3. #include <Servo.h>
   + W( l; i8 d* \  Q3 q
4. 
   + M' Z4 Q: q2 I* M  Y0 p/ I
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   * x3 F4 `. k$ Y  k9 K& t% J5 U
7. #include "MeDCMotor.h"
   8 @# H& p: \- _) I! c/ s+ c
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    8 {" E) ^: w* W9 _
11. float MaxPower = 180;
    3 ~) E5 z( n- z# P2 r- _5 Z8 z( @
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    5 B, u; p- F5 c# P) y
17. float Ki=0.000201;
    + R) _& V% ^# n" J6 m
18. 
    
19. float nPower;
    4 o  x5 r3 t* u; A/ Y( T
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    8 I5 J. K) g0 q# @
23. MeDCMotor motor_10(10);
    / X7 G. X8 V* I( @1 \8 h
24. unsigned long previousMillis = 0;
    7 x' t* i8 [+ G) S
25. const long interval = 1;
    ( T' F2 t  C, `+ {: P' i
26. 
    / i2 V2 d6 }2 x) j
27. void setup(){
    1 v5 H/ a7 I9 T5 s, I
28.     lightsensor_6.dWrite1(1);
    + |+ l1 m8 D# [0 ?1 r& w2 C8 s# F; N
29.     nPower = 160;
    0 e$ v. D6 u! H2 T) l
30.     Error=0;
    
31.     ErrorAcc=0;
    - `: R4 `. J0 O5 U; t8 Q
32. }
      ^/ R9 ^+ _. M& j- G6 q
33. 
    9 U" w! K7 [: p
34. void loop(){
    
35.   unsigned long StartTime = millis();
    0 ^1 t6 C! K0 V% w6 D) P! e1 A& a
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
      u4 |. M# P" c% m
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    5 C$ X* F8 h* D$ L
39.     ErrorDec -=  nError*Ki ;
    3 o3 _6 b$ m7 m& m% f
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    5 }) C+ q8 B4 G
41.     if(nError < 80 && nError > -80){
    - _3 p" Q# r! v2 r' ^* ]1 @
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    5 O# k, K. b( X7 {7 ^; M9 z" n
49.     }
    4 J( {: Y5 C- L* J0 A( s; }
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    % A& {6 `6 n4 A7 ]  B2 w
52.   }else{
    # [# K- G- M7 U8 h+ M
53.     motor_9.run(0);
    
54.     motor_10.run(0);
      F5 s7 |8 h# ?4 @
55.   }
    ; h, j3 v+ ?8 ~' a4 M
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    : r+ p9 V" `$ C) v& ?& }
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
      N3 Y6 h5 p4 U/ D
61.      Power = MaxSpeed;
    
62.   }
    ; c# V% `& u8 F1 _3 ^
63.   if (Power < -MaxSpeed){
    9 {* ?+ M: c( F1 l" @2 l8 Q
64.      Power = -MaxSpeed;
    3 _- e3 V- K( e3 y  }3 u2 d- d/ V8 P
65.   }
    
66.   motor_9.run(Power);
    ; Z8 K- n9 ?& \2 e7 O- _+ f$ f
67. }  
    2 j/ u' h: Z$ h* w- \$ G& R2 @( B1 |
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    ) |+ a! U3 Y6 u. g6 R" Y
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    7 s8 F! g5 l4 _+ \  \0 I( m6 O+ U
74.      Power = -MaxSpeed;
    . J( F, O0 f& ?, y4 i
75.   }
    3 {8 j2 X3 u! y$ Q3 T. {5 d
76.   motor_10.run(Power);
    5 x4 ?, B# A- M. V( N# D% `% P
77. }  

複製代碼

2 k2 t; d6 `4 ?  s

5 N) `3 b( ]# \8 g4 M& c
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
) ?* O$ i0 A' V- z+ v3 a2 d雖然已使用較繁雜的方式處理了跑出黑線外的狀況

0 U9 Y/ ^7 _! j9 [  _但基於想追求更精簡的程式所以還是想請問一下
% i+ G; E2 D% u! I, f5 s# U就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
# s4 b. L6 W/ ~0 |6 M( U9 S# a; E: x不知道您是否願意教我可以如何處理

  H) Y! J1 N' V6 F" _  y
# W( w5 J. J3 f( C  N, Q1 Y以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   2 P" G! i, F7 {/ H
2. double CNY70Val = 1000;
   : N) P; d7 U; Q' `1 v2 J- H
3. int interror = 0;
   + Y2 ~! ~. W2 R. [& J3 c
4. int olderror = 0;
   
5. double values;
   4 C% v+ G8 c6 E
6. 
   
7. 
   : W9 a# ^6 m2 H7 k. N+ T8 q
8. void CNY70()
   
9. {
     c) s8 \" j7 e# r9 y0 y
10.   valuesRR = analogRead(RR)
    ) o1 Q$ w" D$ [, ~
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    ' k/ s8 |# f3 y; B
13.   valuesML = analogRead(ML);
    ( A- z2 X4 J$ @+ w- L
14.   valuesLL = analogRead(LL);
    : C- Y1 c2 s( O% O5 O1 w. c
15. 
    
16.   if (valuesRR > CNY70Val)
    * u8 Y1 W4 }, ?7 q* W# Z
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    $ V( ^: B8 c% T$ s# V
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    ! u9 ]5 u8 }+ l, R, W; z+ b
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    % ~1 I$ W7 f  h5 c8 O; h4 [
30. void Car()
    # c8 L3 \2 K( A4 o6 Z$ r) U1 \
31. {
    
32.   while (1) {
    + b' u0 n1 n1 M0 a' E
33.     CNY70();
    $ O5 q* W3 M4 r6 z& I
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    . Q3 ^8 {7 W2 [5 k9 t! @
37.     int lasterror = error - olderror;
    # k( {5 Z3 P( J6 H+ G* M
38.     olderror = error;
    / @. X* `; G# m1 ]# `* a
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ( L' x! L& u& A+ E4 c  k
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ) ]4 d9 G# C$ E" t0 w4 F
44.       power = -MotoSpeed;
    3 I' S. e5 [7 K# B. P% |0 W
45. 
    
46.     if (power > 0)
    ) a" l- a; S* A! u2 S+ Q
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    / c- e" D. X( s' D; J" B4 N0 j7 T
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    : [6 [; o# z; o: \
51. }

複製代碼

感謝分享

感謝分享