PID 循跡自走車範例

1. #include <Arduino.h>
   6 l+ g" O. H/ K* i3 |
2. #include <Wire.h>
   * ~. a2 K8 ~; |/ L% w
3. #include <Servo.h>
   
4. 
     ?$ z$ K0 `( V7 ^% u. @8 A
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    / J. i# Q1 |: l* G8 Y
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    * W0 H* @) X6 N* H
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    ( ]$ g! I3 H' K8 L; a
20. MePort lightsensor_6(6);
    ; q4 o, ?1 j  _/ j8 Q- l8 e
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    : N0 c; |( J- J8 U# D- R* z
23. MeDCMotor motor_10(10);
    - A; O4 ^& V3 l7 f- m7 J6 r
24. unsigned long previousMillis = 0;
    7 c+ C6 k* q7 b8 k% y3 X
25. const long interval = 1;
    ' p- Q( Z; R- U
26. 
    4 u. m6 d  C, n: t, |
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    0 K0 O5 H$ e7 Y
29.     nPower = 160;
    
30.     Error=0;
    " }3 |, o8 k& o8 P5 m- |1 ?) P
31.     ErrorAcc=0;
    3 \$ ?4 k/ U: ~, h' @
32. }
    9 u& g, D) y' G% t
33. 
    
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    : W& ~- K& U# S' W
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    9 @* E7 m, ~! q9 k8 `$ ~
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    0 f3 a& L6 {  A1 m
43.         nPower += 3;
    
44.       }
    " K4 `8 K0 o! Q1 d4 f
45.     } else{
    8 \8 C  ?: A% H4 J4 j0 o$ x
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    ) C& i, H$ V/ x* ^/ u8 }" J
51.     MotoR(nPower+Error);   
    . u2 }0 @; f: J, X1 _) m+ D
52.   }else{
    % ], `. v# a( u7 K, r' q; c
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    0 {4 Q5 W2 h3 r- G: w. W4 O2 p* |$ E
55.   }
    : p8 r+ e6 F0 Y0 I1 v3 P
56.   do{}while(millis() - StartTime < interval);
    ! W' f0 @  o0 s
57. }
    0 A$ Y: D% m, {6 k9 h1 y
58. 
    
59. void MotoL(int Power){
    3 G- x2 n& ?) y1 Z( a; Q
60.   if (Power > MaxSpeed){
    ) C; J' A8 M, ?; C3 T9 Q
61.      Power = MaxSpeed;
    
62.   }
    & |7 V4 a9 Y8 Z- b
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    ) F% j/ ?2 e6 e, I
65.   }
    + o& N' N9 j8 ]* t4 ^0 B
66.   motor_9.run(Power);
    / x% j4 }+ L% h' `/ g6 L- H
67. }  
    
68. 
    
69. void MotoR(int Power){
    3 J" Z/ D# E7 \6 l6 b3 y* |" R
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    + a1 C7 P" j. e' ^# ~
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

: O4 v& v8 U! b) B& T

您好，不知是否能向您請教。
4 r  T+ P! \& u9 I5 A6 C目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
+ ]$ g9 O) W2 {" i
% S& A. w: u4 M7 C但基於想追求更精簡的程式所以還是想請問一下
& l+ L1 U- f  Y0 u6 @$ J就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
3 f/ ^+ `* d, d# I$ h) R( O不知道您是否願意教我可以如何處理
- Y0 y; R- j6 h

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

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   7 m% e6 t5 j" _$ q1 ~
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   
6. 
   6 Q% K* c. o: w
7. 
   
8. void CNY70()
   % k; A3 W+ `+ |9 Z, `
9. {
   5 j2 I9 z+ Q# _0 ?3 b9 ]
10.   valuesRR = analogRead(RR)
    ) a% x# X& l" l9 x/ x1 c( O
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    7 E( e( G  ]% b- p4 w1 [. x: R
14.   valuesLL = analogRead(LL);
    
15. 
      n7 N7 e4 s! Q1 I8 [, ^  V
16.   if (valuesRR > CNY70Val)
    / z" ~$ y- l$ k+ `
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    ; |- L* ?* J# D) o  Y8 i( u
19.     valuesMR = CNY70Val;
    * d" i$ w+ k( ^
20.   if (valuesMM > CNY70Val)
    2 D" Y8 v; e/ d, p( n+ o
21.     valuesMM = CNY70Val;
    ! h( a* o, R+ R; @  c5 L
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    2 {1 V5 O; L" `4 [+ Y, R
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    
29. 
    
30. void Car()
    
31. {
    . U6 e: s! W, j  j& j7 s: d
32.   while (1) {
    . X) h! b. K$ o1 g( v- W
33.     CNY70();
    " [8 K' s: O0 ~- k& Z. L  [
34. 
    : s6 N8 {9 [& i7 H+ v! J
35.     int error = ((int)values);
    ' |1 [5 E5 i/ W8 ?
36.     interror += error;
    2 d( _% K$ I( `$ T9 x" P# g
37.     int lasterror = error - olderror;
    $ N- n! P$ _; g; t
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    ) [/ H4 \4 `% Y- L# q- Y
40. 
    / }# t$ ~' H* S' |+ X
41.     if (power > MotoSpeed)
    + g8 d) P; |( z: S" x0 @5 C
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    . W7 D: ^# X9 j/ D6 V1 j
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    3 B; [& P  Q+ @8 T! ^* P
50.   }
    . ?1 [8 H3 N8 t8 L  |- }* p
51. }

複製代碼

2 o6 p7 F: r$ d! O
$ u; j5 q8 e( {% V& q

感謝分享

感謝分享