PID 循跡自走車範例

1. #include <Arduino.h>
   
2. #include <Wire.h>
   # U, a7 e4 ?: s+ K9 ^
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   8 I2 H- B- Y% G; ?1 a
6. #include "MeUltrasonic.h"
   & L* K! Y2 o! i7 N% l
7. #include "MeDCMotor.h"
   
8. 
   . p& c# g5 z2 P3 [2 N
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    8 I( j& I: A* I" ?2 R
11. float MaxPower = 180;
    
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    % j# F0 l9 r  z8 t- g1 Y, z
15. float Kp=0.14;
    2 w3 V- b/ u4 H3 V3 y
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    3 K" T9 B: m- s  J8 [$ b
19. float nPower;
    1 {6 D( L3 ~2 Z% d) L% e
20. MePort lightsensor_6(6);
    0 U/ C- d' X& ^3 T
21. MePort lightsensor_8(8);
    7 [" Z% Z( T+ P/ z1 E  V- C
22. MeDCMotor motor_9(9);
    5 [; O2 N# e2 K4 t* F4 M, g& Z+ o& X
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    9 E( T* X- }6 g5 I
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    & q( {) V3 s4 Q! s8 _: F
29.     nPower = 160;
    
30.     Error=0;
    9 c, \9 P3 z, ~7 K) N- ]' V$ j
31.     ErrorAcc=0;
    
32. }
    & C5 G* q, ]5 j! D2 M4 d/ v9 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 ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    ; c& P" _6 T5 u7 h
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    ( c8 E% z. F& y
44.       }
    
45.     } else{
    % z& w) ]( l8 `2 Y  V+ a
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    
48.       }
    . f% c' b5 B! \# G* _% C
49.     }
    
50.     MotoL(nPower-Error);
    : c7 @5 H+ M8 Q. f1 X
51.     MotoR(nPower+Error);   
    ; \1 N* ?) q% }0 J1 i, \9 K
52.   }else{
    . V! N9 A1 X1 Y0 V; J3 Y* S, m5 J4 `/ K
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    7 x& B3 ], R  G2 \) _# H8 m5 G7 B: Z
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    - O+ e( Y9 ]$ Z4 d1 [0 I
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    . _  J/ S. j- x7 L" J
64.      Power = -MaxSpeed;
    - ?2 b) a$ o; C8 A1 F
65.   }
    
66.   motor_9.run(Power);
    
67. }  
    
68. 
    7 f; q2 `  ]" b$ h8 S. v4 |# \
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
      v' G( T0 y* E/ u& ?
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    0 w0 L8 V: W2 [
74.      Power = -MaxSpeed;
    
75.   }
    . D' f" a$ E- ^; f4 R
76.   motor_10.run(Power);
    
77. }  

複製代碼

) B4 E& V, u; C" g6 u* u
2 |/ V) R. R) F

/ x8 h2 f3 X2 f- R$ |$ k9 p3 K
) ^0 W  e7 m& C6 b您好，不知是否能向您請教。
; u0 W. {% x, A6 w目前和宋修賢老師在處理Ardui Car
6 g& C0 \$ l5 U" h: q* b: @雖然已使用較繁雜的方式處理了跑出黑線外的狀況
" L" Q7 k- K6 s+ E* U
但基於想追求更精簡的程式所以還是想請問一下
( y  x' @6 L7 b就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
% d. p) D8 x) J1 \5 t) h; W
$ Q- |9 D) U1 u0 O4 ^' C/ L+ w
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ! ^! B- d/ {% `+ z
2. double CNY70Val = 1000;
   
3. int interror = 0;
   ' i3 k& G  N1 G4 c
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   0 N/ s2 v9 ~, m4 @
9. {
   3 _0 b& [3 j  G/ g% X
10.   valuesRR = analogRead(RR)
    % |' J  V' u1 N" U& O
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    + H$ q$ a& d& t; p$ z) C( v
13.   valuesML = analogRead(ML);
    ' y; ^+ ]* ]+ `. R! y! w) D6 J
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    * q, E/ {2 x/ ~0 h6 |
17.     valuesRR = CNY70Val;
      n, k4 @7 a- o% b
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    
22.   if (valuesML > CNY70Val)
    6 v8 W/ W# c8 v2 R# a0 ~3 Q/ x
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    
25.     valuesLL = CNY70Val;
    
26. 
    5 T7 y3 {+ d  o( }8 |5 R. h
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    5 {4 R0 a) g: A; G; A
29. 
    8 S0 a: S& u* g" C
30. void Car()
    9 D. |1 D0 j/ Q- n1 R$ s' _
31. {
    
32.   while (1) {
    ) E6 k) Q# Z4 z, k3 l3 N4 u5 W
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    . [' O7 B. O! ]( E( o; s1 [% f$ A5 D
36.     interror += error;
    5 o4 f$ a, n- s* n$ B
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    " E9 J" K6 t6 c4 W# _
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    . [0 l' @+ i2 t
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    ! O+ C$ s: N, Q% e1 l
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    
51. }

複製代碼

感謝分享

感謝分享