PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   + F8 E- h  e/ G* ?6 @
7. #include "MeDCMotor.h"
   
8. 
   8 c) B0 O9 ?' }+ S. ^
9. //double Input, Output ;
   ( o$ ~$ V; S) T7 x, ?! h! K
10. float MaxSpeed = 255;
    # F7 M3 x& m" u. s5 _
11. float MaxPower = 180;
    - |$ {# O- E( q5 A+ o! X% s
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ' R1 c9 o' g* c* r% f# ^4 r8 H
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    0 H6 u+ G/ K+ S0 T" ?; }" A, S
19. float nPower;
    
20. MePort lightsensor_6(6);
    * Q! |( R4 b$ Q1 T: |6 I8 T0 r
21. MePort lightsensor_8(8);
    4 W4 a0 b. U/ e
22. MeDCMotor motor_9(9);
    , p5 p9 P7 g4 `0 p% @
23. MeDCMotor motor_10(10);
    " T6 ^/ ]) C% M% V
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    4 r7 \+ D+ o4 X9 O1 i: s. Y8 U
27. void setup(){
    & B- d8 k& H9 c. {3 F% E
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    - j9 X/ S2 P. R2 C. y+ D
32. }
    . P1 l- ]) v- D9 `" L9 v
33. 
    5 u. m6 z, v/ L- }, }
34. void loop(){
    
35.   unsigned long StartTime = millis();
    & [' i7 [( F$ c" @6 \. n
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    9 p# }+ e) D& d. p/ m
43.         nPower += 3;
    ! y5 m; z( @4 N
44.       }
    / @% G- J$ S8 z% I7 o9 {
45.     } else{
    5 a1 ]0 S- C* \2 l
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    % s) @, ]) p& P9 s8 q6 H6 L
48.       }
    
49.     }
    * @5 F; w- G# }, u- \8 D0 H
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    / s6 A; c1 r4 J2 i* f8 z
55.   }
    9 E- d7 X  _% y$ {
56.   do{}while(millis() - StartTime < interval);
    
57. }
    " Q$ `, N8 r2 B* q: J
58. 
    
59. void MotoL(int Power){
    . V- E1 m- F( g9 L  T- s( G. Q5 n3 m
60.   if (Power > MaxSpeed){
    4 j3 k: ?* d( s6 I
61.      Power = MaxSpeed;
    3 L) B5 e) q) |  V; ]
62.   }
    
63.   if (Power < -MaxSpeed){
    5 B4 w  J) N  h3 L" L* n
64.      Power = -MaxSpeed;
    $ Z5 f/ J, V7 r+ l
65.   }
    
66.   motor_9.run(Power);
      f8 Y5 A. X. G5 |
67. }  
    
68. 
    9 V: A2 @" _5 b
69. void MotoR(int Power){
    ( w" q) K) F4 }( V
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    + D4 c% S& `  t1 b
72.   }
    5 W# T: Q0 d7 U
73.   if (Power < -MaxSpeed){
    $ N( z* J; t# ]% `- \6 U/ s; X
74.      Power = -MaxSpeed;
    
75.   }
    . D: [3 Q3 s/ _. W
76.   motor_10.run(Power);
    
77. }  

複製代碼

1 j( ?3 [( F, `7 n

3 n% l% n  [" O% h5 N5 u
您好，不知是否能向您請教。
/ X. d0 z* M3 A目前和宋修賢老師在處理Ardui Car
) L2 F$ ?/ w3 E( [( @( d雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
, ^% O) d/ c0 o就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
3 n3 K5 g* s; k* c; A0 z6 C- b! s不知道您是否願意教我可以如何處理


+ W$ g1 d  h0 f# V以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! [- z" j* f1 E6 l
2. double CNY70Val = 1000;
   # m# E/ S) s% A2 t. |1 H# j
3. int interror = 0;
   
4. int olderror = 0;
   % p. U2 D. Y! [) x  y
5. double values;
   
6. 
   ; J+ \: T! c* {
7. 
   
8. void CNY70()
   1 q1 k# h( J8 K; f+ [8 u
9. {
   % V) ]. e9 D& a6 i& T7 E
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    1 G& w4 R% T! Q/ g! o5 ~; N+ B
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    $ z) n9 n3 G) N! F
15. 
    
16.   if (valuesRR > CNY70Val)
    ) ]2 X% G1 k+ R+ `2 s) h8 L- T
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    ( G$ C9 W0 {% Q5 }6 i3 ]# v) w# b
22.   if (valuesML > CNY70Val)
    - A% r0 v$ k0 ~; q3 v5 ^. R
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
      I1 O" A0 O: T5 m
25.     valuesLL = CNY70Val;
    . v; ?3 ?0 e4 I$ G
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    ! y- X; x0 d: D" a9 n, l+ u
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    ( K8 c! [5 U) ~0 K% S5 U$ ]
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    # c1 q3 B8 M& }7 K' `! `( M0 R
40. 
    & y' d2 U# o& o% _; G$ f& @
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    % z, E: J1 o' B7 C' L( P! r
46.     if (power > 0)
    ! Q8 W( C2 H7 R$ `+ A  }; T) Y
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    0 b5 o+ i" N4 y7 s
49.       Speed(MotoSpeed + power, MotoSpeed);
    # U% S, I& ?4 K/ M, o, m
50.   }
    , j6 z  Q$ z5 n$ S3 I' R- X
51. }

複製代碼

感謝分享

感謝分享