PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   3 N+ e  [* T, Y1 N. s
5. #include "MePort.h"
   6 E" j. a4 j7 W  _8 a* n% L% M
6. #include "MeUltrasonic.h"
   . Z" {( \! s- a& {* r9 E: K5 q
7. #include "MeDCMotor.h"
   
8. 
   6 U6 W' f) @: ]
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    7 q) ~, ^% b. c8 \( w2 T' ^
11. float MaxPower = 180;
    
12. float MinPower = 120;
    6 W  O0 O7 T' U% V1 B5 a9 ]- r
13. float Error,ErrorAcc,ErrorDec;
    / N2 V# k  M' _8 K& ^
14. 
    
15. float Kp=0.14;
    ( b  }# ~. b: z& w% ]3 Q
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    4 z: d& V! R. V+ w( F
20. MePort lightsensor_6(6);
    6 B1 D; A; l" @  ?
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    7 H  U% k9 ~4 |2 @
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    + O* r% {! ~1 C3 E/ R
25. const long interval = 1;
    
26. 
    
27. void setup(){
    * |, x" s$ {( u. D
28.     lightsensor_6.dWrite1(1);
    2 `" r7 V  {- d
29.     nPower = 160;
    
30.     Error=0;
    , s+ m0 a# o$ v
31.     ErrorAcc=0;
    . l4 P( {4 k" F* A: j
32. }
    ) a8 L- S$ }- C$ {, y- F& v
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    4 y8 D" I% i, u' t7 G
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    6 J" l6 @* }8 B8 B4 P2 v/ G
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
      c. v; b. F: f6 P) C1 w1 S1 y$ |% y
44.       }
    0 g. ^# ^) R5 s( U: \( C' d# f7 x0 \7 q
45.     } else{
    
46.       if(nPower > MinPower){
    + p; F$ p$ ]: b1 |9 G9 |
47.         nPower -= 2;
    
48.       }
    ; \5 U. h6 ]0 G+ K  }, P! g
49.     }
    " X5 |' g* ?6 ]: B
50.     MotoL(nPower-Error);
      u/ z8 Q; E% ~, B# r
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    / T# f% d) h+ w) D! K' G
56.   do{}while(millis() - StartTime < interval);
    # |3 j; ]; f& I4 V& i: [4 f- r" B
57. }
    " k- ]4 }7 [9 Z
58. 
    ) H( c6 E- x/ S" I. M* w# N# }$ E
59. void MotoL(int Power){
    * w2 f: U( Q. _2 s* _# t
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    % N4 _( ?) R. U, j. b# Q+ l
62.   }
    % P/ t/ f1 H/ F/ Y
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    4 E! y8 {9 ]8 O! D
65.   }
    . @3 ]5 z* }4 ^4 `( H
66.   motor_9.run(Power);
      ~- x; H; E/ [  O3 B
67. }  
    3 Y8 e2 ]7 Z' U" Y* Q- e! }/ A
68. 
    
69. void MotoR(int Power){
    ! ]( h( y  p8 I) ]- O4 x0 h/ T6 I0 V
70.   if (Power > MaxSpeed){
    7 j: J1 b& |+ K% _* K
71.      Power = MaxSpeed;
    
72.   }
    ( o7 c, X2 L; s& m
73.   if (Power < -MaxSpeed){
    - ^& J* n+ T% K1 Y9 D9 b  q
74.      Power = -MaxSpeed;
    
75.   }
    $ }4 k  `% C; L: i4 H! n) n9 r
76.   motor_10.run(Power);
    . {0 Z( h7 ~9 k5 r5 \  }) f$ o; I5 U1 e
77. }  

複製代碼

5 F5 R) ?3 J5 [8 J2 d5 `

% S! m6 A% \" o7 a' g8 A2 n
+ h0 E% B+ @5 v7 U* C! Z1 q您好，不知是否能向您請教。
& i2 \  l+ F; V# X+ k) k目前和宋修賢老師在處理Ardui Car
. z0 m% k" N6 J# v7 P9 B雖然已使用較繁雜的方式處理了跑出黑線外的狀況

+ `9 N( r! q9 G6 F5 t# {, S$ c1 |但基於想追求更精簡的程式所以還是想請問一下
6 S. g$ l* K; p' u+ s& K就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! ~, e% [/ G3 Z9 l8 P
2. double CNY70Val = 1000;
   1 M. {# O/ n! F% j
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   8 x( Y" [% t! G* g  ^  ~- E
6. 
   
7. 
   " ^8 [2 x, F2 j; u% ?
8. void CNY70()
   ; P/ P4 s7 s% W" H
9. {
   + F4 r$ H% R! k. m3 E  R& _$ q- I
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    6 z+ s4 l! {+ m8 b+ K
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    $ ?' h1 W8 }5 U6 J' C- `
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    * l( _# x! N6 O% u( n
18.   if (valuesMR > CNY70Val)
    / y/ M! V, V4 I3 g! D8 A* p8 K
19.     valuesMR = CNY70Val;
    , }2 M- N; Z3 ?0 C, [( r
20.   if (valuesMM > CNY70Val)
    % t- B7 |3 t! m: O: d) [/ k
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    * s0 W+ @' N  n
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    5 [( h3 O3 @$ D. H- N1 Y
25.     valuesLL = CNY70Val;
    
26. 
    - M# B, X& b" h$ _2 ~+ R7 c
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    8 n( \. Q+ [# u0 \' O1 V# B
28. }
    . T2 t2 s' b" k2 g8 m- [
29. 
    
30. void Car()
    
31. {
    / g3 o4 T/ L1 s: i; Y0 j8 b
32.   while (1) {
    * h/ A0 \/ r7 K7 y1 B
33.     CNY70();
    
34. 
    : ?- Q* u) o5 D: X
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    # o, i# H9 j5 g6 L. H9 n
40. 
    
41.     if (power > MotoSpeed)
    + Y' U) a, R+ w" A- r
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    8 w  J0 m! a4 }8 [+ @0 k2 h0 l+ K
44.       power = -MotoSpeed;
    
45. 
    2 C; o1 M  Z7 h5 G1 O2 q
46.     if (power > 0)
    % ~5 C- i6 M" O. L2 L- A- X
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    . \0 p, A6 J4 }
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ) n6 [& D( J6 W( G
50.   }
    
51. }

複製代碼

感謝分享

感謝分享