PID 循跡自走車範例

1. #include <Arduino.h>
   0 o2 E3 V2 }+ [/ r
2. #include <Wire.h>
   
3. #include <Servo.h>
   7 g3 o1 x% x' ]+ R% Z5 J3 q
4. 
   
5. #include "MePort.h"
     l# p: f* _0 q
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   8 o9 @, d! Z$ A& e$ E
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    " _6 _: S) h2 M4 S  ?) w
12. float MinPower = 120;
    : u) o3 Z! _9 _5 N% h
13. float Error,ErrorAcc,ErrorDec;
    
14. 
      i6 J$ d$ j/ R5 z' [% l' e: L
15. float Kp=0.14;
    " B3 r# A5 {; T% R% u9 e0 I3 J
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    1 ?" \0 X/ w  A: D3 x# r% g( X
18. 
    
19. float nPower;
    + _" R2 y: }* C/ [- q* ^
20. MePort lightsensor_6(6);
    ) e, V: w2 X$ _5 t4 ?; U+ W' M8 b
21. MePort lightsensor_8(8);
    , R8 P5 |; ]% B7 Y9 C+ ?# Z# a
22. MeDCMotor motor_9(9);
    ) ^- ]# |8 e+ G* y. X1 {
23. MeDCMotor motor_10(10);
    " m$ m5 g9 }  @! X) W
24. unsigned long previousMillis = 0;
    ( u0 S7 |! L- R0 s5 t
25. const long interval = 1;
    
26. 
    
27. void setup(){
    4 K: E$ d7 B. e* Q3 Y3 j. l# D" ~
28.     lightsensor_6.dWrite1(1);
    1 J3 m/ E! O  n* h6 ?
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    1 H4 S# T* b7 P* Y
32. }
    : c: h- V9 _2 {8 y+ z
33. 
    ( q# s+ c# y- n9 V
34. void loop(){
    ( k( L4 T0 U; k! g
35.   unsigned long StartTime = millis();
    + C3 L+ Z$ u8 a' K
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    2 h$ q: z& A/ _5 S% ~
41.     if(nError < 80 && nError > -80){
    3 p1 H5 Z0 T5 u9 A# r4 }* n: A6 T
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    1 z; G& j: s' J9 h8 ]
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    ( l! B" N& Y* h2 J7 [
47.         nPower -= 2;
    
48.       }
    
49.     }
    
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    # |* v8 P8 G9 Q, M1 ?
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    $ b- M. r9 I/ y: \+ T$ \: i5 U. |
57. }
      E. j2 |. a& D7 z) V8 s
58. 
    0 f. I8 m' C1 _0 o$ M5 T4 y
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    $ V  X5 F+ L+ x1 i) ]
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
      l& D( B' f! d. I/ {# `& S, o9 y! Y
66.   motor_9.run(Power);
    4 w. q1 K" f9 i( ^  V: N% L
67. }  
    + P, L8 J7 t0 F5 u
68. 
    2 z/ k- Y( m2 G0 Q3 Y* v" U7 U
69. void MotoR(int Power){
    0 v" m. \  b# M8 j; t% c8 [% {0 z
70.   if (Power > MaxSpeed){
    + G9 C9 f8 t0 A9 ]
71.      Power = MaxSpeed;
    + l$ I5 o1 O3 h6 l; `
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    ) H( O; A& ?& s) ~5 K0 z; h: ^- c
75.   }
    3 X6 c# Q; j" X
76.   motor_10.run(Power);
    0 v  O+ `, n) I$ x) y1 I; H
77. }  

複製代碼

7 n8 N! `7 m$ M0 J( q, H0 o2 A您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
6 y' D+ ?" a, U) g1 r0 U2 M8 q雖然已使用較繁雜的方式處理了跑出黑線外的狀況
( Y+ N% O& g, e: }( \0 B' t" B% P
( R. W0 J3 B, w# c- s3 I但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
  G) r! _# q/ B不知道您是否願意教我可以如何處理


+ r# |7 \: D% ^( @' t% E* \: r以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
     V0 A4 p( B' \  l! F! X
2. double CNY70Val = 1000;
   
3. int interror = 0;
   
4. int olderror = 0;
   
5. double values;
   % Z* }5 J4 M+ u
6. 
   
7. 
   
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
      @; H/ B+ a4 s& c
11.   valuesMR = analogRead(MR);
    $ a3 v) U5 ]) V! B& t
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    ! E( H4 l0 ^) f+ Z# C" Y0 W
14.   valuesLL = analogRead(LL);
    0 l4 g+ L2 `" x; O: ]
15. 
    2 k! x; R. F5 e- e2 K( V4 q" I
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    * I2 l# E* k; \6 {* s, j2 d2 A  x
20.   if (valuesMM > CNY70Val)
    ; o" M8 n; o2 M3 ?7 _; O$ w  N
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
      H9 ?- C. e- c
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    + o- L0 h/ D$ |" k2 b0 g
29. 
    
30. void Car()
    0 t8 ]7 K% u1 e# D2 m0 f
31. {
    
32.   while (1) {
    . V1 e4 U) m) a; e
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    : h6 ]( H5 y2 h0 d7 k: M
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    4 |! f; {1 ]  l5 _) {' n3 H
39.     int power = error / 5 + interror / 10000 + lasterror;
    , M* u8 P' i5 m
40. 
    3 c1 h5 b- x, R& Z8 Z* K
41.     if (power > MotoSpeed)
    1 u4 Z9 M: h5 w) {: z! G
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    * d# Y$ Z) A, k* r5 \
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    0 H; e3 J( Z  [
50.   }
    # t9 ^7 \* \+ W) L8 z/ Q5 {% a( y& n
51. }

複製代碼

6 {8 Z. e/ b9 b

感謝分享

感謝分享