PID 循跡自走車範例

1. #include <Arduino.h>
   - X5 R! y3 z" w, J
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   % a- ^1 I6 O2 f% V% Z) U2 u
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   6 `! z9 W+ G' G  g- T8 ?
7. #include "MeDCMotor.h"
   
8. 
   1 k  V+ p2 T( ]3 R3 t3 h
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    ( _; t3 j% h& n& O9 b
12. float MinPower = 120;
    4 u/ z3 q, u3 K, E6 F
13. float Error,ErrorAcc,ErrorDec;
    0 N4 ]5 f" v9 N" ~1 I! N
14. 
    7 S% C: a3 D% p. B+ e0 X! {7 K5 K
15. float Kp=0.14;
    ) ^6 @/ s$ B  W7 l- m2 r( H' y
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    * W+ \: f. g/ D) p& k
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    5 A1 k4 x2 _( O% r/ u  }4 M5 O
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    ! s# G* M* J) j, x% e: m
30.     Error=0;
    
31.     ErrorAcc=0;
    2 j8 D* i  A; c+ B# @. M
32. }
    1 H- u! ^* d* i
33. 
    . S; a+ g6 O' j# C+ M% \
34. void loop(){
    
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    , V7 _$ p% k  C
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & I: [. ]* D% m  E0 `
38.     ErrorAcc +=  nError*Kd ;
    ' n4 n- w/ W- c/ X5 y
39.     ErrorDec -=  nError*Ki ;
    " J) ^; E$ |1 G& G2 [
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    5 s, v5 m  F8 e: p4 c* P* ^* }
42.       if(nPower < MaxPower){
    5 E5 ^! `. a8 {( J& h0 Z
43.         nPower += 3;
    
44.       }
    ( I) n1 r- _% c  a% i2 e
45.     } else{
    
46.       if(nPower > MinPower){
    $ o8 Q7 f' P$ \# q
47.         nPower -= 2;
    % j+ b& H2 q6 X7 D) ?4 k. v
48.       }
    ' d- {9 v, R+ `  ]) v9 ^
49.     }
    
50.     MotoL(nPower-Error);
    5 V; z; n( R2 G6 M; a+ j$ K! a
51.     MotoR(nPower+Error);   
    ; V2 s  K3 e9 J3 K
52.   }else{
    
53.     motor_9.run(0);
    " G. G! F+ a# `- k$ c4 M4 b  M
54.     motor_10.run(0);
    2 _2 Z  F: ], f% [
55.   }
    7 q. u/ d. o3 y4 W
56.   do{}while(millis() - StartTime < interval);
    4 K9 h! M9 I+ f
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    : q4 m* @2 \( y0 Q1 q+ A& Q& D
61.      Power = MaxSpeed;
    
62.   }
    4 @# x# P% ^& k) K9 l& b
63.   if (Power < -MaxSpeed){
    ! S4 O- C2 g. E; Q( T5 V
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    # z6 {, O5 J) W  T" x& k0 J6 |
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    
71.      Power = MaxSpeed;
    
72.   }
    ! B/ S- U7 W. p& [
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    ) Z) N6 [5 g3 ~  o+ u; U1 H
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

) {0 ?! v: {/ q: K: K8 V' W

" F$ |) }% |6 h$ e
; k" t4 d& C2 t3 C, A您好，不知是否能向您請教。
) J! f; t" @9 N7 o/ q4 a目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
2 j2 q( f: Z+ @4 J, e不知道您是否願意教我可以如何處理
7 z; @" k: B3 N8 o- y
4 y$ W1 S: t7 l8 v
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   " G# S5 g+ J' U4 p8 l% C# i; O4 Y
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ) y$ H8 h& t* o& H$ n
4. int olderror = 0;
   * v' K! n8 S7 o, b; ?
5. double values;
   
6. 
   0 g/ n5 T/ g, z% f
7. 
   + o* i3 N7 V% K9 E, [, D: L% r; x" ^
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    8 w. ?& w. X% D
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    * w; O9 Y  P5 S4 `4 Q8 B
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    & M) a# \5 {8 s2 L% n
18.   if (valuesMR > CNY70Val)
    5 _, z& j5 y/ W0 `' n/ R
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    7 G8 k# Q4 N* N6 K8 t
21.     valuesMM = CNY70Val;
    ( T0 l5 F: |7 E
22.   if (valuesML > CNY70Val)
    1 C& D" A+ b0 J* F; d! M: ?& ]/ V! J0 J
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;
    6 m  x8 k* D2 d+ h6 V! q* y
28. }
    
29. 
    . W$ K3 ^7 J) Q, u4 b
30. void Car()
    7 i" y4 }# N: v1 x0 ^5 ?" [
31. {
    
32.   while (1) {
    ( N. K" X7 e, c% q: [
33.     CNY70();
    
34. 
    ! c" R& H5 g- H# ~
35.     int error = ((int)values);
    5 b9 V4 _5 g7 Z% W* R( T' t+ j+ g# ?: g
36.     interror += error;
    
37.     int lasterror = error - olderror;
    $ N3 E1 c2 O* n" a  L0 l" q
38.     olderror = error;
    / F2 s8 _' T! f( E
39.     int power = error / 5 + interror / 10000 + lasterror;
    ; t% w8 T4 e' s4 R' [) c# L
40. 
    
41.     if (power > MotoSpeed)
    - S7 i8 f/ F/ z
42.       power = MotoSpeed;
    9 b4 P' k. V1 ~0 W
43.     if (power < -MotoSpeed)
    * T" U- v0 H$ b6 c* y+ [6 h
44.       power = -MotoSpeed;
    9 h7 J" n9 N+ R5 t- w
45. 
    * U1 `3 H: |3 z8 |1 G
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    ! R! i( D3 V4 y. @& R
51. }

複製代碼

" V, A! H: D' O- h8 a+ ^

感謝分享

感謝分享