PID 循跡自走車範例

1. #include <Arduino.h>
   7 Y! Y) Y* i5 x; K+ P  V" R( T; X
2. #include <Wire.h>
   
3. #include <Servo.h>
   2 N, m5 k% }( ]7 j
4. 
   8 K2 Q- A+ S! h3 F5 l* j; F! Z
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   
8. 
   % u; c/ y) i) d  M/ ^
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    ! b: M2 W4 _7 k' W! \* _
11. float MaxPower = 180;
    0 O) m: W7 x/ E% ]& P0 }) Q
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    ' a6 g+ k  R' [
14. 
    
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    5 f9 a; c* ?/ V2 @+ y1 Q
17. float Ki=0.000201;
    
18. 
    + }/ R2 `6 x+ y- M  m
19. float nPower;
    
20. MePort lightsensor_6(6);
    : |* [1 K1 o# s8 y% a: v
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    % J, p5 Y3 M; C8 `; u
23. MeDCMotor motor_10(10);
    # o  m/ F9 H7 V$ n" v
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    4 \" g, C! }3 A' W/ H$ p- ~2 v: ?
26. 
    + @" g5 ^, b5 l' Y  j" y  G! j, u
27. void setup(){
    " |: r5 t8 h5 x! |% T
28.     lightsensor_6.dWrite1(1);
    2 q' Z  T8 }8 \" ^$ l3 Z- Z
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    7 L2 T: `. [$ v" _. Q, r
32. }
    
33. 
    
34. void loop(){
    ! r7 M! C! y2 l7 `! ^4 S
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 ;
    8 |6 s, X  ]) R7 O" K- }! _7 S5 J$ J
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    $ I, }" t7 y8 p3 E# m% G
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    
45.     } else{
    9 K1 F# g3 L$ P$ W
46.       if(nPower > MinPower){
    
47.         nPower -= 2;
    " I3 P4 e2 u% W. ?5 h" T
48.       }
    + W0 @) a* r4 o, n9 H" C, d
49.     }
    # |1 a$ \# }" G. c7 B
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    
59. void MotoL(int Power){
    , {9 b8 |( S, Z- R5 ]* z* Z0 \
60.   if (Power > MaxSpeed){
    0 S$ r0 T0 k9 h
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    * Z, E  q, X; @$ H/ I" H
65.   }
    
66.   motor_9.run(Power);
    & V& t5 w3 }8 M5 A. q( r' r. ^
67. }  
    5 @$ D& q. }$ }  ~6 w
68. 
    0 A/ {0 s0 m: v! L, E
69. void MotoR(int Power){
    , p2 ^3 U/ I2 l/ d4 ~
70.   if (Power > MaxSpeed){
    1 [  [6 }0 ^6 x5 \" ?4 C
71.      Power = MaxSpeed;
    
72.   }
    # D' A5 S8 v- D& F/ e. k  D; a9 L
73.   if (Power < -MaxSpeed){
    & y, k; m( t( c# J  u, {9 f* c
74.      Power = -MaxSpeed;
    9 n9 b  K8 e2 M8 G7 D! v9 n
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

5 I. J" W7 O$ {9 B4 {

. K& F0 c& D+ d7 C
您好，不知是否能向您請教。
7 }4 a% L1 j% L4 h目前和宋修賢老師在處理Ardui Car
/ f6 ]. N% ]* z$ Y( C2 W& E6 i雖然已使用較繁雜的方式處理了跑出黑線外的狀況
5 V" F- M. X; l8 v  [1 u
但基於想追求更精簡的程式所以還是想請問一下
0 l! Z  F: Z+ K/ }. g9 T  [$ R就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
3 f, {: a6 s: D8 x3 z不知道您是否願意教我可以如何處理
) ^5 m4 O. x% l" M- [* D8 Z

  L! ?5 [/ {& t( K以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   
3. int interror = 0;
   2 u  b+ m$ }" V* `
4. int olderror = 0;
   
5. double values;
   
6. 
   ! q5 E/ N2 s) l5 ~; m5 A
7. 
   4 z6 g% @. V! [6 |
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    + Y: y/ e4 f8 o) U! q9 c
11.   valuesMR = analogRead(MR);
    2 K  s# }. h/ D* m0 v
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    " L3 S1 `. ^7 I2 R% d$ m
15. 
    * M& B) `  S( b0 u  z3 l' |
16.   if (valuesRR > CNY70Val)
    5 w; a8 R& }* C+ p# c, \* Q: O
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    5 J, D# t! z' u  C9 ~2 `8 n( ~
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    ( n4 X. N- v" F* n0 x
24.   if (valuesLL > CNY70Val)
    3 G9 y: S  X- j; V# b2 [+ b2 u! H
25.     valuesLL = CNY70Val;
    
26. 
    * C8 z/ U" ?* S/ m& D
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    , O7 l/ ~6 S) H  o
28. }
    6 p7 N! l! h' o7 J* K9 t. W2 a
29. 
    1 l" J  l1 c3 Z1 q$ \9 u
30. void Car()
    2 S; \' b7 e4 M' f( o8 F( K
31. {
    ; A, y- g& l; d) X" J( m
32.   while (1) {
    
33.     CNY70();
    
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
      ^& T1 S7 ~& }. P' f; v2 Q
37.     int lasterror = error - olderror;
    # K% M, G4 ]8 [" V0 u9 P4 S
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    + s8 {( a& N/ i* ~* Y& s$ g
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    2 \9 @3 O: [& q
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    ! J  Y3 r! v, v* v$ G
45. 
    9 d/ q; W8 _, ?1 h/ F( P
46.     if (power > 0)
    ( J/ d: G; `( n) Y+ }
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ( P4 z3 L+ k$ ?! w1 z
50.   }
    0 `  L3 l& E+ z* Y% j6 W
51. }

複製代碼

感謝分享

感謝分享