PID 循跡自走車範例

1. #include <Arduino.h>
   # b$ T" D% }+ T0 v' r4 O- i
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   3 v9 y% z$ V0 Y" j6 S/ a
7. #include "MeDCMotor.h"
   
8. 
   6 i0 u- H) R# A, l
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    0 {6 L& V1 v8 ~* ^
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    & f8 i9 [. p$ y
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    , G& B( C- B/ j! d- [  d
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    ' ]4 s7 W( `# [! ^. {
25. const long interval = 1;
    9 x$ Y+ @8 ]# [' r; v8 |
26. 
    " _" d& u7 p0 y4 l
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    , A7 U$ h9 x  i" w
29.     nPower = 160;
    
30.     Error=0;
    + N5 l/ \- k3 K2 ~; j' }
31.     ErrorAcc=0;
    7 L1 }0 c. L* A6 o) B! ^; S% x- e
32. }
    ! O$ s' o! d( ?# L" @
33. 
    
34. void loop(){
    $ [3 n. Y5 E/ N
35.   unsigned long StartTime = millis();
    $ h" J2 P; E1 o9 {0 r+ s
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    , P2 B( x5 u' N( p1 y3 h
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    - N4 w( N1 \4 H: J9 ~
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    " [% n( r; o8 t; `. Y: Y
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    / v- c2 u. j# K* P6 F$ a  p; u
45.     } else{
    6 `' R+ \( k. x. y$ ]5 S
46.       if(nPower > MinPower){
    ' B/ p9 `) ^$ i* w+ G- e' u
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    & V2 c$ X6 n5 u
51.     MotoR(nPower+Error);   
    % G5 A* C; R1 S4 p, b9 h9 \
52.   }else{
    . o  e4 ]- p3 N* W7 c
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    2 K- N0 K  j  k( i6 V
57. }
    9 d5 @: p" _0 G* R9 _/ R
58. 
    # ~; y" v+ L9 O% c+ N* v
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    ' F  I; u- {3 B" Q" L
62.   }
    4 y& c( S1 ]0 x. s
63.   if (Power < -MaxSpeed){
    " |; X: N* Y- a  m$ }: C0 ]$ A- U
64.      Power = -MaxSpeed;
    , Z; o& O1 w' M/ t8 S3 h  T
65.   }
    " O# @* l# ^( g/ a
66.   motor_9.run(Power);
    4 l6 c' ?1 \' Y0 |" }) g
67. }  
    ! J; r; A* k* O: F- l7 @) [
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    ( v. V  P+ E! Z# T  G: h" j
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    # @* O! J' ~7 e
75.   }
    
76.   motor_10.run(Power);
    : i& a) y/ ]7 |3 r7 @3 F3 L9 K4 I
77. }  

複製代碼

7 R# J( a. U7 J, ]) |  t* W" m

您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 @: C) ]$ K6 w- J  P9 l* X
% ?1 e9 |2 U( e4 D6 r8 Z但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
$ n1 A( ], R. P0 R. W
0 {: B" ?: e  p# R( P% l4 ?9 U) P- X
6 ]2 x) }: V1 M# `7 T以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   ! {2 _+ ~% U" A$ q( c
5. double values;
   / e8 d' r9 O1 U! v( u+ W1 T
6. 
   
7. 
   * v. a2 W% }% k  S, G" p- e
8. void CNY70()
   
9. {
   . c" R( K1 D7 D7 u
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    & D8 S- E7 Q. J0 {* E# [( J
15. 
    ) O0 t6 o6 a+ ]0 E7 T  e- p
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    " ]& C. J* u% P+ _! j2 m( r
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    " v1 Y0 W8 d, K- p  a( b2 r" p
22.   if (valuesML > CNY70Val)
    1 t! g) n2 t  }/ F0 J+ z
23.     valuesML = CNY70Val;
    ( a' b4 |* y/ q5 c8 s3 U
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    4 q7 o' _8 ~# d4 \) a+ L. Y
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; ]! z) r8 W6 w
28. }
    $ j$ O- q  u* M* s6 _' z# P6 X
29. 
    
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    6 V- y. f9 y6 M; Q% X
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    8 U& P) E$ |# `( v$ q. e
37.     int lasterror = error - olderror;
    8 U0 F- r. [2 h/ b3 f! Y( u. c# O
38.     olderror = error;
    . C- H- ?6 x' ^. H, Z
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    . b5 |5 i( J( ~* d5 P
41.     if (power > MotoSpeed)
    & U. a4 S4 ~( W+ H" v
42.       power = MotoSpeed;
    & Y( C( c4 z$ m2 [% o8 @  D& f4 C% H
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    % T7 @! R3 \# u. y; z8 |! a
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
      K. i! s$ h2 ]9 F% v' D
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

5 t- h  z$ }0 }

感謝分享

感謝分享