PID 循跡自走車範例

1. #include <Arduino.h>
   , W  v' \8 [8 Q
2. #include <Wire.h>
   
3. #include <Servo.h>
   / o2 l, b# t0 S: f: K& |8 m) |3 W
4. 
   
5. #include "MePort.h"
   
6. #include "MeUltrasonic.h"
   " L4 C7 C' l1 i  `
7. #include "MeDCMotor.h"
   ) G3 h& E( I& x. |) k9 r& V) _* ]. h/ V
8. 
   
9. //double Input, Output ;
   0 D$ ^2 R: r( T% N
10. float MaxSpeed = 255;
    
11. float MaxPower = 180;
    3 C0 b) V. X" q9 g' |3 P: W
12. float MinPower = 120;
    ( C6 j# W% l' g
13. float Error,ErrorAcc,ErrorDec;
    9 \, Z- N8 G2 F( O6 }7 k* f6 c
14. 
    # q# N- G* T7 W5 i
15. float Kp=0.14;
    
16. float Kd=0.00020;//23;
    
17. float Ki=0.000201;
    
18. 
    4 H7 q4 l1 B6 F. g8 L+ G7 f
19. float nPower;
    
20. MePort lightsensor_6(6);
    3 a0 T8 a8 T. D4 T
21. MePort lightsensor_8(8);
    3 |, W! @- Z% K5 q# I7 f
22. MeDCMotor motor_9(9);
      |3 H3 _: Z# |
23. MeDCMotor motor_10(10);
    
24. unsigned long previousMillis = 0;
    
25. const long interval = 1;
    
26. 
    
27. void setup(){
    
28.     lightsensor_6.dWrite1(1);
    6 ]* b6 S1 ]1 u. }; J
29.     nPower = 160;
    
30.     Error=0;
    
31.     ErrorAcc=0;
    
32. }
    / Q. a: u+ m) C4 C
33. 
    $ f) x0 u) h% n" v3 t
34. void loop(){
    ; G7 R, O5 Z! z) Y
35.   unsigned long StartTime = millis();
    7 A- c9 o6 W$ d; P6 g
36.   if(ErrorAcc < 18000 && ErrorAcc > -18000){
    
37.     float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
    & i* ~& h0 ]! K. k6 Y0 w) {
38.     ErrorAcc +=  nError*Kd ;
    6 U+ G' B6 T# ^( p0 n
39.     ErrorDec -=  nError*Ki ;
    ; L! F! k7 s( f, f3 q7 }4 n% n9 m
40.     Error = int(nError*Kp+ErrorAcc+ErrorDec);
    
41.     if(nError < 80 && nError > -80){
    9 G- t0 O( {  S! j6 |& \' _
42.       if(nPower < MaxPower){
    ! A1 `4 p5 }' |, N
43.         nPower += 3;
    
44.       }
    - ]6 d+ Q* B7 B& Z& F0 W* E/ r
45.     } else{
    7 d) Z( J8 Z& A
46.       if(nPower > MinPower){
    $ Q( i# i% ?2 _4 J
47.         nPower -= 2;
    
48.       }
    
49.     }
    % ~4 e2 B" k$ _1 c' m
50.     MotoL(nPower-Error);
    
51.     MotoR(nPower+Error);   
    & f5 @% R1 }7 q: F% m5 B
52.   }else{
    % A! L7 {0 t' e$ X8 A6 J
53.     motor_9.run(0);
    1 z& Z3 Q, w) R6 M
54.     motor_10.run(0);
    
55.   }
    
56.   do{}while(millis() - StartTime < interval);
    
57. }
    2 Z0 @+ C9 S( ^- T& Z5 Q8 K
58. 
    % N6 f; `$ V3 k$ A2 u
59. void MotoL(int Power){
    
60.   if (Power > MaxSpeed){
    
61.      Power = MaxSpeed;
    
62.   }
    
63.   if (Power < -MaxSpeed){
    
64.      Power = -MaxSpeed;
    
65.   }
    ) e3 u* D7 E3 p/ R, U. T) y
66.   motor_9.run(Power);
    
67. }  
    
68. 
    
69. void MotoR(int Power){
    
70.   if (Power > MaxSpeed){
    5 ^& ?* K6 }" v& T: x
71.      Power = MaxSpeed;
    0 q' ?& |/ \( w5 p; S
72.   }
    : [! ?4 C; [( [' I3 U& B1 M
73.   if (Power < -MaxSpeed){
    1 Z6 @# H. m4 v* [! l+ F
74.      Power = -MaxSpeed;
    # N0 V7 l" i8 `" r/ v) ~/ {: T+ j
75.   }
    : g" ]! R) @+ W1 Q0 {2 G2 W# U1 }
76.   motor_10.run(Power);
    
77. }  

複製代碼

+ {% G  j1 f8 a0 j  L
% q" [' c! `0 @3 f" M3 s. y0 D- ~. I

, x$ \' |7 f# a3 L5 b5 F1 ~' T
$ @0 D( T2 A8 N1 A* Y4 ^您好，不知是否能向您請教。
目前和宋修賢老師在處理Ardui Car
4 D& @0 X' {" W' z! g5 o9 M雖然已使用較繁雜的方式處理了跑出黑線外的狀況

5 \' s( j  y: }0 G6 `但基於想追求更精簡的程式所以還是想請問一下
就是如果我只以單純寫PID控制時，常遇到CNY70跑出黑線外後便往前衝
不知道您是否願意教我可以如何處理


* W/ H+ B2 M7 \; _以下是我挑出我一般寫純PID控制的Code：

1. int MotoSpeed = 250;
   5 H3 P8 N: T" n; ~
2. double CNY70Val = 1000;
   " [% Z( O" h/ z, ?) a; I
3. int interror = 0;
   # s! Q& ?; y3 j5 Q3 y0 h
4. int olderror = 0;
   
5. double values;
   6 k/ q" Q9 O5 ]
6. 
   
7. 
   
8. void CNY70()
   0 S, f8 O0 t- E# y/ ^
9. {
   
10.   valuesRR = analogRead(RR)
    
11.   valuesMR = analogRead(MR);
    
12.   valuesMM = analogRead(MM);
    
13.   valuesML = analogRead(ML);
    
14.   valuesLL = analogRead(LL);
    
15. 
    
16.   if (valuesRR > CNY70Val)
    
17.     valuesRR = CNY70Val;
    7 Q: s1 z; \# Z; |% U
18.   if (valuesMR > CNY70Val)
    
19.     valuesMR = CNY70Val;
    8 K2 V# u6 U: ]
20.   if (valuesMM > CNY70Val)
    . c; `: U0 Q, m/ ]% L5 l
21.     valuesMM = CNY70Val;
    " S3 s% L+ G7 b7 I3 G4 T) m1 s
22.   if (valuesML > CNY70Val)
    
23.     valuesML = CNY70Val;
    
24.   if (valuesLL > CNY70Val)
    4 ]5 u3 A/ d2 C
25.     valuesLL = CNY70Val;
    : v6 U2 n, s" O6 h
26. 
    9 W, Y, Z2 c4 c& h) d4 R
27.   values = valuesLL * -1 + valuesML * -0.5 + valuesMM * 0 + valuesMR * 0.5 valuesRR * 1;
    7 {/ E% Y: Z( ~+ a* {9 A5 D5 |  P
28. }
    & R* C! z- g. Q- H9 `, B, f
29. 
    % ^' Z; |4 M# W" h. i1 _
30. void Car()
    2 y) L" F7 p' {& k
31. {
    
32.   while (1) {
    
33.     CNY70();
    . Q. M5 ^! u2 }" j5 E6 ~
34. 
    
35.     int error = ((int)values);
    
36.     interror += error;
    
37.     int lasterror = error - olderror;
    
38.     olderror = error;
    % r. \. O+ h. a0 v% w
39.     int power = error / 5 + interror / 10000 + lasterror;
    0 y/ c( k+ h) p6 p; p
40. 
    4 O8 |: _9 N- K# H9 N9 [4 l4 g
41.     if (power > MotoSpeed)
    
42.       power = MotoSpeed;
    ! A! |$ U( O0 R" Z9 V7 V# q
43.     if (power < -MotoSpeed)
    1 Y: E' f! J3 Z5 ~  d
44.       power = -MotoSpeed;
    
45. 
    
46.     if (power > 0)
    
47.       Speed(MotoSpeed, MotoSpeed - power);   //馬達動作，正數正轉 負數反轉。
    % l# l3 N- C% W- {1 w% I
48.     else
      L1 }; C) [( [% R
49.       Speed(MotoSpeed + power, MotoSpeed);
    
50.   }
    - y; O9 T" E2 [. N  {
51. }

複製代碼

感謝分享

感謝分享