PID 循跡自走車範例

1. #include <Arduino.h>
   0 @; R; `7 G9 h
2. #include <Wire.h>
   
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   4 R  D6 [4 V4 x/ s( Y8 i8 S+ w
7. #include "MeDCMotor.h"
   . I; ?$ Y' L, [: J! a/ q
8. 
   
9. //double Input, Output ;
     Q2 t) E8 w1 K5 P4 j: K5 A
10. float MaxSpeed = 255;
      i$ ^; ^; }8 n' O$ u
11. float MaxPower = 180;
    5 A+ o! d. c7 R  z
12. float MinPower = 120;
    
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    % B9 |( n& g9 o8 L3 f! F5 _
15. float Kp=0.14;
    ' t+ B+ Q6 h8 X7 ~) E! P) z
16. float Kd=0.00020;//23;
      L8 u! O. D; I
17. float Ki=0.000201;
    
18. 
    
19. float nPower;
    * k7 e/ ]/ h+ R: Q! Q" F
20. MePort lightsensor_6(6);
    
21. MePort lightsensor_8(8);
    
22. MeDCMotor motor_9(9);
    
23. MeDCMotor motor_10(10);
    $ o) a* X( ^8 V- i7 V6 d
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    ; {6 I) K1 e* V2 F! i% w
28.     lightsensor_6.dWrite1(1);
    ' m- s/ v5 v$ h1 z. H
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    ' ~" Q2 v9 f2 \/ T0 w8 A7 ~) i
32. }
    
33. 
    
34. void loop(){
    $ s- V- J, |; ~8 G3 c! x5 Y
35.   unsigned long StartTime = millis();
    
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    
38.     ErrorAcc +=  nError*Kd ;
    5 y- @" `( O& z0 ~$ H  e! H# K
39.     ErrorDec -=  nError*Ki ;
    ; U' y! p* ~9 t% ]' s1 s5 n6 x( ]
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    
42.       if(nPower < MaxPower){
    + {! n( }8 I/ B* J
43.         nPower += 3;
    7 i, V4 P8 Y9 ]# m# \
44.       }
    
45.     } else{
    
46.       if(nPower > MinPower){
    * L3 [/ e8 j4 w) T
47.         nPower -= 2;
    & n4 Q3 @/ o* ^  {& V' w( P! t
48.       }
    7 L6 K! E) u1 N, A
49.     }
    3 P2 f# n2 P( y& t
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    
52.   }else{
    ( j; t2 ~, G5 J) j; \
53.     motor_9.run(0);
    
54.     motor_10.run(0);
    
55.   }
    9 o. M6 H8 J6 t5 x' ^6 f: [
56.   do{}while(millis() - StartTime < interval);
    7 e: x+ [; B, [4 i9 ?
57. }
    
58. 
    
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    % i! N/ Z5 Y# g3 Y" W
61.      Power = MaxSpeed;
    
62.   }
    4 d) R0 a4 O7 O
63.   if (Power < -MaxSpeed){
    , y3 e, F( k* D' j8 P# O! E
64.      Power = -MaxSpeed;
    3 n9 R$ @$ f" z' c/ W6 m
65.   }
    
66.   motor_9.run(Power);
    & y4 U% c) l! e4 ~
67. }  
    
68. 
    , F" s+ {' n' Y8 q, p# p
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    3 r7 \! |3 I5 E
71.      Power = MaxSpeed;
    
72.   }
    
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    & |' |1 T9 G( ^6 O
75.   }
    
76.   motor_10.run(Power);
    
77. }  

複製代碼

: ]# E# P" k) F! m, M

+ V8 A+ l9 T% i  |9 k您好，不知是否能向您請教。
0 p4 j6 l6 a! M  k/ i9 }- _$ K目前和宋修賢老師在處理Ardui Car
. Z% E  [5 n, z. L雖然已使用較繁雜的方式處理了跑出黑線外的狀況

但基於想追求更精簡的程式所以還是想請問一下
9 G- r& `: [" p7 s: ~就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
: \: N* _" I5 p! E) l
6 z* c7 @# W& w
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   
2. double CNY70Val = 1000;
   + M$ ?6 z1 n2 [4 H9 `3 _1 b7 m  A
3. int interror = 0;
   
4. int olderror = 0;
   9 f1 ?% O4 a8 _& `0 O( [4 N
5. double values;
   
6. 
   
7. 
   ; a9 |, @0 `, F' A8 k
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    + U% S. C5 T- B* E5 N* j
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    & n. ?5 m* Y% p
14.   valuesLL = analogRead(LL);
    & @4 q2 [7 B* E% }6 {" X
15. 
    & ?2 O- b* X3 w5 J& x7 _
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    : d) A+ J% T. B9 h) ~; s/ ?: ^
18.   if (valuesMR > CNY70Val)
    9 ~7 W& F( g1 H- T. a
19.     valuesMR = CNY70Val;
    9 j' v, ?* H: L: Z- V* A# g4 p, f
20.   if (valuesMM > CNY70Val)
    5 U3 z; W/ I( H1 N/ D4 j
21.     valuesMM = CNY70Val;
    0 O! {. J- |6 l
22.   if (valuesML > CNY70Val)
    4 y1 d, C4 D; w/ o
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    % [% ?* z5 U$ d. x# e6 O6 _
25.     valuesLL = CNY70Val;
    
26. 
    , {1 E  k( _! y5 o" i
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    
28. }
    * m" k7 C; W5 c# c! @  ^$ s
29. 
    
30. void Car()
    / J0 k; y, O1 z! |
31. {
    
32.   while (1) {
    2 Q! ^4 q) a6 j! s* g0 _! U
33.     CNY70();
      |) M- b4 j, J
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    4 @' t' F+ A. n: d
38.     olderror = error;
    3 \% O# X8 L" C5 o
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    7 r9 w& H; T7 ?2 B5 {. a
41.     if (power > MotoSpeed)
    ' D) e3 w8 z9 g& V0 x
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    ! Z4 b8 L5 Q$ w& ?
44.       power = -MotoSpeed;
    
45. 
    1 Z( A% V, ]( s! R; |/ w6 d8 l
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    3 V5 O) v0 L& D
48.     else
    / f4 C. Y/ L* E5 Q
49.       Speed(MotoSpeed + power, MotoSpeed);
    - f8 [- c3 t1 Y( ]! r2 T# c( V
50.   }
    
51. }

複製代碼

+ O, P% S3 e1 {8 [! h; {
) Y  k/ z4 m& B5 I- l7 w) B

感謝分享

感謝分享