PID 循跡自走車範例

1. #include <Arduino.h>
   ) V( X2 N$ F8 I( `- O/ D. J8 E
2. #include <Wire.h>
   1 D$ ]2 P9 v/ e& B
3. #include <Servo.h>
   9 J  y- o; N1 U4 Q. N3 j2 D/ T3 h/ r
4. 
   
5. #include "MePort.h"
   2 N" C# n+ E# G; E- V, ]; Y
6. #include "MeUltrasonic.h"
   / M* a6 e4 g; i" ]+ ?7 @/ S- t
7. #include "MeDCMotor.h"
   
8. 
   
9. //double Input, Output ;
   
10. float MaxSpeed = 255;
    7 ~9 O7 u5 \+ w: ]
11. float MaxPower = 180;
    # H% H% ^1 ^# {9 e- V
12. float MinPower = 120;
    0 Z+ n1 ]8 p. [8 `! B& v; U
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    ' A1 }" b$ i) i+ z$ i3 ~
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    / z; q. m% K( P* x+ }: x
18. 
    3 q( y1 i3 r- U4 V. p/ Y
19. float nPower;
    0 C& Q: g# r9 ~1 U
20. MePort lightsensor_6(6);
    ; Y! u' X. I4 `/ Y6 s, J! d
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    $ f! @3 U- K/ l; S8 \
26. 
    & a, d, }$ k5 P. \" s- z
27. void setup(){
    ) b7 i; R9 _1 ~5 a: B
28.     lightsensor_6.dWrite1(1);
    
29.     nPower = 160;
    9 f! g' N/ P1 j! C* V+ u
30.     Error=0;
    
31.     ErrorAcc=0;
    ) H5 m8 x; w9 R* g" g" o
32. }
    . O% f$ r' i4 D# A7 R/ T$ k5 y: |7 Z
33. 
    ; [, K5 M% q1 h2 H9 R3 h, t. h
34. void loop(){
      W- A: \" a8 e: q/ y2 L
35.   unsigned long StartTime = millis();
    5 U! d0 b/ S2 F2 V% O: |
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    & V7 b- o" Z- f8 Z7 g! w" W
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    2 J: c2 F4 o7 ?# t5 }' A, r
39.     ErrorDec -=  nError*Ki ;
    
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    ; @+ t0 U: Q  L% U& d' Y* ^! O
43.         nPower += 3;
    
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    3 K- G. E0 i: m7 a
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);
    1 S* I2 {' L* t, G3 Z3 e
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    
58. 
    ( R' ]7 q5 p8 I
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    % E0 G  w! Q- m$ C1 ~& _4 T# i
61.      Power = MaxSpeed;
    
62.   }
      V6 W+ B. L  u4 z& v1 F4 t* p5 C4 P
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    
66.   motor_9.run(Power);
    + ]5 Z4 g- V! K' j( [6 G
67. }  
    
68. 
    . |  l5 C; D& o& ~& ^4 b
69. void MotoR(int Power){
    * z5 @& g! `9 c; @
70.   if (Power > MaxSpeed){
    + a. ]+ t3 H, v& V* b
71.      Power = MaxSpeed;
    
72.   }
    ) L. |, V4 h- v6 i$ @
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    
75.   }
    
76.   motor_10.run(Power);
    : C$ i0 `2 R' H5 ?, {
77. }  

複製代碼

, {& i0 e" P5 m9 {* ?- c
) W% [- y( k, L6 R

" ^5 C3 K( ^) t' G$ m1 K
您好，不知是否能向您請教。
/ W3 t1 i* g! [9 `: W; V( b目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
( r8 N' L. {9 N
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
6 V6 ]' O; D) N# m+ ?9 {不知道您是否願意教我可以如何處理

0 D" W/ n: a! d$ F
1 |  k5 a! h# r: }( I$ ~: k" E以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   ; u8 J+ g7 e0 D3 e( e. {" _
2. double CNY70Val = 1000;
   0 ?7 l: [( Z& _. E2 I) w7 O# K3 Z8 i8 V
3. int interror = 0;
   6 A# p$ B! l. l! Z  b) a! A
4. int olderror = 0;
   
5. double values;
   
6. 
   
7. 
   
8. void CNY70()
   
9. {
   2 \6 H2 W$ ^! |7 T( w  Q1 ]
10.   valuesRR = analogRead(RR)
    ' E5 M5 Y! W: `: O
11.   valuesMR = analogRead(MR);
    0 l6 ^& M5 v6 Q7 m# L
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    9 J" C  |& D3 m" E! P2 i$ w- X7 E; s
15. 
    7 U; c- x3 E0 n# r) z
16.   if (valuesRR > CNY70Val)
    $ O9 ^; T& j( F% ?
17.     valuesRR = CNY70Val;
    8 j; H$ S+ y; t4 ~
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    ) r$ |( ~7 u( W9 {) L( o! y  H# m
20.   if (valuesMM > CNY70Val)
    
21.     valuesMM = CNY70Val;
    ( j9 R7 M/ A8 _, c7 O
22.   if (valuesML > CNY70Val)
    # J' r1 w% `, U- s* _
23.     valuesML = CNY70Val;
    1 y- }5 e& z- q
24.   if (valuesLL > CNY70Val)
    7 B, L, v2 Q$ ^. L( }+ H; y, M3 h
25.     valuesLL = CNY70Val;
    : A$ t  j6 b; H7 h8 c& H4 @& K
26. 
    
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    + \3 ^6 R2 i% A. _8 s" K( O
28. }
    : \* T" q6 x1 C
29. 
    $ U; t, _/ u6 ^' a
30. void Car()
    : o  g2 Z4 j1 a" D8 C, v( J4 X" ?
31. {
    ' H4 g- n" F1 E, b, T# a4 r
32.   while (1) {
    
33.     CNY70();
    
34. 
    ' m3 W0 K2 o  i, \# M1 @
35.     int error = ((int)values);
    1 R! p( M/ g* `7 K6 l
36.     interror += error;
    7 Z+ r! \- `1 p7 D. t4 x2 |
37.     int lasterror = error - olderror;
    ; s1 f0 C  A; ?, f' I
38.     olderror = error;
    
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    : T' Y! y5 k+ h3 }
42.       power = MotoSpeed;
    ' {5 E- W8 R1 q) I! U% X2 I' q6 a
43.     if (power < -MotoSpeed)
    
44.       power = -MotoSpeed;
    
45. 
    ' b+ d7 I0 c2 ^2 U$ x; D4 w, ]
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    4 L  U% m( x9 P: l
50.   }
    7 w* ]9 k  c4 A
51. }

複製代碼

感謝分享

感謝分享