PID 循跡自走車範例

1. #include <Arduino.h>
   4 S9 `% B6 l3 W2 k& w7 m+ U
2. #include <Wire.h>
   ' }. U2 C: c3 @+ C
3. #include <Servo.h>
   
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   
7. #include "MeDCMotor.h"
   6 G! f7 d7 Z6 U+ h
8. 
   
9. //double Input, Output ;
   , n4 s/ G; E6 k% s
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    
12. float MinPower = 120;
    2 N, r: r1 T: c: v+ S
13. float Error,ErrorAcc,ErrorDec;
    
14. 
    
15. float Kp=0.14;
    / z$ l! [) }' d" f0 v. B- V3 G0 n1 q
16. float Kd=0.00020;//23;
    ! h7 r. R2 n% [3 _( o
17. float Ki=0.000201;
    / ?' r8 L7 r+ z3 g  Y* U
18. 
    
19. float nPower;
    
20. MePort lightsensor_6(6);
    0 }' j. |- R2 X' C
21. MePort lightsensor_8(8);
    1 ~( }0 g) @, |6 k. I
22. MeDCMotor motor_9(9);
    6 i$ Y6 \/ p* y% |7 x
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    $ M+ X2 o9 E( c5 }  a0 e8 D
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    , w, W0 c5 Y: k# {  N
29.     nPower = 160;
    ; M3 G3 t+ [+ L6 r! `( t6 u0 I
30.     Error=0;
    2 S; [7 N/ G7 E) ~. F  Z
31.     ErrorAcc=0;
    
32. }
    * Z1 C, `0 w0 y5 ~7 W0 _& J4 n5 J
33. 
    ; m  P2 N; I1 t
34. void loop(){
    * k1 J& r2 u# f7 r
35.   unsigned long StartTime = millis();
      \3 x0 i# Y$ d1 m% z
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    8 b1 u5 A% v) K+ j
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){
    
42.       if(nPower < MaxPower){
    
43.         nPower += 3;
    
44.       }
    & s: }6 E: _  ]* H# p6 k2 l
45.     } else{
    
46.       if(nPower > MinPower){
    + ~1 L0 x+ x% U* a3 x
47.         nPower -= 2;
    
48.       }
    3 ?) A" e5 Q9 F) l5 o, u
49.     }
    
50.     MotoL(nPower-Error);
    - z) G1 q) Z6 X# p, R. F
51.     MotoR(nPower+Error);   
    $ n5 T  I; x* p) M2 I
52.   }else{
    * Y, a' l8 N0 _
53.     motor_9.run(0);
    . b0 W+ p; e% v6 r  u2 F3 Q
54.     motor_10.run(0);
    
55.   }
    ) r9 j9 L+ z/ @- Z
56.   do{}while(millis() - StartTime < interval);
    
57. }
    + s+ w2 {! \. f
58. 
    - \# i' k0 m3 F8 c3 F
59. void MotoL(int Power){
    4 e- c, I, L6 [
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    # K$ s& H9 ?/ A/ k, d" ~/ R
65.   }
    0 g% ^: R1 h# x2 w' a* [
66.   motor_9.run(Power);
    
67. }  
    
68. 
    8 O# g2 C$ O/ H$ N
69. void MotoR(int Power){
    $ }: o* \7 R9 U4 ]7 b7 G4 T* H
70.   if (Power > MaxSpeed){
    : e( o+ o4 U3 k+ N
71.      Power = MaxSpeed;
    
72.   }
    " t0 l5 ]; `3 s' K
73.   if (Power < -MaxSpeed){
    
74.      Power = -MaxSpeed;
    9 V( Z# S8 `3 |) U1 w/ L; F
75.   }
    ' y' l  Z) q3 c) k* f4 p' [
76.   motor_10.run(Power);
    8 _$ J& g! Y+ l: `
77. }  

複製代碼

  E: T, B* \, U4 J6 I
. p/ d2 Q$ @; `& D% i* b, k

* G7 }2 s( {' Q( R5 ]
3 w' Y2 ]/ x  y% I8 ], I9 ]1 Q您好，不知是否能向您請教。
& p. f, W- G0 [- ^目前和宋修賢老師在處理Ardui Car
雖然已使用較繁雜的方式處理了跑出黑線外的狀況
4 U! f8 R- R5 s% c. A
但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理
' r9 S) A# k! L9 C  g6 L9 }) G) O
7 Q; p/ j% w( {7 B. o
以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   1 x& {1 N7 \7 Y
2. double CNY70Val = 1000;
   5 n6 K# A" |( |& J# s, J
3. int interror = 0;
   2 N( L' l& C. S3 @: F
4. int olderror = 0;
   : b. ]' I: f. w- X6 L/ m
5. double values;
   : @. R: b* `" t
6. 
   - S8 O3 s3 Y+ Q& b# r
7. 
   2 I/ `* f2 L0 m0 @0 I$ R+ s* m& o
8. void CNY70()
   
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    + N& i: m' I$ ~! {7 u' E1 o7 p
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    , _( o+ x- X9 W2 B4 F
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    ( N5 z2 b8 X6 B6 C$ ?; q
17.     valuesRR = CNY70Val;
    
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    
20.   if (valuesMM > CNY70Val)
    9 [7 n: u# h0 y' ~: ^
21.     valuesMM = CNY70Val;
    2 ?3 w6 w4 ]! y) o% b
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    / j+ E6 L9 I. o* v* E* l
25.     valuesLL = CNY70Val;
    
26. 
    ; I( s+ z0 _, V. g$ j8 S5 ]7 O' p+ o
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    ) q4 v) p( f$ o7 u3 i8 h0 I: m
28. }
    
29. 
    
30. void Car()
    
31. {
    # R- _6 d: H1 G$ }
32.   while (1) {
    0 J) B% N3 ?3 d* b$ w
33.     CNY70();
    * U6 t7 W- y: c! e7 `
34. 
    
35.     int error = ((int)values);
    ! g3 U4 o8 q% x: y* T5 C
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    0 Y; m, b! Q4 `0 {+ x3 q
39.     int power = error / 5 + interror / 10000 + lasterror;
    
40. 
    
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    
43.     if (power < -MotoSpeed)
    3 D: b: R$ a/ X7 G2 o% V! E
44.       power = -MotoSpeed;
    
45. 
    % i' i3 b" |$ _9 n2 H3 {. Z! s2 ^
46.     if (power > 0)
    8 ^: c0 S0 M% i: ]0 m; U" |
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    ' E" ^+ u9 o$ p* f5 B5 O, S
48.     else
    
49.       Speed(MotoSpeed + power, MotoSpeed);
    ; L& T# w2 B  m: V0 N
50.   }
    7 x1 h4 d" x" K8 n& V1 V; \9 E! W
51. }

複製代碼

感謝分享

感謝分享