PID 循跡自走車範例

1. #include <Arduino.h>
     T; Z. w8 p1 O2 O" z% ~
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   8 _* x' Y" B& a9 L9 u  W$ m
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   ( G7 b# x) y( z2 N" g
8. 
   8 u# K9 @! \; `
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ) Z) h+ t. `9 m! ^/ n9 \4 f) @
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    7 N* j7 T6 `1 P  x5 E
14. 
    3 g6 l+ A# r+ J
15. float Kp=0.14;
    , f- u" {" J1 Z, j; A" g* ]
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    ) O' `/ w% X- p5 P! K
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
      F0 R" D' U# ?% T9 E
21. MePort lightsensor_8(8);
    : l+ [! j) x! w7 V/ w
22. MeDCMotor motor_9(9);
    ' c" X" [* n0 Q( F2 j
23. MeDCMotor motor_10(10);
    ( j/ S  c" N2 h2 k/ l; e+ w
24. unsigned long previousMillis = 0;
    $ s) L; T( q5 K6 N" B  L+ L
25. const long interval = 1;
    ( X! O* Q+ ~1 |
26. 
    - c6 l  J& t5 Q3 {
27. void setup(){
    + e% Y) \7 Z& l4 a/ q2 J6 A
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    % n- ~. s$ k1 ^' R5 v5 h
30.     Error=0;
    . F. t1 b8 M8 A& w5 c0 R
31.     ErrorAcc=0;
    : ^1 o6 K8 {- \& L: t8 G0 N: L
32. }
    2 R5 M) H/ ]$ a% y
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    + Z8 K5 A! g' z+ p, b
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    # z. ^5 C  x4 v) `, Y
39.     ErrorDec -=  nError*Ki ;
    ( F3 a* L3 H2 K: M. D
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    ' p- B% [0 K% `  J1 R- l$ ^
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    9 M' b" W" v' e
44.       }
    
45.     } else{
    1 }/ r8 p$ s) P: a6 G0 ?
46.       if(nPower > MinPower){
    ; u, e( c# g. @/ M) l% W
47.         nPower -= 2;
      \- D3 b6 Q" Q8 G
48.       }
    . n5 {# L( c( H9 C& ^$ t
49.     }
    1 Q( Y6 i# f+ P1 t' ^2 W' R
50.     MotoL(nPower-Error);
    6 v+ K3 A; b8 q; u  O! \
51.     MotoR(nPower+Error);   
    - e' j8 t9 M! D- X! ^" V0 J
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    - W1 j% B2 r0 b  t
55.   }
    ; [" i( ?- h9 L3 d
56.   do{}while(millis() - StartTime < interval);
    1 `9 k8 m+ j& G
57. }
    ' a0 e! ^0 m+ x. q# s- U
58. 
    
59. void MotoL(int Power){
    7 {8 G  P6 E2 m' ^4 }& f3 B+ U6 V
60.   if (Power > MaxSpeed){
    % P$ Y( N6 D2 ?
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    8 \6 y8 g2 D  c  p. `6 r& @9 n
64.      Power = -MaxSpeed;
    
65.   }
    9 _4 f6 d2 ^! x! @6 w( _
66.   motor_9.run(Power);
    
67. }  
    
68. 
    ) r( f1 ?+ @/ C- _0 b6 ?" g6 b
69. void MotoR(int Power){
    4 s- H, I% y5 [- }8 i  B' c5 E
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    + s; R7 d" _9 G1 s% e
72.   }
    # T" ~# m+ K" w) {) x
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    & L/ k% u3 [; Z3 F
75.   }
    
76.   motor_10.run(Power);
    ; {% N6 [. A! n! p$ U' |
77. }  

複製代碼

% Q& ^) a6 t( h  ]您好，不知是否能向您請教。
% o8 Z/ |+ \( Y* J7 X5 h目前和宋修賢老師在處理Ardui Car
% N0 a# i. c, b6 X- D$ M# v雖然已使用較繁雜的方式處理了跑出黑線外的狀況
- q: \" n5 V% Q  O/ F$ S
7 ~" E: _! L- U' B& C8 X但基於想追求更精簡的程式所以還是想請問一下
7 B* I7 [0 k1 O; ~# X8 J4 O$ k3 V就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理

* t( y2 O( ^+ l3 h
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   3 b/ E. K  M% `# Z/ ?3 n2 Z5 T& ^- v
2. double CNY70Val = 1000;
   8 y" V# v# n& D$ B
3. int interror = 0;
   2 b/ v$ m7 h: H0 {
4. int olderror = 0;
   " j8 `$ \" \& E- F$ p9 N! E
5. double values;
   
6. 
   ! V! _$ W1 z+ g4 z# v: O4 F  G/ f
7. 
   ! o( W. h* E/ l# W5 Q  n
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    . p* t6 O# k6 A8 \6 T
20.   if (valuesMM > CNY70Val)
    # w. p+ }, s! {, V' E) t+ @" W
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    ' N9 W5 C  Q) g  }6 U
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    * v- A& F. F5 R. N
28. }
    , W, A, X, v" }8 x
29. 
    # E4 V2 _3 c4 w4 D3 R9 a/ g
30. void Car()
    
31. {
    
32.   while (1) {
    
33.     CNY70();
    : G9 Y7 \% m. s. E# n
34. 
    
35.     int error = ((int)values);
    ) j% B7 ?( x' [/ ~1 X1 z# E, i
36.     interror += error;
    6 W9 m' {8 a! r5 [9 e5 w
37.     int lasterror = error - olderror;
    5 c8 v. C0 I7 D3 _/ g& [, v
38.     olderror = error;
    " o( c* Y8 H; b! r9 s  ^
39.     int power = error / 5 + interror / 10000 + lasterror;
    6 A! t# R9 `( P% h" n7 O& `0 h. X/ m
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    5 S# J+ U4 @4 g+ k
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    7 W$ V0 C0 q# u- ?! s
45. 
    
46.     if (power > 0)
    * M& ?$ h5 \- Z" C7 w( f& h* @( q
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ( i2 j3 q/ B& c- m5 n7 |. c
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    * U0 y5 o! h- `% [
51. }

複製代碼

2 o3 ^" }  W; H' `* [5 I

感謝分享

感謝分享