PID 循跡自走車範例

1. #include <Arduino.h>
   ' p" Q4 K& _. o) T
2. #include <Wire.h>
   & }) w" q# ~. N1 C2 I4 Z
3. #include <Servo.h>
   
4. 
   ' B. E! ~& U6 e  \/ U
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   4 w8 T6 l: i1 k' H
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   5 J6 A2 k9 p+ A. R
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ! j9 v- X9 o' G# }  F9 M
12. float MinPower = 120;
    " ~; w0 h0 R& F9 G
13. float Error,ErrorAcc,ErrorDec;
    ( M- I/ y# j7 r+ ~
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    % n9 V; v9 l6 H6 A! g. B% z$ Y
17. float Ki=0.000201;
    3 p0 ^: R4 U6 t5 _/ ~  `
18. 
    , x5 j* \; O$ Z
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    2 {- I/ D6 y- ]" u
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    9 L& j5 Q, ~# h5 w' t7 E: q
29.     nPower = 160;
    4 t2 S6 }2 R3 L1 T
30.     Error=0;
    : F7 X9 e. D8 G7 v2 i
31.     ErrorAcc=0;
    
32. }
    5 ?0 z7 j, X2 ~9 n5 [& w
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    " s. c% F3 O2 R! c9 c/ P
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    9 z+ s$ z! D, \8 A, |  I
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    8 M0 r+ j6 G! I  G. y, W) ]
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    / ?% L  v0 @! @$ F- \
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    3 T$ d  y8 S6 U1 W+ G
47.         nPower -= 2;
    - Q6 n% f% Z: e5 V, V7 V
48.       }
    ( |+ y2 P0 l1 p! N
49.     }
    
50.     MotoL(nPower-Error);
    ! m0 Z& O6 l, R) R% I5 M
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    ' Y, P$ p. S. E) g" M% Y3 n0 B
57. }
    
58. 
    
59. void MotoL(int Power){
    - _9 l, {5 z1 \# f4 k& l# L* @  Q
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    * A6 o( J3 ?6 @$ k, {  O% a% r
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    / d5 i5 S7 `3 E7 E) ~2 L9 o
69. void MotoR(int Power){
    4 s9 I( v8 n! R$ J! @6 o& T
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    - F: O& d0 x3 l
72.   }
    * U+ T9 ~0 [" E3 ^6 p
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    2 K4 P( e$ I  M0 l. [$ m5 D+ |, u2 O, ]
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

8 i# K1 J7 B* G8 W- ~8 V# k! m

" f" O- \, l2 l6 y/ c您好，不知是否能向您請教。
5 P2 i# [* E: e目前和宋修賢老師在處理Ardui Car
$ z# o0 K* {. F4 v; z2 V' k雖然已使用較繁雜的方式處理了跑出黑線外的狀況
; }* u4 }2 c$ q% X6 W% V
但基於想追求更精簡的程式所以還是想請問一下
$ H4 a1 f+ Q+ Q: I& B7 `就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
. c0 }1 |1 E: r1 }不知道您是否願意教我可以如何處理
7 x6 k% S" Y/ P2 R: T
! j' y5 K$ F, |- \
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   & f: E5 W/ X' T) \. s% p4 u" M
3. int interror = 0;
   3 }6 Q- b% f1 X0 ]
4. int olderror = 0;
   
5. double values;
   
6. 
   7 ]' {" \9 F; _2 r
7. 
   , ~/ M8 v. g' w! x! d
8. void CNY70()
   
9. {
   ; p) n+ S0 R5 s% h
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    ! d$ m; ?, m4 |
12.   valuesMM = analogRead(MM);
    * \/ _) x. M; V9 i* c2 ~- D( f
13.   valuesML = analogRead(ML);
    7 k: C9 `, ]) ?) [
14.   valuesLL = analogRead(LL);
    ) W$ |7 `2 o* u( ]6 D6 B% {) |
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    " E% d) L1 ], n9 h) t, e: L
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    9 e  q3 d, y" ~6 D8 T$ i
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    4 r1 h6 Q0 Z2 P7 Z+ M
25.     valuesLL = CNY70Val;
    
26. 
    % |0 h4 n3 K, h1 Q- J7 e% W2 L
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    . @- N  E% k. b3 |+ `. K
31. {
    ' W$ ~2 [+ k+ \! q- V5 [0 |" F7 v+ B
32.   while (1) {
    , C0 f$ w0 h" U0 e, x' k! P: b6 V
33.     CNY70();
    + @& D" s/ I* Z  J
34. 
    
35.     int error = ((int)values);
    7 n0 Q% x  I% |2 k0 S0 Y
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    7 i# U8 o1 h' i7 S
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    + g& w% d+ `" ~6 W' J- T$ W$ S
45. 
      g' L6 p! E# t5 N$ e
46.     if (power > 0)
    8 r* a2 o" g4 }6 m3 s& g
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    3 b8 E% @& @* Z; {5 i, J3 L6 h
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    # A# U2 P8 \; C# G* e+ {
51. }

複製代碼

! c; P4 D9 x& ]5 x, z7 ]. s

感謝分享

感謝分享