PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   
3. #include <Servo.h>
   4 @1 p' a9 u; r2 H  J4 p: P6 p
4. 
   * U; M, C8 M; D0 y7 P
5. #include "MePort.h"
   # I- d$ s2 i. ~) B
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   8 p9 c- N0 w  |' C1 d3 ]% }) m! l& L
8. 
   
9. //double Input, Output ;
   8 ]9 u2 P0 B4 |; m4 X
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    + q2 T2 w$ j7 [. I
13. float Error,ErrorAcc,ErrorDec;
    & [; C9 \) i0 b' b) ?: V$ k
14. 
    
15. float Kp=0.14;
    / \1 [* S% J$ s! y- v
16. float Kd=0.00020;//23;
    3 H5 G; b& y  J
17. float Ki=0.000201;
    * k3 U( Z0 ]2 y! a$ h7 k4 b
18. 
    9 ?) L% D/ Y% \0 Q
19. float nPower;
    
20. MePort lightsensor_6(6);
    4 v3 y$ c& a& Q6 l: ?- O4 \- c4 _
21. MePort lightsensor_8(8);
    3 x9 ~) I7 J, J! P
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    # ^1 E% ]: z) U( `* G% \% L
26. 
    
27. void setup(){
    * v9 |, f6 R0 g* s% m
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    ( j2 q0 q6 Z: i- B/ Z, O) Z  A
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    
33. 
    ' E: L" W% I0 i1 W
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    + A5 x3 @- P1 _. Z7 D
38.     ErrorAcc +=  nError*Kd ;
    ' ?/ z8 G4 ?( L* E$ O# y
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    2 j" T( z, e/ q& @6 K1 ^
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    ) V( Q# V% p$ }0 j/ v: B6 h& y! M
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    : X6 u: G- V* \+ }2 m
52.   }else{
    * i& w- o6 H. c) C- `/ O" s0 y
53.     motor_9.run(0);
    ( F6 b  ]) K9 [
54.     motor_10.run(0);
    
55.   }
    3 h( p+ m4 e. R* @( X
56.   do{}while(millis() - StartTime < interval);
    
57. }
    2 r1 q- V* d' u$ K- Y
58. 
    
59. void MotoL(int Power){
    4 o  C0 \) s+ u8 n0 v
60.   if (Power > MaxSpeed){
    1 x! u) Q) H, R2 R- A+ J$ o
61.      Power = MaxSpeed;
    
62.   }
    " [' g6 A' ?9 R  q; G' T
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    $ H/ r+ Z* J$ J% d: ?8 O' X$ p
65.   }
    
66.   motor_9.run(Power);
    ; {6 |+ s3 e8 f, P4 Z1 }4 O
67. }  
    
68. 
    1 w  T* d* l1 K5 x# R& @7 ^6 f
69. void MotoR(int Power){
    . m1 H1 D5 h  {0 Z+ o
70.   if (Power > MaxSpeed){
    % P. S* w3 y' x$ b, ~4 i  h. [
71.      Power = MaxSpeed;
    5 i5 `$ U& N$ \( d1 n' ^' H
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    4 }, @( ^1 I$ J" Y. i
75.   }
    7 _8 c" H7 n2 ]# O. K+ ]0 a" `1 t+ @( S
76.   motor_10.run(Power);
    
77. }  

複製代碼

7 h! K0 |# C1 O3 R9 M
, K2 B# O/ t6 V. o' [您好，不知是否能向您請教。
- ^3 T- n1 t; z" a  ?目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
1 D4 b" n4 J6 |8 Y, D: s" e4 }- _3 t2 R
: M) ^' a5 f  L. ~1 |但基於想追求更精簡的程式所以還是想請問一下
* U) z4 m8 H) B! y$ @. r就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
, @  d9 x9 H2 a1 p/ d+ M, g. V

以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   . f9 M4 l% P/ @* H1 e
2. double CNY70Val = 1000;
   4 L3 q9 z: y1 W
3. int interror = 0;
   & s) V& X/ T- ?. v
4. int olderror = 0;
   
5. double values;
   + r$ u% ~4 l( j  ?
6. 
   1 Q3 e9 N% `; G0 ~6 n
7. 
   
8. void CNY70()
   
9. {
   3 S# B! g, A0 Q! P/ t1 Y+ K
10.   valuesRR = analogRead(RR)
    # Y9 E- ^  }2 P# g/ L
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    6 Z* P" G9 x3 J5 Z$ p6 U
14.   valuesLL = analogRead(LL);
    7 q! t: h2 i" p* a1 G" S$ j6 I
15. 
    ; H6 [6 h  ]4 |4 S$ Q
16.   if (valuesRR > CNY70Val)
    - Q% T) [' ^3 _% [. W
17.     valuesRR = CNY70Val;
    4 v3 D, U7 y1 f. h+ ^
18.   if (valuesMR > CNY70Val)
      ?" D: W# h# ^; p% C
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    ) f# J6 K4 M2 I. C; L3 u0 Z2 x
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    : S4 }$ s1 T! K% P' k3 v
29. 
    ' U6 ^/ _7 E3 q+ M
30. void Car()
    & e* V: k5 d% U" S) w  q# Q
31. {
    / @3 y# s5 {, a6 @0 H8 C
32.   while (1) {
    3 E7 T( z' M) G% N% m
33.     CNY70();
    # V# U  t# X  [' J* ^* X1 S
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    6 F1 V, m1 s  N. t: c
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    : u6 W. T, X7 _2 T
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    - E/ ^4 P. m5 r8 S6 U; W2 A
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    & j, r& K/ x6 k& e: G4 R
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    - T( X, l# B1 b+ N! P
50.   }
    $ h( d2 R) k8 v/ s1 |: Y
51. }

複製代碼

9 r& n) S& \" f( Q% J

感謝分享

感謝分享