PID 循跡自走車範例

1. #include <Arduino.h>
     A0 p7 J) W2 a5 D' ^  [
2. #include <Wire.h>
   5 m2 G( C4 c: W: y- s
3. #include <Servo.h>
   . f4 X9 D0 U/ G5 s6 X* ~
4. 
   " Z* u1 a! D- v
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   : G( V; I: w) G; O8 }  p5 F& D! J
7. #include "MeDCMotor.h"
   # w& F5 C( b! v$ R& X
8. 
   4 v6 k7 z& ?6 J7 J2 x: {
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    0 v! F; F( g- w4 Z  O- O
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    8 q! M8 |5 K7 }6 n
14. 
    ' w) k  X+ H# O# J7 b/ Q: K
15. float Kp=0.14;
    & A0 p/ T! B- L$ X0 f# Z1 D
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    8 ]' P* |2 L5 E& X# q* x7 n5 m$ u
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    ) B. h7 P2 k- U5 P! p) U. ~
22. MeDCMotor motor_9(9);
    ( o. b" P( J7 v1 [; k
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    7 E4 U9 \( T" V  ?& _
25. const long interval = 1;
    5 I1 `) G, S% {
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    . G# t* Q$ @$ g2 |5 w2 O4 ^
29.     nPower = 160;
    
30.     Error=0;
    ; Z) P+ y& l% L1 z1 F* g, S
31.     ErrorAcc=0;
    
32. }
    
33. 
    2 c% D/ F, B9 C' z
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & |. N0 Y7 J" ?+ B
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
      p' X6 I% }9 g- }" M& f. }' w6 O/ G* o" P
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    0 x. l' z5 @9 J; G$ T6 [& M2 Y
41.     if(nError < 80 && nError > -80){
    ! b# Q! x$ F6 k1 Z# P
42.       if(nPower < MaxPower){
    & `0 a9 f7 a, {3 U3 ^" l# t
43.         nPower += 3;
    ) N9 c* o' S. _7 u! w4 d2 _
44.       }
    
45.     } else{
    , e! N1 U( @; z) h
46.       if(nPower > MinPower){
    / M7 |/ L& N( b' O
47.         nPower -= 2;
    # V$ ~* Z1 `( h# P
48.       }
    
49.     }
    6 o* R8 p8 N) X9 c6 `4 T7 g
50.     MotoL(nPower-Error);
    , ~3 i" d) R7 L' h( V/ N
51.     MotoR(nPower+Error);   
    % Q- n3 G4 w, X' ~
52.   }else{
    + M8 K6 Y: M# u! u+ {
53.     motor_9.run(0);
    : l2 U' |6 W. h
54.     motor_10.run(0);
    
55.   }
    ( N9 s3 ~. _" {0 N+ D$ M( K
56.   do{}while(millis() - StartTime < interval);
    % g- u1 Z% K7 \5 R& R9 i( {- b+ d& M; h
57. }
    1 I; U* v2 S- i" b; I4 i
58. 
    3 ?( ^$ V  b, j5 r: N
59. void MotoL(int Power){
    + n$ s' q, m' n: D$ \2 O+ z
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    + o+ R  z  d/ T0 v) d8 ?
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    1 p/ \- F% o1 T" Z5 `$ u+ }4 p
71.      Power = MaxSpeed;
    - I: t% i$ z" F& [
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    " T7 j3 ^2 C: K% b
75.   }
    
76.   motor_10.run(Power);
    9 ?5 Y) U! u+ W; p& U$ u
77. }  

複製代碼

5 H, f5 G" m; m( B

, y' y  S0 ^, I4 e
3 ?0 D, v" `' X* Z3 P您好，不知是否能向您請教。
8 h( S# H* z% m+ W4 v7 s目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
4 A) n/ `. U1 T+ U# W$ Z; s4 V+ i( |
但基於想追求更精簡的程式所以還是想請問一下
8 W4 x! T% }$ [9 t就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

; c" V, z1 L% }2 }
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ; o, q2 O% I+ M% v' U7 q% o& l
2. double CNY70Val = 1000;
   1 k' E, |: D: C' ^: d9 T( @& O! _
3. int interror = 0;
   
4. int olderror = 0;
   4 [+ i8 D& {: ]1 |; v
5. double values;
   7 W, y# J  F, ^! r# p' j! _  M. `
6. 
   9 S7 R7 E( z9 l9 B2 ]
7. 
   $ I6 ]2 W+ O9 k
8. void CNY70()
     M2 g6 O% g3 p9 ]$ W4 {
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    0 x5 N+ z( W2 e6 f, j( ]6 V
12.   valuesMM = analogRead(MM);
    9 K$ l$ Y/ S% z& {. T! V
13.   valuesML = analogRead(ML);
    & N5 V  ?6 r5 L1 q9 q
14.   valuesLL = analogRead(LL);
    
15. 
    - i8 n* A0 O- p6 Y4 W* l- W
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    8 }5 U/ y+ a$ {7 b
18.   if (valuesMR > CNY70Val)
    0 I# I9 Y" F9 u
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    ' y' h( N/ |, k' h! y6 ?% B" E
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    ; {  J2 _  R* n+ W6 _
25.     valuesLL = CNY70Val;
    
26. 
    8 H. i/ a, `0 {1 W, U
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ; J* Q# w/ s- T2 `! R4 R7 s, d
28. }
    
29. 
    
30. void Car()
    $ a0 m" \; ]+ z2 t5 _( z9 O2 v8 h
31. {
    * z! L4 W! a) n. ]
32.   while (1) {
    6 K. S& [% Y/ L- v# R7 W
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    ( \. W5 h8 q; T- j4 C5 @
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ( f! s1 n% d2 C2 W- Z  [; z4 n( y* ]
44.       power = -MotoSpeed;
    2 ?6 r5 `" `* v+ D( x/ Z
45. 
    " \3 k. x, |: [  _+ [( y
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    - M# R- @0 R4 O  O5 S* t3 ]
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    5 N/ t- y* r+ h. ^" }' H
51. }

複製代碼

! H7 t7 @7 m3 I8 L) Y
" t! f0 Q- d7 y

感謝分享

感謝分享