PID 循跡自走車範例

1. #include <Arduino.h>
   % h+ k* k1 e# ^0 H/ l  g! U2 k0 L
2. #include <Wire.h>
   0 f/ s; K  k/ B- _$ u, A( T
3. #include <Servo.h>
   # U$ m; ~& ]6 n5 y% p+ g2 {
4. 
   8 P+ S: x" D! u4 Z7 f% i
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   1 r! I8 r/ G) J5 L+ u6 \
8. 
   ) o9 e# |% V! J& e( Q
9. //double Input, Output ;
   % L/ s" ^' `: F$ Y2 X6 d0 J. W
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    0 K# p' |/ o" T; F1 m- ?
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    % o. l4 c4 x8 D+ p
15. float Kp=0.14;
    2 Y( U5 ~4 Q( S5 Z  `
16. float Kd=0.00020;//23;
    , }" V, P- j) h8 N, Z8 F& w
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    / {( B7 o! a1 w+ ]
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    " j0 u! j1 J. A+ i& W7 [
23. MeDCMotor motor_10(10);
    / D9 ~, k% G3 Y1 Z1 c
24. unsigned long previousMillis = 0;
    $ z" X' \' k) _- [2 \. Y( O6 _
25. const long interval = 1;
    
26. 
    
27. void setup(){
    $ e$ M3 J; M* A+ J1 `* M% A
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    8 G5 U- `! ~3 K8 Y! N
33. 
    
34. void loop(){
      B$ A& f+ U% m+ W1 l! A2 J6 V
35.   unsigned long StartTime = millis();
      }: Q2 f( P! b  T
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    ! I5 t6 v" Z( M! y+ V4 h
38.     ErrorAcc +=  nError*Kd ;
    + Y) \# w4 N+ e5 @; W5 `
39.     ErrorDec -=  nError*Ki ;
    % J  z7 i# H. ?3 N, p6 J
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    7 r- b  s/ Z$ _' d9 Y
43.         nPower += 3;
    * ?+ R/ K9 T3 [
44.       }
    6 I$ o3 R5 J  X# P" v
45.     } else{
      w9 {. q& h7 x/ s2 z* o
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    " k6 z1 ~* K) g# b+ A- P1 O9 f
49.     }
      n$ K& X3 i: _: P
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    6 y, H  X# |% i/ a% r2 u1 z! z. H6 L
52.   }else{
    
53.     motor_9.run(0);
    $ t+ {( l7 `; {6 Z$ {- N
54.     motor_10.run(0);
    + f5 z; X! {4 c: X/ E6 G
55.   }
    $ |. b+ c) E3 w1 l5 u
56.   do{}while(millis() - StartTime < interval);
    
57. }
    9 Y* V) j( a* q$ L+ e) j. B
58. 
    
59. void MotoL(int Power){
    - Q; L& P" c6 g) ?3 `( `. d
60.   if (Power > MaxSpeed){
    0 @4 \8 i% r0 s# V' s. J
61.      Power = MaxSpeed;
    
62.   }
    ; F/ J: C& p" i5 j8 O1 k( g$ `
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    7 F5 ^& I+ o5 J# p* g/ k
65.   }
    9 @" @+ H+ B) _" P
66.   motor_9.run(Power);
    ' S6 f2 V- {9 }6 d7 {
67. }  
    ' t, h; N4 T1 @$ C5 _" ]
68. 
    
69. void MotoR(int Power){
    2 a/ Z" V7 I; d
70.   if (Power > MaxSpeed){
    & L8 D8 Y0 e3 L
71.      Power = MaxSpeed;
    ; U6 s; l5 |* I# x6 G
72.   }
    7 I% T+ p6 x! M* L
73.   if (Power < -MaxSpeed){
    3 Y2 ^6 {5 j8 h0 P" q; w6 z1 W
74.      Power = -MaxSpeed;
    3 H' j- d8 Q3 G% [3 C" |8 _* V& n% B4 s
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

1 E- a* l% O4 D# Y  a( s( a

9 p) `3 K" t5 U4 e$ R
. s3 q7 @3 z1 @# P您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
% y+ u% [0 I, [3 Q
- w( I5 O, ?2 F6 X+ g$ |" M但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

9 X& O+ A: z2 A" e9 I; Z" c
0 s; f) n" Q" ?$ B) t以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   # J+ }1 C$ l2 p) Y
4. int olderror = 0;
   
5. double values;
   $ M5 W: I. X! D, k+ E: K
6. 
   + q/ {5 ^4 K, v% S, n
7. 
   
8. void CNY70()
   6 S1 K5 i; P  j3 [$ }$ a
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ' R- j1 e! R1 W' ^/ J4 l
12.   valuesMM = analogRead(MM);
    5 X2 [5 A& `+ Q: m7 y+ H  {
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    1 O3 U4 e/ i! K
15. 
    ( n7 c6 ?0 e6 k" d5 U3 b9 E
16.   if (valuesRR > CNY70Val)
    5 G; o: Y# x9 N/ h
17.     valuesRR = CNY70Val;
    & H. F; b% I+ s2 ]* [2 O6 a
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    - A* g+ e. m/ V
20.   if (valuesMM > CNY70Val)
    . |3 W4 Q3 b1 \" P, p; @/ J# \1 P
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    + x; J, e' Q* L( H# d
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    ; l) O4 @" ]& }* j0 G5 @5 Q
30. void Car()
    
31. {
    
32.   while (1) {
    ; _- A# g( v" [8 X  _
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    , ]0 ]3 }9 v. `6 Y) A/ T
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    : r  ]' {. V' g/ N/ I5 d, [7 w
40. 
    ( K+ L1 }4 I7 w) W$ G7 O  r7 c; o
41.     if (power > MotoSpeed)
    + u) ~- k" E# f4 n
42.       power = MotoSpeed;
    4 X/ E- b: o- r. F, w
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
      k/ R7 v' w7 Y4 ?  E
45. 
    
46.     if (power > 0)
    1 I+ Y2 H& F5 B$ F/ t' q' }, [
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    - z8 @2 {& V4 i1 T9 _
50.   }
    
51. }

複製代碼

6 {" z0 [( j( N+ Q2 a$ v/ a9 z
* S8 L, v# x1 R! B

感謝分享

感謝分享