PID 循跡自走車範例

1. #include <Arduino.h>
   ; F' x- B" F3 @
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   0 Y2 O, K$ i3 M1 q2 J) J/ S/ B9 L% p
8. 
     i; [- l! ^. i9 S$ O
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    , W9 `. f9 M7 L# w
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    9 D# ^( |) ^& M' b
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    5 y3 |: C8 u: e0 [/ t
17. float Ki=0.000201;
    2 b% ?5 O& M( `
18. 
    
19. float nPower;
    1 m" A% g; j2 }, w  z
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
      r9 [/ \0 f/ e4 W+ n: N
23. MeDCMotor motor_10(10);
    : c7 J' w. F- h- d& }) q
24. unsigned long previousMillis = 0;
    ! i' D7 R8 H2 t- t# d$ z
25. const long interval = 1;
    ! P/ W# l; }- d  V8 \
26. 
    
27. void setup(){
    $ |! r; T2 @8 a4 Q* z+ W
28.     lightsensor_6.dWrite1(1);
    ; ^- a, \5 g3 C: s- a9 S
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    $ [! r& }' [0 s' h6 m( s
32. }
    
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    " \, l4 [, p- O5 X; J1 ^
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    ; e$ B! G2 t7 ]) m
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    - p$ [& _: T5 O) g
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    5 l! n& U9 S( T8 H) W. w; K1 B
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    0 o) h; ?, A- B8 X) j
43.         nPower += 3;
    ; @, y. R- k5 N9 a+ s3 Y+ M
44.       }
    / b9 b- D5 f) y, {8 C% b5 G6 O
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    2 u9 c8 e! \" l
50.     MotoL(nPower-Error);
    ; V" b  }2 N9 c( w
51.     MotoR(nPower+Error);   
    7 Y9 _. i# {; V4 z* l1 w0 i+ F/ P# }
52.   }else{
    ' s6 b( R% J4 g/ Q
53.     motor_9.run(0);
    , u: }& r# h# O/ S. }9 I, p
54.     motor_10.run(0);
    . E/ r+ [6 o( g6 R& p/ f5 G/ F
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    ; J4 l, ~( E- ?4 s9 j' {
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ) [8 @( F7 C- N. ~$ p1 ~
65.   }
    0 V- x: r$ v4 ^4 z% _/ y0 ?! W
66.   motor_9.run(Power);
    
67. }  
    9 P* a- g# S* a, Z; T* E6 J6 n
68. 
    
69. void MotoR(int Power){
    - t5 t$ D/ h' {/ Z$ Q$ h
70.   if (Power > MaxSpeed){
    1 h0 @6 Q) `3 b' n7 I
71.      Power = MaxSpeed;
    6 P& F# f- f6 w) b3 l" y  I
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    . k% h3 y1 h, W
75.   }
    3 t0 R) Q9 j* o+ w8 _3 G
76.   motor_10.run(Power);
    5 h3 Y% r* i/ Z6 m+ Q0 F0 M( o& W
77. }  

複製代碼

+ V. P$ p# z$ S! z. J8 ?5 K0 G. c

. V; m, R' ^, L3 b+ P
8 I+ E6 }9 M  U: \. g% ?" i您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
* J5 J2 H7 z7 Y' l7 f  K1 E雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

+ ]7 }5 T! p  g$ c& R/ ?  J
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! M+ Q. M" ~9 Z! @
2. double CNY70Val = 1000;
   0 |+ ]% s4 ~& |1 }! V
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   " O# B$ _$ r3 a
6. 
   
7. 
   + b4 N* r$ H2 v& T0 b1 W$ x
8. void CNY70()
   # ]1 a, J8 O1 b( M7 L
9. {
   
10.   valuesRR = analogRead(RR)
    . U9 V- L* N& O4 `4 y8 B- k1 B/ o8 w
11.   valuesMR = analogRead(MR);
    6 b9 r/ e* U5 R$ Z
12.   valuesMM = analogRead(MM);
    2 z; j5 F- Q; M! \* R; m6 \0 N
13.   valuesML = analogRead(ML);
    0 H8 Q/ H% h2 x- [) b
14.   valuesLL = analogRead(LL);
    
15. 
    % W" B9 Y7 J% {; i! Q9 D( w
16.   if (valuesRR > CNY70Val)
    ! ^7 b) [# t! k6 \9 y2 x4 N% B
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ' R6 j: H! i6 @# V( S$ c; n* ]3 N- n, v, h
20.   if (valuesMM > CNY70Val)
    ! P/ C- k5 d# Z4 _+ V
21.     valuesMM = CNY70Val;
    3 U& H% ]  v% M) Z+ D7 ^2 S
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    4 `/ m, S- s- w0 _8 L' Q
24.   if (valuesLL > CNY70Val)
    ) |2 @3 B* R' I
25.     valuesLL = CNY70Val;
    5 j& Y( d) H! _+ C' G  |
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 k" U. {. `. S! b) W/ p
28. }
    3 u3 Z- }! K7 C" Q. l6 o
29. 
    4 ~5 d2 ~+ G' ^9 i. y
30. void Car()
    6 G8 R6 b6 A. Y" @# ~$ ~6 ~0 E! L! F
31. {
    3 F! @- N6 P1 ]9 e
32.   while (1) {
    
33.     CNY70();
    ) ]0 t6 w- ?1 ^+ c* t
34. 
    
35.     int error = ((int)values);
    ' J! A3 n) F, T' x8 @  r7 t/ [: x
36.     interror += error;
    
37.     int lasterror = error - olderror;
    8 `5 c2 H+ Z; P* E2 |* `( j
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    : E8 P% @6 O) a
44.       power = -MotoSpeed;
    $ A& X' `' r7 e
45. 
    ) T9 G% h' w( u+ M5 a* L" M
46.     if (power > 0)
    8 A/ s: f- m! Z" l. n7 ~
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ( Q/ P2 E7 U$ J( M" s/ [- T2 r
48.     else
    ! R3 A# _8 e, f7 V
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    ! y, k( i( g4 K" D0 H- ~1 d7 J
51. }

複製代碼

6 ?+ s2 y4 l3 o6 f7 S  L

感謝分享

感謝分享