PID 循跡自走車範例

1. #include <Arduino.h>
   2 @/ h% ^% N" _+ J
2. #include <Wire.h>
   6 o, G9 J$ Q1 L
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   % {0 z( k! }! N+ u' ^! z
6. #include "MeUltrasonic.h"
   8 r+ D% m! n, a, D. l
7. #include "MeDCMotor.h"
   
8. 
   9 ~7 E- A+ ~9 U1 |* t" n2 P- D
9. //double Input, Output ;
   5 U4 B4 D& T% |1 j
10. float MaxSpeed = 255;
    / R% v* y1 j  N# {/ @0 k- m3 _
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    , r% [5 s( A4 F8 R- @; t  g$ v3 i
14. 
    $ J7 G3 _: S/ D9 T3 {* o0 \4 F( t
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    3 I' H+ Y7 J; X3 I& Q  E
17. float Ki=0.000201;
    8 V5 W+ o2 q# m
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    / a' A* i, p% M; k; u6 D6 L* Z8 B0 `
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    - G, Y: y/ L4 i# A' Z+ t
27. void setup(){
    & T% X+ N2 Q  d3 k9 H( u% |
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    . P1 L3 g, N8 w
30.     Error=0;
    , C. @- Q( W; y7 j
31.     ErrorAcc=0;
    : K% E( V6 }5 N# m( F5 s) n! Q+ E
32. }
    ' {* n- I9 e# H# P
33. 
    ' d" R6 o  t- ?$ S3 G$ q
34. void loop(){
    
35.   unsigned long StartTime = millis();
    4 d1 h: `/ }* X! {6 [- B
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    + W' Q  ]) M% i( L$ v% t# G# ]7 d
38.     ErrorAcc +=  nError*Kd ;
    + P+ ]) b# a1 t2 w+ j- {7 L
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    $ x+ _  \7 K, P( n3 F; y$ ]
43.         nPower += 3;
    
44.       }
    9 V3 m% e' |3 @0 O5 l
45.     } else{
    , o$ z  T$ d' W' N; {, C
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    ; U+ T8 b# a0 M: V/ l% m$ b
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    . G+ I( l! q- P; N% O
52.   }else{
    
53.     motor_9.run(0);
    % N" y7 E6 V: ~' w$ t& v
54.     motor_10.run(0);
    , t; o1 H' D5 }
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    . [9 q! T/ P3 k
59. void MotoL(int Power){
    . q. p( N3 g2 k1 M3 R: Q
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    : a- P' t( l9 I
62.   }
    ( M) T2 `1 }$ E& m3 G
63.   if (Power < -MaxSpeed){
    7 y3 \9 M5 h. p, H
64.      Power = -MaxSpeed;
    
65.   }
    2 `0 r7 z! K( l  F4 F( Z
66.   motor_9.run(Power);
    
67. }  
    : l  [9 y8 v' `) m; T- y
68. 
    0 r3 C; \# u' D( w6 ^2 t% H& C% S
69. void MotoR(int Power){
    ) j$ `7 j" O2 G5 q- ~: o9 T( J
70.   if (Power > MaxSpeed){
    , G1 I/ Y. S7 h$ {: g
71.      Power = MaxSpeed;
    
72.   }
    5 G6 Y4 g" v5 q$ a0 |* P3 {+ C4 P) f# w
73.   if (Power < -MaxSpeed){
    - v3 l- {7 Y; |4 H2 V( @
74.      Power = -MaxSpeed;
    / S) F% f# u) J/ v2 u
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

! a# X  W' T4 ~$ Q8 y
您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
9 D+ \6 Y& c! v# |1 @雖然已使用較繁雜的方式處理了跑出黑線外的狀況
( U& g8 n+ W6 B; d
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
& j: _1 B8 b  o! B2 w不知道您是否願意教我可以如何處理


  }8 o' q$ t$ c以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ) d% U* |# k9 Y9 C9 t/ o
4. int olderror = 0;
   , q1 m* h, D( A4 k6 c/ m: Q3 Y
5. double values;
   5 S3 h+ F7 E* i/ U
6. 
   0 Y6 p. w/ g. l( [0 D5 y
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    8 r; q& w8 q2 b
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    , k' v8 a6 z" I0 R  g2 c* T  A  f
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    , p. C0 z, s3 B4 ~' y- t
23.     valuesML = CNY70Val;
    " [+ Z4 z9 E) \; [4 L
24.   if (valuesLL > CNY70Val)
    % H$ R: G' @7 S* `. a3 R# `1 M
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    " K% F+ u/ A5 s# a- j; O  ~
30. void Car()
    * a, S; C' w2 |: _
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    ( X" L9 d+ d3 i4 g
37.     int lasterror = error - olderror;
    4 l6 t" T7 x9 i+ {2 k0 Y" Y
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    1 M/ f! X" b: _& H
40. 
    ( E: A7 a) L3 B- j, x
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    6 \% R$ {& j7 X, E( K" E; A# i
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    , Y/ K/ |- ?; D. \% v6 L: p
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    % @, n# Q& u, b# ~: [
50.   }
    1 u" [9 V4 U0 ~
51. }

複製代碼

' ^5 B, j; E- y5 c" B- ]/ h5 z" Z8 S

感謝分享

感謝分享